Added twist broadcasting to demos

.gitignore

@@ -0,0 +1 @@
+crisp_controllers

crisp_controllers_robot_demos/config/fr3/controllers.yaml

@@ -9,6 +9,9 @@
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
 
+      twist_broadcaster:
+        type: crisp_controllers/TwistBroadcaster
+
       pose_broadcaster:
         type: crisp_controllers/PoseBroadcaster
 
@@ -49,6 +52,20 @@
     ros__parameters:
       use_local_topics: False
 
+  twist_broadcaster:
+    ros__parameters:
+      joints:
+        - fr3_joint1
+        - fr3_joint2
+        - fr3_joint3
+        - fr3_joint4
+        - fr3_joint5
+        - fr3_joint6
+        - fr3_joint7
+
+      end_effector_frame: fr3_hand_tcp
+      base_frame: base
+
   pose_broadcaster:
     ros__parameters:
       joints:

crisp_controllers_robot_demos/config/fr3/left_controllers.yaml

@@ -9,6 +9,9 @@ left:
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
 
+      twist_broadcaster:
+        type: crisp_controllers/TwistBroadcaster
+
       pose_broadcaster:
         type: crisp_controllers/PoseBroadcaster
 
@@ -55,6 +58,20 @@ left:
     ros__parameters:
       use_local_topics: False
 
+  twist_broadcaster:
+    ros__parameters:
+      joints:
+        - left_fr3_joint1
+        - left_fr3_joint2
+        - left_fr3_joint3
+        - left_fr3_joint4
+        - left_fr3_joint5
+        - left_fr3_joint6
+        - left_fr3_joint7
+
+      end_effector_frame: left_fr3_hand_tcp
+      base_frame: left_base
+
   pose_broadcaster:
     ros__parameters:
       joints:

crisp_controllers_robot_demos/config/fr3/right_controllers.yaml

@@ -9,6 +9,9 @@ right:
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
 
+      twist_broadcaster:
+        type: crisp_controllers/TwistBroadcaster
+
       pose_broadcaster:
         type: crisp_controllers/PoseBroadcaster
 
@@ -55,6 +58,20 @@ right:
     ros__parameters:
       use_local_topics: False
 
+  twist_broadcaster:
+    ros__parameters:
+      joints:
+        - right_fr3_joint1
+        - right_fr3_joint2
+        - right_fr3_joint3
+        - right_fr3_joint4
+        - right_fr3_joint5
+        - right_fr3_joint6
+        - right_fr3_joint7
+
+      end_effector_frame: right_fr3_hand_tcp
+      base_frame: right_base
+
   pose_broadcaster:
     ros__parameters:
       joints:

crisp_controllers_robot_demos/config/iiwa/controllers.yaml

@@ -9,6 +9,9 @@
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
 
+      twist_broadcaster:
+        type: crisp_controllers/TwistBroadcaster
+
       pose_broadcaster:
         type: crisp_controllers/PoseBroadcaster
 
@@ -43,6 +46,20 @@
     ros__parameters:
       use_local_topics: False
 
+  twist_broadcaster:
+    ros__parameters:
+      joints:
+        - joint_a1
+        - joint_a2
+        - joint_a3
+        - joint_a4
+        - joint_a5
+        - joint_a6
+        - joint_a7
+
+      end_effector_frame: tool0
+      base_frame: iiwa_base
+
   pose_broadcaster:
     ros__parameters:
       joints:

crisp_controllers_robot_demos/config/kinova_gen3/controllers.yaml

@@ -9,6 +9,9 @@
       joint_state_broadcaster:
         type: joint_state_broadcaster/JointStateBroadcaster
 
+      twist_broadcaster:
+        type: crisp_controllers/TwistBroadcaster
+
       gravity_compensation:
         type: crisp_controllers/CartesianController
 
@@ -50,6 +53,20 @@
     ros__parameters:
       use_local_topics: False
 
+  twist_broadcaster:
+    ros__parameters:
+      joints:
+        - joint_1
+        - joint_2
+        - joint_3
+        - joint_4
+        - joint_5
+        - joint_6
+        - joint_7
+
+      end_effector_frame: end_effector_link
+      base_frame: base_link
+
   pose_broadcaster:
     ros__parameters:
       joints:

crisp_controllers_robot_demos/launch/franka.launch.py

@@ -229,6 +229,12 @@ def generate_launch_description():
                 arguments=["joint_trajectory_controller"],
                 output="screen",
             ),
+            Node(
+                package="controller_manager",
+                executable="spawner",
+                arguments=["twist_broadcaster"],
+                output="screen",
+            ),
             Node(
                 package="controller_manager",
                 executable="spawner",

crisp_controllers_robot_demos/launch/iiwa.launch.py

@@ -180,70 +180,78 @@ def generate_launch_description():
     namespace = LaunchConfiguration("namespace")
 
     # Get URDF via xacro
-    robot_description_content = Command([
-        PathJoinSubstitution([FindExecutable(name="xacro")]),
-        " ",
-        PathJoinSubstitution([
-            FindPackageShare("crisp_controllers_robot_demos"),
-            "config",
-            "iiwa",
-            "iiwa.urdf.xacro",
-        ]),
-        " ",
-        "prefix:=",
-        prefix,
-        " ",
-        "use_fake_hardware:=",
-        use_fake_hardware,
-        " ",
-        "robot_ip:=",
-        robot_ip,
-        " ",
-        "robot_port:=",
-        robot_port,
-        " ",
-        "initial_positions_file:=",
-        initial_positions_file,
-        " ",
-        "command_interface:=",
-        command_interface,
-        " ",
-        "base_frame_file:=",
-        base_frame_file,
-        " ",
-        "description_package:=",
-        description_package,
-        " ",
-        "runtime_config_package:=",
-        runtime_config_package,
-        " ",
-        "controllers_file:=",
-        controllers_file,
-        " ",
-        "namespace:=",
-        namespace,
-    ])
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("crisp_controllers_robot_demos"),
+                    "config",
+                    "iiwa",
+                    "iiwa.urdf.xacro",
+                ]
+            ),
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+            "use_fake_hardware:=",
+            use_fake_hardware,
+            " ",
+            "robot_ip:=",
+            robot_ip,
+            " ",
+            "robot_port:=",
+            robot_port,
+            " ",
+            "initial_positions_file:=",
+            initial_positions_file,
+            " ",
+            "command_interface:=",
+            command_interface,
+            " ",
+            "base_frame_file:=",
+            base_frame_file,
+            " ",
+            "description_package:=",
+            description_package,
+            " ",
+            "runtime_config_package:=",
+            runtime_config_package,
+            " ",
+            "controllers_file:=",
+            controllers_file,
+            " ",
+            "namespace:=",
+            namespace,
+        ]
+    )
 
     robot_description = {"robot_description": robot_description_content}
 
     # Use our controllers
-    robot_controllers = PathJoinSubstitution([
-        FindPackageShare("crisp_controllers_robot_demos"),
-        "config",
-        "iiwa",
-        "controllers.yaml",
-    ])
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("crisp_controllers_robot_demos"),
+            "config",
+            "iiwa",
+            "controllers.yaml",
+        ]
+    )
     # robot_controllers = PathJoinSubstitution([
     #     FindPackageShare(runtime_config_package),
     #     "config",
     #     controllers_file,
     # ])
 
-    rviz_config_file = PathJoinSubstitution([
-        FindPackageShare(description_package),
-        "rviz",
-        "iiwa.rviz",
-    ])
+    rviz_config_file = PathJoinSubstitution(
+        [
+            FindPackageShare(description_package),
+            "rviz",
+            "iiwa.rviz",
+        ]
+    )
 
     control_node = Node(
         package="controller_manager",
@@ -282,7 +290,6 @@ def generate_launch_description():
         ],
     )
 
-
     robot_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
@@ -314,6 +321,15 @@ def generate_launch_description():
         ],
     )
 
+    twist_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "twist_broadcaster",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
 
     nodes = [
         control_node,
@@ -323,6 +339,7 @@ def generate_launch_description():
         joint_state_broadcaster_spawner,
         cartesian_impedance_controller_spawner,
         pose_broadcaster_spawner,
+        twist_broadcaster_spawner,
     ]
 
     return LaunchDescription(declared_arguments + nodes)

crisp_controllers_robot_demos/launch/kinova_gen3.launch.py

@@ -108,6 +108,12 @@ def generate_launch_description():
             arguments=["pose_broadcaster"],
             output="screen",
         ),
+        Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=["twist_broadcaster"],
+            output="screen",
+        ),
         Node(
             package="controller_manager",
             executable="spawner",

docker/Dockerfile.robots

@@ -1,5 +1,5 @@
 ARG ROS_DISTRO=humble
-ARG CRISP_CONTROLLERS_VERSION=1.0.0
+ARG CRISP_CONTROLLERS_VERSION=1.1.0
 
 FROM osrf/ros:${ROS_DISTRO}-desktop AS base
 
@@ -157,7 +157,7 @@ RUN source /opt/ros/$ROS_DISTRO/setup.bash \
 
 FROM iiwa AS iiwa-overlay
 
-ARG CRISP_CONTROLLERS_VERSION=1.0.0
+ARG CRISP_CONTROLLERS_VERSION=1.1.0
 
 COPY . src/crisp_controllers_demos