Merge branch 'main' into merge-v5.0-into-main

src/picknik_ur_gazebo_scan_and_plan_config/objectives/estimate_object_pose.xml

@@ -4,7 +4,7 @@
   <BehaviorTree ID="Estimate Object Pose" _description="Estimate an object pose from a reference mesh file using ICP on live point cloud data." _favorite="false" _hardcoded="false">
     <Control ID="Sequence">
       <Action ID="LoadPointCloudFromFile" file_path="{object_model_file_path}" frame_id="world" num_sampled_points="10000" random_seed="1234" point_cloud="{model_point_cloud}" scale="1.0" color="255;150;0"/>
-      <Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
+      <Action ID="GetSyncedImageAndPointCloud" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
       <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud}"/>
       <Action ID="CreateStampedPose" reference_frame="world" position_xyz="{guess_position}" orientation_xyzw="{guess_orientation}" stamped_pose="{object_stamped_pose_estimate}"/>
       <Action ID="TransformPointCloud" input_cloud="{model_point_cloud}" transform_pose="{object_stamped_pose_estimate}" output_cloud="{model_point_cloud}"/>

src/picknik_ur_gazebo_scan_and_plan_config/objectives/object_inspection.xml

@@ -11,7 +11,7 @@
         <Control ID="Sequence">
           <Action ID="TransformPoseWithPose" input_pose="{trajectory_pose}" transform_pose="{model_to_real_pose}" output_pose="{target_pose}"/>
           <SubTree ID="Move To Pose" target_pose="{target_pose}" ik_frame="grasp_link" planning_group_name="manipulator" controller_names="/joint_trajectory_controller /robotiq_gripper_controller" use_all_planners="false"/>
-          <Action ID="GetSynchronizedCameraTopics" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
+          <Action ID="GetSyncedImageAndPointCloud" point_cloud_topic_name="/wrist_mounted_camera/depth/color/points" rgb_image_topic_name="/wrist_mounted_camera/color/image_raw" rgb_camera_info_topic_name="/wrist_mounted_camera/color/camera_info" point_cloud="{point_cloud}" rgb_image="{rgb_image}" rgb_camera_info="{rgb_camera_info}"/>
           <Action ID="TransformPointCloudFrame" input_cloud="{point_cloud}" target_frame="world" output_cloud="{point_cloud_world}"/>
           <Action ID="AddPointCloudToVector" point_cloud="{point_cloud_world}" point_cloud_vector="{point_cloud_vector}"/>
         </Control>

src/picknik_ur_multi_arm_config/config/cameras.yaml

@@ -23,8 +23,8 @@
       registered_info: "/scene_camera/color/camera_info"
       registered_image: "/scene_camera/depth/image_rect_raw"
 
-- left_wrist_mounted_camera:
-    camera_name: "left_wrist_mounted_camera"
+- first_wrist_mounted_camera:
+    camera_name: "first_wrist_mounted_camera"
     type: "sim"
     use: True
     serial_no: "0"
@@ -35,18 +35,18 @@
     enable_pointcloud: True
 
     # information about the topics the camera publishes the raw image and info
-    rgb_info: "/left_wrist_mounted_camera/color/camera_info"
-    rgb_image: "/left_wrist_mounted_camera/color/image_raw"
+    rgb_info: "/first_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/first_wrist_mounted_camera/color/image_raw"
 
     # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
     registered_rgb_depth_pair:
-      depth_info: "/left_wrist_mounted_camera/depth/camera_info"
-      depth_image: "/left_wrist_mounted_camera/depth/image_rect_raw"
-      registered_info: "/left_wrist_mounted_camera/depth_registered/camera_info"
-      registered_image: "/left_wrist_mounted_camera/depth_registered/image_rect"
+      depth_info: "/first_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/first_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/first_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/first_wrist_mounted_camera/depth_registered/image_rect"
 
-- right_wrist_mounted_camera:
-    camera_name: "right_wrist_mounted_camera"
+- second_wrist_mounted_camera:
+    camera_name: "second_wrist_mounted_camera"
     type: "sim"
     use: True
     serial_no: "0"
@@ -57,12 +57,56 @@
     enable_pointcloud: True
 
     # information about the topics the camera publishes the raw image and info
-    rgb_info: "/right_wrist_mounted_camera/color/camera_info"
-    rgb_image: "/right_wrist_mounted_camera/color/image_raw"
+    rgb_info: "/second_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/second_wrist_mounted_camera/color/image_raw"
 
     # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
     registered_rgb_depth_pair:
-      depth_info: "/right_wrist_mounted_camera/depth/camera_info"
-      depth_image: "/right_wrist_mounted_camera/depth/image_rect_raw"
-      registered_info: "/right_wrist_mounted_camera/depth_registered/camera_info"
-      registered_image: "/right_wrist_mounted_camera/depth_registered/image_rect"
+      depth_info: "/second_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/second_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/second_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/second_wrist_mounted_camera/depth_registered/image_rect"
+
+- third_wrist_mounted_camera:
+    camera_name: "third_wrist_mounted_camera"
+    type: "sim"
+    use: True
+    serial_no: "0"
+    device_type: "D415"
+    framerate: 6
+    image_width: 640
+    image_height: 480
+    enable_pointcloud: True
+
+    # information about the topics the camera publishes the raw image and info
+    rgb_info: "/third_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/third_wrist_mounted_camera/color/image_raw"
+
+    # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
+    registered_rgb_depth_pair:
+      depth_info: "/third_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/third_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/third_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/third_wrist_mounted_camera/depth_registered/image_rect"
+
+- fourth_wrist_mounted_camera:
+    camera_name: "fourth_wrist_mounted_camera"
+    type: "sim"
+    use: True
+    serial_no: "0"
+    device_type: "D415"
+    framerate: 6
+    image_width: 640
+    image_height: 480
+    enable_pointcloud: True
+
+    # information about the topics the camera publishes the raw image and info
+    rgb_info: "/fourth_wrist_mounted_camera/color/camera_info"
+    rgb_image: "/fourth_wrist_mounted_camera/color/image_raw"
+
+    # By adding registered_rgb_depth_pair, This camera can be used for "Set Transform From Click"
+    registered_rgb_depth_pair:
+      depth_info: "/fourth_wrist_mounted_camera/depth/camera_info"
+      depth_image: "/fourth_wrist_mounted_camera/depth/image_rect_raw"
+      registered_info: "/fourth_wrist_mounted_camera/depth_registered/camera_info"
+      registered_image: "/fourth_wrist_mounted_camera/depth_registered/image_rect"

src/picknik_ur_multi_arm_config/config/config.yaml

@@ -62,15 +62,15 @@ hardware:
   robot_description:
     urdf:
       package: "picknik_ur_multi_arm_config"
-      path: "description/dual_arm_ur.xacro"
+      path: "description/multi_arm_ur.xacro"
     srdf:
       package: "picknik_ur_multi_arm_config"
-      path: "config/moveit/dual_arm_ur.srdf"
+      path: "config/moveit/multi_arm_ur.srdf"
     # Specify any additional parameters required for the URDF.
     # Many of these are specific to the UR descriptions packages, and can be customized as needed.
     # [Optional]
     urdf_params:
-      - name: "dual_arm_ur"
+      - name: "multi_arm_ur"
       - prefix: ""
       - use_fake_hardware: "%>> hardware.simulated"
       - use_pinch_links: "true"
@@ -112,7 +112,7 @@ ros_global_params:
 # [Required]
 moveit_params:
   # Used by the Waypoint Manager to save joint states from this joint group.
-  joint_group_name: "dual_arm_manipulator"
+  joint_group_name: "first_manipulator"
 
   ompl_planning:
     package: "picknik_ur_multi_arm_config"
@@ -128,7 +128,7 @@ moveit_params:
     path: "config/moveit/trac_ik_kinematics_distance.yaml"
   servo:
     package: "picknik_ur_multi_arm_config"
-    path: "config/moveit/right_ur_servo.yaml"
+    path: "config/moveit/multi_ur_servo.yaml"
   sensors_3d:
     package: "picknik_ur_base_config"
     path: "config/moveit/sensors_3d.yaml"
@@ -137,7 +137,7 @@ moveit_params:
     path: "config/moveit/trac_ik_kinematics_speed.yaml"
   joint_limits:
     package: "picknik_ur_multi_arm_config"
-    path: "config/moveit/dual_arm_joint_limits.yaml"
+    path: "config/moveit/multi_arm_joint_limits.yaml"
   pilz_cartesian_limits:
     package: "picknik_ur_base_config"
     path: "config/moveit/pilz_cartesian_limits.yaml"
@@ -160,34 +160,37 @@ ui_params:
   # By default, MoveIt Pro uses a frame called "grasp_link" for tool grasp pose rendering
   # and planning.
   # [Required]
-  servo_endpoint_frame_id: "right_grasp_link"
+  servo_endpoint_frame_id: "first_grasp_link"
 
 # Configuration for launching ros2_control processes.
 # [Required, if using ros2_control]
 ros2_control:
   config:
     package: "picknik_ur_multi_arm_config"
-    path: "config/control/picknik_dual_ur.ros2_control.yaml"
+    path: "config/control/picknik_multi_ur.ros2_control.yaml"
   # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
   # If not specified, it is up to the user to ensure the appropriate controllers are active and available
   # for running the application.
   # [Optional, default=[]]
   controllers_active_at_startup:
     - "joint_state_broadcaster"
-    - "left_servo_controller"
-    - "right_servo_controller"
-    - "left_io_and_status_controller"
-    - "right_io_and_status_controller"
-    - "left_robotiq_activation_controller"
-    - "left_robotiq_gripper_controller"
-    - "right_robotiq_activation_controller"
-    - "right_robotiq_gripper_controller"
+    - "servo_controller"
+    - "first_io_and_status_controller"
+    - "second_io_and_status_controller"
+    - "third_io_and_status_controller"
+    - "fourth_io_and_status_controller"
+    - "first_robotiq_activation_controller"
+    - "second_robotiq_activation_controller"
+    - "third_robotiq_activation_controller"
+    - "fourth_robotiq_activation_controller"
+    - "first_robotiq_gripper_controller"
+    - "second_robotiq_gripper_controller"
+    - "third_robotiq_gripper_controller"
+    - "fourth_robotiq_gripper_controller"
   # Load but do not start these controllers so they can be activated later if needed.
   # [Optional, default=[]]
   controllers_inactive_at_startup:
-    - "left_joint_trajectory_controller"
-    - "right_joint_trajectory_controller"
-    - "dual_arm_joint_trajectory_controller"
+    - "multi_arm_joint_trajectory_controller"
   # Any controllers here will not be spawned by MoveIt Pro.
   # [Optional, default=[]]
   controllers_not_managed: []