feat: add viz code from Tristan's ik_test and add CAD to URDF

2026-02-11 17:30:36 +00:00 · 2025-08-17 09:46:56 -05:00
parent 1281236b36
commit 2db9b67ebc
11 changed files with 417 additions and 32 deletions
--- a/arm_launch.py
+++ b/arm_launch.py
@@ -0,0 +1,91 @@
+#!/usr/bin/env python3
+
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
+from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch.conditions import IfCondition
+
+
+
+#Prevent making __pycache__ directories
+from sys import dont_write_bytecode
+dont_write_bytecode = True
+
+def launch_setup(context, *args, **kwargs):
+    # Retrieve the resolved value of the launch argument 'mode'
+    mode = LaunchConfiguration('mode').perform(context)
+    nodes = []
+
+    # Arm
+    # package_dir = get_package_share_directory('arm_pkg')
+    package_dir = "/home/david/repos/rover-ros2/src/arm_pkg"  # TODO: copy files to share and point there
+    urdf_file_name = 'arm12.urdf'
+    urdf = os.path.join(package_dir, "urdf", urdf_file_name)
+    # urdf = "/home/david/repos/rover-ros2/src/arm_pkg/urdf/" + urdf_file_name
+    with open(urdf, 'r') as infp:
+        robot_desc = infp.read()
+
+    rviz_config_path = os.path.join(package_dir, 'viz.rviz')
+
+    if mode == 'anchor':
+        nodes.append(
+            Node(
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
+                output='both',
+                parameters=[{'robot_description': robot_desc}],
+                arguments=[urdf],
+                on_exit=Shutdown()
+            )
+        )
+        nodes.append(
+            Node(
+                package='arm_pkg',
+                executable='arm',  # change as needed
+                name='arm',
+                output='both',
+                parameters=[{'launch_mode': 'anchor'}],
+                on_exit=Shutdown()
+            )
+        )
+        nodes.append(
+            Node(
+                package='rviz2',
+                executable='rviz2',
+                name='rviz2',
+                output='both',
+                arguments=['-d', rviz_config_path],
+                on_exit=Shutdown()
+            )
+        )
+        # nodes.append(
+        #     Node(
+        #         package='anchor_pkg',
+        #         executable='anchor',  # change as needed
+        #         name='anchor',
+        #         output='both',
+        #         parameters=[{'launch_mode': 'anchor'}],
+        #         on_exit=Shutdown()
+        #     )
+        # )
+    else:
+        # If an invalid mode is provided, print an error.
+        print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
+
+    return nodes
+
+def generate_launch_description():
+    declare_arg = DeclareLaunchArgument(
+        'mode',
+        default_value='anchor',
+        description='Launch mode: arm, core, bio, anchor, or ptz'
+    )
+
+    return LaunchDescription([
+        declare_arg,
+        OpaqueFunction(function=launch_setup)
+    ])