Critical bug in rvc_pose_detector fixes and multiple realsense camera support (open-edge-platform#975)

Author: dariorusso1972

robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d405camera.xacro

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+
+<robot name="d405_camera" xmlns:xacro="http://ros.org/wiki/xacro">
+<xacro:arg name="use_nominal_extrinsics" default="false"/>
+<xacro:arg name="name" default="cameraipc0"/>
+  <xacro:include filename="$(find realsense2_description)/urdf/_d405.urdf.xacro" />
+  <link name="world" />
+  <xacro:sensor_d405 parent="world" name="$(arg name)" use_nominal_extrinsics="$(arg use_nominal_extrinsics)" >
+	  <origin xyz="0.36 0.66 0.755" rpy="-3.141592654 1.570796327 -1.570796327"/>
+  </xacro:sensor_d405>
+</robot>
+

robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d405cameraipc.xacro

@@ -1,14 +1,14 @@
 <?xml version="1.0"?>
 
-<robot name="realsense2_camera" xmlns:xacro="http://ros.org/wiki/xacro">
+<robot name="d435_camera" xmlns:xacro="http://ros.org/wiki/xacro">
 <xacro:arg name="use_nominal_extrinsics" default="false"/>
-<xacro:arg name="name" default="camera1"/>
+<xacro:arg name="name" default="cameraipc"/>
 <xacro:arg name="use_mesh" default="true"/>
-<xacro:arg name="use_plug" default="true"/>
+<xacro:arg name="use_plug" default="false"/>
   <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
   <link name="world" />
   <xacro:sensor_d435 parent="world" name="$(arg name)" use_mesh="$(arg use_mesh)" add_plug="$(arg use_plug)" use_nominal_extrinsics="$(arg use_nominal_extrinsics)" >
-      <origin rpy="-1.503104 0.485758 0.066405" xyz="-0.785000 0.802000 0.657000"/>
+      <origin xyz="0.36 0.66 0.755" rpy="-3.141592654 1.570796327 -1.570796327"/>
   </xacro:sensor_d435>
 </robot>
 

robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d415cameraipc0.xacro

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+
+<robot name="d455_camera" xmlns:xacro="http://ros.org/wiki/xacro">
+<xacro:arg name="use_nominal_extrinsics" default="false"/>
+<xacro:arg name="name" default="cameraipc0"/>
+  <xacro:include filename="$(find realsense2_description)/urdf/_d455.urdf.xacro" />
+  <link name="world" />
+  <xacro:sensor_d455 parent="world" name="$(arg name)"  use_nominal_extrinsics="$(arg use_nominal_extrinsics)" >
+	  <origin xyz="0.36 0.66 0.755" rpy="-3.141592654 1.570796327 -1.570796327"/>
+  </xacro:sensor_d455>
+</robot>
+

robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d415cameraipc1.xacro

@@ -15,7 +15,6 @@ Panels:
         - /Pose1/Topic1
         - /PointCloud22/Topic1
         - /MarkerArray2/Topic1
-        - /PlanningScene1
         - /PlanningScene1/Scene Geometry1
         - /PlanningScene1/Scene Robot1
       Splitter Ratio: 0.5823529362678528
@@ -425,7 +424,7 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Best Effort
-        Value: camera/depth/color/points
+        Value: camera/cameraipc/depth/color/points
       Use Fixed Frame: true
       Use rainbow: true
       Value: false
@@ -729,21 +728,34 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: d405rs_description
-      Enabled: false
+        Value: rs_description
+      Enabled: true
       Links:
         All Links Enabled: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        cameraipc_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        cameraipc_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
       Mass Properties:
         Inertia: false
         Mass: false
       Name: d405RS
       TF Prefix: ""
       Update Interval: 0
-      Value: false
+      Value: true
       Visual Enabled: true
   Enabled: true
   Global Options:

…ler_use_case/cameraurdf/d435camera.xacro …_use_case/cameraurdf/d435cameraipc.xacrorobotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d435camera.xacro renamed to robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d435cameraipc.xacro robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d435camera.xacro renamed to robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d435cameraipc.xacro

@@ -74,6 +74,10 @@ def launch_setup(context, *args, **kwargs):
     # UR specific arguments
     #ur_type_arg = LaunchConfiguration( "ur_type", default="ur5e")
     #robot_ip_arg = LaunchConfiguration( "robot_ip", default="10.11.12.99")
+    
+    myRSModelTypeConf = LaunchConfiguration("rs_model")
+    myRSModelType= context.perform_substitution(myRSModelTypeConf)
+    
     launch_rviz_arg = LaunchConfiguration( "launch_rviz", default="false")
     headless_mode_arg = LaunchConfiguration( "headless_mode", default="false")
 
@@ -384,7 +388,7 @@ def launch_setup(context, *args, **kwargs):
 
     )
 
-    camera_xacro_path = robot_demo_main_dir + '/cameraurdf/d415camera' + whoAmI_arg + '.xacro'
+    camera_xacro_path = robot_demo_main_dir + '/cameraurdf/' + myRSModelType + 'camera' + whoAmI_arg + '.xacro'
     camera_robot_state_publisher_node = Node(
             name='camera_robot_state_publisher',
 
@@ -528,13 +532,13 @@ def launch_setup(context, *args, **kwargs):
             )
         ]
     if ((motion_controller_string == "servo") or (motion_controller_string == "linear_controller")):
-       nodes_to_start.append(io_and_status_controller_spawner) 
-       nodes_to_start.append(dashboard_client_node) 
+       #nodes_to_start.append(io_and_status_controller_spawner) 
+       #nodes_to_start.append(dashboard_client_node) 
        nodes_to_start.append(control_node) 
        nodes_to_start.append(joint_state_broadcaster_spawner) 
        nodes_to_start.append(robot_controller_spawner) 
        nodes_to_start.append(gripper_forward_command_controller_spawner)
-       nodes_to_start.append(joint_trajectory_controller_node)
+       #nodes_to_start.append(joint_trajectory_controller_node)
        nodes_to_start.append(gripper_sensor_broadcaster)
     return nodes_to_start
 
@@ -729,4 +733,12 @@ def generate_launch_description():
             description="Port that will be opened to forward script commands from the driver to the robot",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument( 
+            "rs_model", 
+            description="model type, can be d405, d415, d435, d455",
+            default_value="d415",
+            choices=['d405', 'd415', 'd435', 'd455']
+        )
+    )
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)] )

robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/cameraurdf/d455cameraipc.xacro

@@ -44,6 +44,11 @@ find_package(rvc_messages REQUIRED)
 find_package(rvc_vision_messages REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
 
 # find non-ROS dependencies
 find_package(PCL REQUIRED)
@@ -66,6 +71,9 @@ ament_target_dependencies(${PROJECT_NAME_PLUGIN}
     pcl_conversions
     message_filters
     tf2_eigen
+    tf2_ros
+    tf2_geometry_msgs 
+    tf2_sensor_msgs 
 )
 target_include_directories(${PROJECT_NAME_PLUGIN} PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

robotics-ai-suite/robot-vision-control/src/rvc_dynamic_motion_controller_use_case/config/robot_demo_config_qt.rviz

@@ -35,8 +35,15 @@
 #include <message_filters/sync_policies/exact_time.h>
 
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/extract_indices.h>
 
 #include <tf2_eigen/tf2_eigen.hpp>
+
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>  // for doTransform(sensor_msgs::PointCloud2)
+
 #include "matcher.hpp"
 
 namespace RVC
@@ -50,6 +57,13 @@ struct MatchOperation
 class PoseDetector : public rclcpp::Node
 {
 private:
+    std::unique_ptr<tf2_ros::Buffer> tfBuffer;
+    std::shared_ptr<tf2_ros::TransformListener> tfListener;
+	    // Camera intrinsics
+    double fx_, fy_, cx_, cy_;
+    bool intrinsicsReceived;
+    void cameraInfoCallback(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg);
+    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;
 
     message_filters::Subscriber<rvc_vision_messages::msg::RotatedBBList> m_rotated_bb_list_sub;
     message_filters::Subscriber<sensor_msgs::msg::PointCloud2> m_pcl_sub;