ROS 2 add UR, Kinova, Robotiq components (#44)

* Fixed prefix Signed-off-by: Jakub Delicat <[email protected]> * Added launches and gz mappings Signed-off-by: Jakub Delicat <[email protected]> * update launches Signed-off-by: Jakub Delicat <[email protected]> * fixed astra Signed-off-by: Jakub Delicat <[email protected]> * removed print Signed-off-by: Jakub Delicat <[email protected]> * Fixed unused file Signed-off-by: Jakub Delicat <[email protected]> * renamed files Signed-off-by: Jakub Delicat <[email protected]> * Added ur Signed-off-by: Jakub Delicat <[email protected]> * added kinova Signed-off-by: Jakub Delicat <[email protected]> * split 6dof and 7dof Signed-off-by: Jakub Delicat <[email protected]> * Added rootiq Signed-off-by: Jakub Delicat <[email protected]> * Added suggestion Signed-off-by: Jakub Delicat <[email protected]> * simplfied urfd | removed use_gpu | removed simulation-engine Signed-off-by: Jakub Delicat <[email protected]> * simplfied urfd | removed use_gpu | removed simulation-engine Signed-off-by: Jakub Delicat <[email protected]> * applied new changes to ur Signed-off-by: Jakub Delicat <[email protected]> * removed simulation-engine from s3 | fix with no namespace Signed-off-by: Jakub Delicat <[email protected]> * fixed namespaced ros2_control Signed-off-by: Jakub Delicat <[email protected]> * Suggestions * add * workflow simplify * Removed unnecessary topic form realsense Signed-off-by: Jakub Delicat <[email protected]> * Made new tests Signed-off-by: Jakub Delicat <[email protected]> * fixed astra Signed-off-by: Jakub Delicat <[email protected]> * applied tests Signed-off-by: Jakub Delicat <[email protected]> * Added comment about GRP03 Signed-off-by: Jakub Delicat <[email protected]> --------- Signed-off-by: Jakub Delicat <[email protected]> Co-authored-by: rafal-gorecki <[email protected]> Co-authored-by: rafal-gorecki <[email protected]>
husarion · May 23, 2024 · f50cee0 · f50cee0
1 parent 2f4ca32
commit f50cee0
Show file tree

Hide file tree

Showing 16 changed files with 1,335 additions and 20 deletions.
diff --git a/config/gz_kinova_remappings.yaml b/config/gz_kinova_remappings.yaml
@@ -0,0 +1,20 @@
+---
+  - topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/camera_info
+    ros_type_name: sensor_msgs/msg/CameraInfo
+    gz_type_name: ignition.msgs.CameraInfo
+    lazy: true
+
+  - topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/image
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
+
+  - topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/depth_image
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
+
+  - ros_topic_name: <robot_namespace><device_namespace>/wrist_mounted_camera/points
+    ros_type_name: sensor_msgs/msg/PointCloud2
+    gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true
diff --git a/config/kinova_6dof_controllers.yaml b/config/kinova_6dof_controllers.yaml
@@ -0,0 +1,38 @@
+/**:
+  joint_trajectory_controller:
+    ros__parameters:
+      joints:
+        - joint_1
+        - joint_2
+        - joint_3
+        - joint_4
+        - joint_5
+        - joint_6
+      command_interfaces:
+        - position
+      state_interfaces:
+        - position
+        - velocity
+      state_publish_rate: 100.0
+      action_monitor_rate: 20.0
+      allow_partial_joints_goal: false
+      constraints:
+        stopped_velocity_tolerance: 0.0
+        goal_time: 0.0
+
+  twist_controller:
+    ros__parameters:
+      joint: tcp
+      interface_names:
+        - twist.linear.x
+        - twist.linear.y
+        - twist.linear.z
+        - twist.angular.x
+        - twist.angular.y
+        - twist.angular.z
+
+  robotiq_gripper_controller:
+    ros__parameters:
+      default: true
+      joint: robotiq_85_left_knuckle_joint
+      allow_stalling: true
diff --git a/config/kinova_7dof_controllers.yaml b/config/kinova_7dof_controllers.yaml
@@ -0,0 +1,39 @@
+/**:
+  joint_trajectory_controller:
+    ros__parameters:
+      joints:
+        - joint_1
+        - joint_2
+        - joint_3
+        - joint_4
+        - joint_5
+        - joint_6
+        - joint_7
+      command_interfaces:
+        - position
+      state_interfaces:
+        - position
+        - velocity
+      state_publish_rate: 100.0
+      action_monitor_rate: 20.0
+      allow_partial_joints_goal: false
+      constraints:
+        stopped_velocity_tolerance: 0.0
+        goal_time: 0.0
+
+  twist_controller:
+    ros__parameters:
+      joint: tcp
+      interface_names:
+        - twist.linear.x
+        - twist.linear.y
+        - twist.linear.z
+        - twist.angular.x
+        - twist.angular.y
+        - twist.angular.z
+
+  robotiq_gripper_controller:
+    ros__parameters:
+      default: true
+      joint: robotiq_85_left_knuckle_joint
+      allow_stalling: true
diff --git a/config/robotiq_controllers.yaml b/config/robotiq_controllers.yaml
@@ -0,0 +1,13 @@
+/**:
+  robotiq_gripper_controller:
+    ros__parameters:
+      default: true
+      joint: robotiq_85_left_knuckle_joint
+      use_effort_interface: true
+      use_speed_interface: true
+
+  robotiq_activation_controller:
+    ros__parameters:
+      default: true
+      joint: robotiq_85_left_knuckle_joint
+
diff --git a/config/ur_controllers.yaml b/config/ur_controllers.yaml
@@ -0,0 +1,95 @@
+/**:
+  speed_scaling_state_broadcaster:
+    ros__parameters:
+      state_publish_rate: 100.0
+
+
+  force_torque_sensor_broadcaster:
+    ros__parameters:
+      sensor_name: tcp_fts_sensor
+      state_interface_names:
+        - force.x
+        - force.y
+        - force.z
+        - torque.x
+        - torque.y
+        - torque.z
+      frame_id: tool0
+      topic_name: ft_data
+
+
+  joint_trajectory_controller:
+    ros__parameters:
+      joints:
+        - shoulder_pan_joint
+        - shoulder_lift_joint
+        - elbow_joint
+        - wrist_1_joint
+        - wrist_2_joint
+        - wrist_3_joint
+      command_interfaces:
+        - position
+      state_interfaces:
+        - position
+        - velocity
+      state_publish_rate: 100.0
+      action_monitor_rate: 20.0
+      allow_partial_joints_goal: false
+      constraints:
+        stopped_velocity_tolerance: 0.2
+        goal_time: 0.0
+        shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+        shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+        elbow_joint: { trajectory: 0.2, goal: 0.1 }
+        wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+        wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+        wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+
+
+  scaled_joint_trajectory_controller:
+    ros__parameters:
+      joints:
+        - shoulder_pan_joint
+        - shoulder_lift_joint
+        - elbow_joint
+        - wrist_1_joint
+        - wrist_2_joint
+        - wrist_3_joint
+      command_interfaces:
+        - position
+      state_interfaces:
+        - position
+        - velocity
+      state_publish_rate: 100.0
+      action_monitor_rate: 20.0
+      allow_partial_joints_goal: false
+      constraints:
+        stopped_velocity_tolerance: 0.2
+        goal_time: 0.0
+        shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
+        shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
+        elbow_joint: { trajectory: 0.2, goal: 0.1 }
+        wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
+        wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
+        wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
+
+  forward_velocity_controller:
+    ros__parameters:
+      joints:
+        - shoulder_pan_joint
+        - shoulder_lift_joint
+        - elbow_joint
+        - wrist_1_joint
+        - wrist_2_joint
+        - wrist_3_joint
+      interface_name: velocity
+
+  forward_position_controller:
+    ros__parameters:
+      joints:
+        - shoulder_pan_joint
+        - shoulder_lift_joint
+        - elbow_joint
+        - wrist_1_joint
+        - wrist_2_joint
+        - wrist_3_joint
diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py
@@ -41,10 +41,11 @@ def get_launch_description(name: str, package: str, namespace: str, component: y
     device_namespace = get_value(component, "device_namespace")
     robot_namespace = namespace
 
-    if len(robot_namespace) and robot_namespace[0] != "/":
-        robot_namespace = "/" + robot_namespace
-    if len(device_namespace) and device_namespace[0] != "/":
-        device_namespace = "/" + device_namespace
+    if not name.find("ur3") and not name.find("kinova"):
+        if len(robot_namespace) and robot_namespace[0] != "/":
+            robot_namespace = "/" + robot_namespace
+        if len(device_namespace) and device_namespace[0] != "/":
+            device_namespace = "/" + device_namespace
 
     return IncludeLaunchDescription(
         PythonLaunchDescriptionSource([package, "/launch/gz_", name, ".launch.py"]),
@@ -67,6 +68,16 @@ def get_launch_descriptions_from_yaml_node(
         "LDR13": "ouster_os",
         "LDR20": "velodyne",
         "CAM01": "orbbec_astra",
+        "MAN01": "ur",
+        "MAN02": "ur",
+        # "MAN03": "kinova_lite"  sim_isaac error
+        "MAN04": "kinova_6dof",
+        "MAN05": "kinova_6dof",
+        "MAN06": "kinova_7dof",
+        "MAN07": "kinova_7dof",
+        "GRP02": "robotiq",
+        # "GRP03": "robotiq", Waiting for release
+        # https://github.com/PickNikRobotics/ros2_robotiq_gripper/blob/main/robotiq_description/urdf/robotiq_2f_85_macro.urdf.xacro
     }
 
     for component in node["components"]: