Syncing & RS updates Humble (#586)

depthai_ros_driver/launch/camera.launch.py

@@ -74,31 +74,78 @@ def launch_setup(context, *args, **kwargs):
     color_sens_name = "rgb"
     stereo_sens_name = "stereo"
     points_topic_name = f"{name}/points"
+    if pointcloud_enable.perform(context) == "true":
+        parameter_overrides = {
+            "pipeline_gen": {"i_enable_sync": True},
+            "rgb": {"i_synced": True},
+            "stereo": {"i_synced": True},
+        }
+    depth_topic_suffix = "image_raw"
     if rs_compat.perform(context) == "true":
+        depth_topic_suffix = "image_rect_raw"
+        depth_profile = LaunchConfiguration("depth_module.depth_profile").perform(
+            context
+        )
+        color_profile = LaunchConfiguration("rgb_camera.color_profile").perform(context)
+        infra_profile = LaunchConfiguration("depth_module.infra_profile").perform(
+            context
+        )
+        # split profile string (0,0,0 or 0x0x0 or 0X0X0) into with (int) height(int) and fps(double)
+        # find delimiter
+        delimiter = ","
+        if "x" in depth_profile:
+            delimiter = "x"
+        elif "X" in depth_profile:
+            delimiter = "X"
+        depth_profile = depth_profile.split(delimiter)
+        color_profile = color_profile.split(delimiter)
+        infra_profile = infra_profile.split(delimiter)
+
         color_sens_name = "color"
         stereo_sens_name = "depth"
         if name == "oak":
             name = "camera"
         points_topic_name = f"{name}/depth/color/points"
-        if namespace== "":
+        if namespace == "":
             namespace = "camera"
         if parent_frame == "oak-d-base-frame":
             parent_frame = f"{name}_link"
         parameter_overrides = {
             "camera": {
                 "i_rs_compat": True,
             },
+            "pipeline_gen": {
+                "i_enable_sync": True,
+            },
             "color": {
                 "i_publish_topic": is_launch_config_true(context, "enable_color"),
+                "i_synced": True,
+                "i_width": int(color_profile[0]),
+                "i_height": int(color_profile[1]),
+                "i_fps": float(color_profile[2]),
             },
             "depth": {
                 "i_publish_topic": is_launch_config_true(context, "enable_depth"),
+                "i_synced": True,
+                "i_width": int(depth_profile[0]),
+                "i_height": int(depth_profile[1]),
+                "i_fps": float(depth_profile[2]),
+            },
+            "infra1": {
+                "i_width": int(infra_profile[0]),
+                "i_height": int(infra_profile[1]),
+                "i_fps": float(infra_profile[2]),
+            },
+            "infra2": {
+                "i_width": int(infra_profile[0]),
+                "i_height": int(infra_profile[1]),
+                "i_fps": float(infra_profile[2]),
             },
         }
         parameter_overrides["depth"] = {
-                "i_publish_left_rect": True,
-                "i_publish_right_rect": True,
-            }
+            "i_publish_left_rect": True,
+            "i_publish_right_rect": True,
+        }
 
     tf_params = {}
     if publish_tf_from_calibration.perform(context) == "true":
@@ -217,7 +264,7 @@ def launch_setup(context, *args, **kwargs):
                     remappings=[
                         (
                             "depth_registered/image_rect",
-                            f"{name}/{stereo_sens_name}/image_raw",
+                            f"{name}/{stereo_sens_name}/{depth_topic_suffix}",
                         ),
                         (
                             "rgb/image_rect_color",
@@ -285,6 +332,9 @@ def generate_launch_description():
         DeclareLaunchArgument("enable_depth", default_value="true"),
         DeclareLaunchArgument("enable_infra1", default_value="false"),
         DeclareLaunchArgument("enable_infra2", default_value="false"),
+        DeclareLaunchArgument("depth_module.depth_profile", default_value="1280,720,30"),
+        DeclareLaunchArgument("rgb_camera.color_profile", default_value="1280,720,30"),
+        DeclareLaunchArgument("depth_module.infra_profile", default_value="1280,720,30"),
     ]
 
     return LaunchDescription(

depthai_ros_driver/launch/rtabmap.launch.py

@@ -43,28 +43,6 @@ def launch_setup(context, *args, **kwargs):
             ),
             launch_arguments={"name": name, "params_file": params_file}.items(),
         ),
-        LoadComposableNodes(
-            condition=IfCondition(LaunchConfiguration("rectify_rgb")),
-            target_container=name + "_container",
-            composable_node_descriptions=[
-                ComposableNode(
-                    package="image_proc",
-                    plugin="image_proc::RectifyNode",
-                    name="rectify_color_node",
-                    remappings=[
-                        ("image", name + "/rgb/image_raw"),
-                        ("camera_info", name + "/rgb/camera_info"),
-                        ("image_rect", name + "/rgb/image_rect"),
-                        ("image_rect/compressed", name + "/rgb/image_rect/compressed"),
-                        (
-                            "image_rect/compressedDepth",
-                            name + "/rgb/image_rect/compressedDepth",
-                        ),
-                        ("image_rect/theora", name + "/rgb/image_rect/theora"),
-                    ],
-                )
-            ],
-        ),
         LoadComposableNodes(
             target_container=name + "_container",
             composable_node_descriptions=[
@@ -107,7 +85,6 @@ def generate_launch_description():
             "params_file",
             default_value=os.path.join(depthai_prefix, "config", "rgbd.yaml"),
         ),
-        DeclareLaunchArgument("rectify_rgb", default_value="True"),
     ]
 
     return LaunchDescription(

depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp

@@ -155,7 +155,7 @@ void Stereo::setupRectQueue(std::shared_ptr<dai::Device> device,
     pubConfig.width = ph->getOtherNodeParam<int>(sensorName, "i_width");
     pubConfig.height = ph->getOtherNodeParam<int>(sensorName, "i_height");
     pubConfig.topicName = "~/" + sensorName;
-    pubConfig.topicSuffix = "/image_rect";
+    pubConfig.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw";
     pubConfig.maxQSize = ph->getOtherNodeParam<int>(sensorName, "i_max_q_size");
     pubConfig.socket = sensorInfo.socket;
     pubConfig.infoMgrSuffix = "rect";
@@ -197,6 +197,7 @@ void Stereo::setupStereoQueue(std::shared_ptr<dai::Device> device) {
     utils::ImgPublisherConfig pubConf;
     pubConf.daiNodeName = getName();
     pubConf.topicName = "~/" + getName();
+	pubConf.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw";
     pubConf.rectified = !convConfig.alphaScalingEnabled;
     pubConf.width = ph->getParam<int>("i_width");
     pubConf.height = ph->getParam<int>("i_height");

depthai_ros_driver/src/param_handlers/pipeline_gen_param_handler.cpp

@@ -11,7 +11,7 @@ PipelineGenParamHandler::~PipelineGenParamHandler() = default;
 void PipelineGenParamHandler::declareParams() {
     declareAndLogParam<bool>("i_enable_imu", true);
     declareAndLogParam<bool>("i_enable_diagnostics", true);
-    declareAndLogParam<bool>("i_enable_sync", true);
+    declareAndLogParam<bool>("i_enable_sync", false);
 }
 dai::CameraControl PipelineGenParamHandler::setRuntimeParams(const std::vector<rclcpp::Parameter>& /*params*/) {
     dai::CameraControl ctrl;

depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp

@@ -38,7 +38,7 @@ void SensorParamHandler::declareCommonParams(dai::CameraBoardSocket socket) {
     declareAndLogParam<bool>("i_add_exposure_offset", false);
     declareAndLogParam<int>("i_exposure_offset", 0);
     declareAndLogParam<bool>("i_reverse_stereo_socket_order", false);
-    declareAndLogParam<bool>("i_synced", true);
+    declareAndLogParam<bool>("i_synced", false);
     declareAndLogParam<bool>("i_publish_compressed", false);
 }
 

depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp

@@ -100,7 +100,7 @@ void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> s
 
     declareAndLogParam<bool>("i_enable_spatial_nn", false);
     declareAndLogParam<std::string>("i_spatial_nn_source", "right");
-    declareAndLogParam<bool>("i_synced", true);
+    declareAndLogParam<bool>("i_synced", false);
 
     stereo->setLeftRightCheck(declareAndLogParam<bool>("i_lr_check", true));
     int width = 1280;