feat: make Moveit2 demo talk to arm_pkg

src/arm_pkg/arm_pkg/arm_node.py

@@ -67,15 +67,17 @@ class SerialRelay(Node):
         self.joint_state_pub = self.create_publisher(JointState, 'joint_states', 10)
         self.tf_broadcaster = TransformBroadcaster(self)
         self.joint_state = JointState()
-        self.joint_state.name = ['Axis_0', 'Axis_1_Joint', 'Axis_2_Joint', 'Axis_3_Joint', 'Continuous_Joint', 'Wrist_Joint']
+        self.joint_state.name = ['Axis_0_Joint', 'Axis_1_Joint', 'Axis_2_Joint', 'Axis_3_Joint', 'Wrist_Differential_Joint', 'Wrist-EF_Roll_Joint', "Gripper_Slider_Left"]
-        self.joint_state.position = [0.0] * 6  # Initialize with zeros
+        self.joint_state.position = [0.0] * len(self.joint_state.name)  # Initialize with zeros
         self.odom_trans = TransformStamped()
         self.odom_trans.header.frame_id = 'odom'
-        self.odom_trans.child_frame_id = 'base_footprint'
+        self.odom_trans.child_frame_id = 'base_link'
         self.odom_trans.transform.translation.x = 0.0
         self.odom_trans.transform.translation.y = 0.0
         self.odom_trans.transform.translation.z = 0.0
 
+        self.joint_command_sub = self.create_subscription(JointState, '/joint_commands', self.joint_command_callback, 10)
+
         # Topics used in anchor mode
         if self.launch_mode == 'anchor':
             self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
@@ -238,6 +240,23 @@ class SerialRelay(Node):
             #self.debug_pub.publish(tempMsg)
 
 
+    def joint_command_callback(self, msg: JointState):
+        # Embedded takes deg*10, ROS2 uses Radians
+        positions = [math.degrees(pos)*10 for pos in msg.position]
+        # Axis 2 & 3 URDF direction is inverted
+        positions[2] = -positions[2]
+        positions[3] = -positions[3]
+
+        # Set target angles for each arm axis for embedded IK PID to handle
+        command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
+        # Wrist yaw
+        command += f"can_relay_tovic,digit,36,0,{positions[4]}\n"
+        # Wrist roll
+        command += f"can_relay_tovic,digit,35,{positions[5]}\n"
+        # Gripper IK does not have adequate hardware yet
+        self.send_cmd(command)
+
+
     def send_manual(self, msg: ArmManual):
         axis0 = msg.axis0
         axis1 = -1 * msg.axis1
@@ -334,8 +353,8 @@ class SerialRelay(Node):
             # Joint state publisher for URDF visualization
             self.joint_state.position[0] = math.radians(angles[0])  # Axis 0
             self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
-            self.joint_state.position[2] = math.radians(angles[2])  # Axis 2
+            self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
-            self.joint_state.position[3] = math.radians(angles[3])  # Axis 3
+            self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
             self.joint_state.position[4] = math.radians(angles[4])  # Wrist roll
             self.joint_state.position[5] = math.radians(angles[5])  # Wrist yaw
             self.joint_state.header.stamp = self.get_clock().now().to_msg()

src/astra_arm_moveit_config/config/ASTRA_Arm.ros2_control.xacro

@@ -6,7 +6,12 @@
         <ros2_control name="${name}" type="system">
             <hardware>
                 <!-- By default, set up controllers for simulation. This won't work on real hardware -->
-                <plugin>mock_components/GenericSystem</plugin>
+                <!-- <plugin>mock_components/GenericSystem</plugin> -->
+                <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
+                <param name="joint_commands_topic">/joint_commands</param>
+                <param name="joint_states_topic">/joint_states</param>
+                <param name="trigger_joint_command_threshold">1e-5</param>  <!-- Set to -1 to disable -->
+                <param name="sum_wrapped_joint_states">false</param>
             </hardware>
             <joint name="Axis_0_Joint">
                 <command_interface name="position"/>

src/astra_arm_moveit_config/launch/demo.launch.py

@@ -1,7 +1,117 @@
 from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import generate_demo_launch
+# from moveit_configs_utils.launches import generate_demo_launch
 
+import os
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+from srdfdom.srdf import SRDF
+
+from moveit_configs_utils.launch_utils import (
+    add_debuggable_node,
+    DeclareBooleanLaunchArg,
+)
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("ASTRA_Arm", package_name="astra_arm_moveit_config").to_moveit_configs()
-    return generate_demo_launch(moveit_config)
+    # return generate_demo_launch(moveit_config)
+    launch_package_path = moveit_config.package_path
+
+    ld = LaunchDescription()
+    ld.add_action(
+        DeclareBooleanLaunchArg(
+            "db",
+            default_value=False,
+            description="By default, we do not start a database (it can be large)",
+        )
+    )
+    ld.add_action(
+        DeclareBooleanLaunchArg(
+            "debug",
+            default_value=False,
+            description="By default, we are not in debug mode",
+        )
+    )
+    ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True))
+    # If there are virtual joints, broadcast static tf by including virtual_joints launch
+    virtual_joints_launch = (
+        launch_package_path / "launch/static_virtual_joint_tfs.launch.py"
+    )
+
+    if virtual_joints_launch.exists():
+        ld.add_action(
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(str(virtual_joints_launch)),
+            )
+        )
+
+    # Given the published joint states, publish tf for the robot links
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(launch_package_path / "launch/rsp.launch.py")
+            ),
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(launch_package_path / "launch/move_group.launch.py")
+            ),
+        )
+    )
+
+    # Run Rviz and load the default config to see the state of the move_group node
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(launch_package_path / "launch/moveit_rviz.launch.py")
+            ),
+            condition=IfCondition(LaunchConfiguration("use_rviz")),
+        )
+    )
+
+    # If database loading was enabled, start mongodb as well
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(launch_package_path / "launch/warehouse_db.launch.py")
+            ),
+            condition=IfCondition(LaunchConfiguration("db")),
+        )
+    )
+
+    # Fake joint driver
+    ld.add_action(
+        Node(
+            package="controller_manager",
+            executable="ros2_control_node",
+            parameters=[
+                str(moveit_config.package_path / "config/ros2_controllers.yaml"),
+            ],
+            remappings=[
+                ("/controller_manager/robot_description", "/robot_description"),
+            ],
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                str(launch_package_path / "launch/spawn_controllers.launch.py")
+            ),
+        )
+    )
+
+    return ld

src/astra_arm_moveit_config/launch/spawn_controllers.launch.py

@@ -1,7 +1,42 @@
 from moveit_configs_utils import MoveItConfigsBuilder
 from moveit_configs_utils.launches import generate_spawn_controllers_launch
 
+import os
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+from srdfdom.srdf import SRDF
+
+from moveit_configs_utils.launch_utils import (
+    add_debuggable_node,
+    DeclareBooleanLaunchArg,
+)
+
 
 def generate_launch_description():
     moveit_config = MoveItConfigsBuilder("ASTRA_Arm", package_name="astra_arm_moveit_config").to_moveit_configs()
-    return generate_spawn_controllers_launch(moveit_config)
+    # return generate_spawn_controllers_launch(moveit_config)
+    controller_names = moveit_config.trajectory_execution.get(
+        "moveit_simple_controller_manager", {}
+    ).get("controller_names", [])
+    ld = LaunchDescription()
+    for controller in controller_names:  # + ["joint_state_broadcaster"]:
+        ld.add_action(
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=[controller],
+                output="screen",
+            )
+        )
+    return ld