Skip to content

Commit

Permalink
update launch file with rs mode
Browse files Browse the repository at this point in the history
  • Loading branch information
@Serafadam
Serafadam committed Sep 17, 2024
1 parent cce4546 commit 92ba383
Show file tree
Hide file tree
Showing 9 changed files with 191 additions and 94 deletions.
236 changes: 169 additions & 67 deletions depthai_ros_driver/launch/camera.launch
View file
Original file line number Diff line number Diff line change
@@ -1,70 +1,172 @@
<?xml version="1.0"?>
<launch>

<arg name="publish_tf_from_calibration" default="false" />
<arg name="imu_from_descr" default="false" />
<arg name="override_cam_model" default="false" />

<arg name="name" default="oak" />
<arg name="params_file" default="$(find depthai_ros_driver)/config/camera.yaml"/>
<arg name="camera_model" default="OAK-D" />

<arg name="base_frame" default="oak-d_frame" />
<arg name="parent_frame" default="oak-d-base-frame" />

<arg name="cam_pos_x" default="0.0" />
<!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_y" default="0.0" />
<!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_z" default="0.0" />
<!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_roll" default="0.0" />
<!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_pitch" default="0.0" />
<!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_yaw" default="0.0" />
<!-- Orientation respect to base frame (i.e. "base_link) -->

<param name="$(arg name)/camera_i_camera_model" value="$(arg camera_model)" if="$(arg override_cam_model)"/>
<param name="$(arg name)/camera_i_base_frame" value="$(arg base_frame)"/>
<param name="$(arg name)/camera_i_parent_frame" value="$(arg parent_frame)"/>
<param name="$(arg name)/camera_i_cam_pos_x" value="$(arg cam_pos_x)"/>
<param name="$(arg name)/camera_i_cam_pos_y" value="$(arg cam_pos_y)"/>
<param name="$(arg name)/camera_i_cam_pos_z" value="$(arg cam_pos_z)"/>
<param name="$(arg name)/camera_i_cam_roll" value="$(arg cam_roll)"/>
<param name="$(arg name)/camera_i_cam_pitch" value="$(arg cam_pitch)"/>
<param name="$(arg name)/camera_i_cam_yaw" value="$(arg cam_yaw)"/>
<param name="$(arg name)/camera_i_imu_from_descr" value="$(arg imu_from_descr)"/>
<param name="$(arg name)/camera_i_publish_tf_from_calibration" value="$(arg publish_tf_from_calibration)"/>

<arg name="launch_prefix" default=""/>


<rosparam file="$(arg params_file)" />
<node pkg="rosservice" if="$(optenv DEPTHAI_DEBUG 0)" type="rosservice" name="set_log_level" args="call --wait /oak_nodelet_manager/set_logger_level 'ros.depthai_ros_driver' 'debug'" />

<include unless="$(arg publish_tf_from_calibration)" file="$(find depthai_descriptions)/launch/urdf.launch">
<arg name="base_frame" value="$(arg name)" />
<arg name="parent_frame" value="$(arg parent_frame)"/>
<arg name="camera_model" value="$(arg camera_model)"/>
<arg name="tf_prefix" value="$(arg name)" />
<arg name="cam_pos_x" value="$(arg cam_pos_x)" />
<arg name="cam_pos_y" value="$(arg cam_pos_y)" />
<arg name="cam_pos_z" value="$(arg cam_pos_z)" />
<arg name="cam_roll" value="$(arg cam_roll)" />
<arg name="cam_pitch" value="$(arg cam_pitch)" />
<arg name="cam_yaw" value="$(arg cam_yaw)" />
</include>


<node pkg="nodelet" type="nodelet" name="$(arg name)_nodelet_manager" launch-prefix="$(arg launch_prefix)" args="manager" output="screen">
<remap from="/nodelet_manager/load_nodelet" to="$(arg name)/nodelet_manager/load_nodelet"/>
<remap from="/nodelet_manager/unload_nodelet" to="$(arg name)/nodelet_manager/unload_nodelet"/>
<remap from="/nodelet_manager/list" to="$(arg name)/nodelet_manager/list"/>
</node>

<node name="$(arg name)" pkg="nodelet" type="nodelet" output="screen" required="true" args="load depthai_ros_driver/Camera $(arg name)_nodelet_manager">
</node>


</launch>
<arg name="publish_tf_from_calibration" default="false" />
<arg name="imu_from_descr" default="false" />
<arg name="override_cam_model" default="false" />
<arg name="rectify_rgb" default="true" />
<arg name="enable_pointcloud" default="false" />
<arg name="enable_color" default="true" />
<arg name="enable_depth" default="true" />
<arg name="enable_infra1" default="false" />
<arg name="enable_infra2" default="false" />
<arg name="depth_module_depth_profile" default="1280,720,30" />
<arg name="rgb_camera_color_profile" default="1280,720,30" />
<arg name="depth_module_infra_profile" default="1280,720,30" />
<arg name="rs_compat" default="false" />

<arg name="name" default="camera" if="$(arg rs_compat)" />
<arg name="name" default="oak" unless="$(arg rs_compat)" />

<arg name="namespace" default="camera" if="$(arg rs_compat)" />
<arg name="namespace" default="" unless="$(arg rs_compat)" />


<arg name="parent_frame" value="$(arg name)_link" if="$(arg rs_compat)" />
<arg name="parent_frame" default="oak-d-base-frame" unless="$(arg rs_compat)" />

<arg name="points_topic_name" value="$(arg name)/depth/color/points" if="$(arg rs_compat)"/>
<arg name="points_topic_name" value="$(arg name)/points" unless="$(arg rs_compat)"/>

<arg name="color_sens_name" value="color" if="$(arg rs_compat)"/>
<arg name="color_sens_name" value="rgb" unless="$(arg rs_compat)"/>

<arg name="stereo_sens_name" value="depth" if="$(arg rs_compat)" />
<arg name="stereo_sens_name" value="stereo" unless="$(arg rs_compat)" />

<arg name="depth_topic_suffix" value="image_rect_raw" if="$(arg rs_compat)"/>
<arg name="depth_topic_suffix" value="image_raw" unless="$(arg rs_compat)"/>
<group if="$(arg rs_compat)">

<param name="$(arg name)/camera_i_rs_compat" value="true" />
<param name="$(arg name)/pipeline_gen_i_enable_sync" value="true" />
<param name="$(arg name)/color_i_synced" value="true" />
<param name="$(arg name)/color_i_publish_topic" value="$(arg enable_color)" />
<param name="$(arg name)/color_i_width"
value="$(eval rgb_camera_color_profile.split(',')[0])" />
<param name="$(arg name)/color_i_height"
value="$(eval rgb_camera_color_profile.split(',')[1])" />
<param name="$(arg name)/color_i_fps" value="$(eval rgb_camera_color_profile.split(',')[2])" />

<param name="$(arg name)/depth_i_synced" value="true" />
<param name="$(arg name)/depth_i_publish_topic" value="$(arg enable_depth)" />
<param name="$(arg name)/depth_i_width"
value="$(eval depth_module_depth_profile.split(',')[0])" />
<param name="$(arg name)/depth_i_height"
value="$(eval depth_module_depth_profile.split(',')[1])" />
<param name="$(arg name)/depth_i_fps"
value="$(eval depth_module_depth_profile.split(',')[2])" />

<param name="$(arg name)/infra1_i_publish_topic" value="$(arg enable_infra1)" />
<param name="$(arg name)/infra1_i_width"
value="$(eval depth_module_infra_profile.split(',')[0])" />
<param name="$(arg name)/infra1_i_height"
value="$(eval depth_module_infra_profile.split(',')[1])" />
<param name="$(arg name)/infra1_i_fps"
value="$(eval depth_module_infra_profile.split(',')[2])" />

<param name="$(arg name)/infra2_i_publish_topic" value="$(arg enable_infra2)" />
<param name="$(arg name)/infra2_i_width"
value="$(eval depth_module_infra_profile.split(',')[0])" />
<param name="$(arg name)/infra2_i_height"
value="$(eval depth_module_infra_profile.split(',')[1])" />
<param name="$(arg name)/infra2_i_fps"
value="$(eval depth_module_infra_profile.split(',')[2])" />

<param name="$(arg name)/depth_i_publish_left_rect" value="true" />
<param name="$(arg name)/depth_i_publish_right_rect" value="true" />
</group>

<arg name="params_file" default="$(find depthai_ros_driver)/config/camera.yaml" />
<arg name="camera_model" default="OAK-D" />

<arg name="base_frame" default="oak-d_frame" />

<arg name="cam_pos_x" default="0.0" />
<!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_y" default="0.0" />
<!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_pos_z" default="0.0" />
<!-- Position respect to base frame (i.e. "base_link) -->
<arg name="cam_roll" default="0.0" />
<!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_pitch" default="0.0" />
<!-- Orientation respect to base frame (i.e. "base_link) -->
<arg name="cam_yaw" default="0.0" />
<!-- Orientation respect to base frame (i.e. "base_link) -->

<param name="$(arg name)/camera_i_camera_model" value="$(arg camera_model)"
if="$(arg override_cam_model)" />
<param name="$(arg name)/camera_i_base_frame" value="$(arg base_frame)" />
<param name="$(arg name)/camera_i_parent_frame" value="$(arg parent_frame)" />
<param name="$(arg name)/camera_i_cam_pos_x" value="$(arg cam_pos_x)" />
<param name="$(arg name)/camera_i_cam_pos_y" value="$(arg cam_pos_y)" />
<param name="$(arg name)/camera_i_cam_pos_z" value="$(arg cam_pos_z)" />
<param name="$(arg name)/camera_i_cam_roll" value="$(arg cam_roll)" />
<param name="$(arg name)/camera_i_cam_pitch" value="$(arg cam_pitch)" />
<param name="$(arg name)/camera_i_cam_yaw" value="$(arg cam_yaw)" />
<param name="$(arg name)/camera_i_imu_from_descr" value="$(arg imu_from_descr)" />
<param name="$(arg name)/camera_i_publish_tf_from_calibration"
value="$(arg publish_tf_from_calibration)" />

<group if="$(arg enable_pointcloud)">
<param name="$(arg name)/pipeline_gen_i_enable_sync" value="true" />
<param name="$(arg name)/rgb_i_synced" value="true" />
<param name="$(arg name)/stereo_i_synced" value="true" />
</group>

<arg name="launch_prefix" default="" />


<rosparam file="$(arg params_file)" />
<node pkg="rosservice" if="$(optenv DEPTHAI_DEBUG 0)" type="rosservice" name="set_log_level"
args="call --wait /oak_nodelet_manager/set_logger_level 'ros.depthai_ros_driver' 'debug'" />

<include unless="$(arg publish_tf_from_calibration)"
file="$(find depthai_descriptions)/launch/urdf.launch">
<arg name="base_frame" value="$(arg name)" />
<arg name="parent_frame" value="$(arg parent_frame)" />
<arg name="camera_model" value="$(arg camera_model)" />
<arg name="tf_prefix" value="$(arg name)" />
<arg name="cam_pos_x" value="$(arg cam_pos_x)" />
<arg name="cam_pos_y" value="$(arg cam_pos_y)" />
<arg name="cam_pos_z" value="$(arg cam_pos_z)" />
<arg name="cam_roll" value="$(arg cam_roll)" />
<arg name="cam_pitch" value="$(arg cam_pitch)" />
<arg name="cam_yaw" value="$(arg cam_yaw)" />
<arg name="rs_compat" value="$(arg rs_compat)" />
</include>


<node pkg="nodelet" type="nodelet" name="$(arg name)_nodelet_manager"
launch-prefix="$(arg launch_prefix)" args="manager" output="screen">
<remap from="/nodelet_manager/load_nodelet" to="$(arg name)/nodelet_manager/load_nodelet" />
<remap from="/nodelet_manager/unload_nodelet"
to="$(arg name)/nodelet_manager/unload_nodelet" />
<remap from="/nodelet_manager/list" to="$(arg name)/nodelet_manager/list" />
</node>

<node name="$(arg name)" pkg="nodelet" type="nodelet" output="screen" required="true"
args="load depthai_ros_driver/Camera $(arg name)_nodelet_manager">
</node>

<node pkg="nodelet" type="nodelet" name="rectify_color"
args="load image_proc/rectify $(arg name)_nodelet_manager" if="$(arg rectify_rgb)">
<remap from="image_mono" to="$(arg name)/$(arg color_sens_name)/image_raw" />
<remap from="image_rect" to="$(arg name)/$(arg color_sens_name)/image_rect" />
</node>

<node pkg="nodelet" type="nodelet" name="depth_image_to_rgb_pointcloud"
args="load depth_image_proc/point_cloud_xyzrgb $(arg name)_nodelet_manager"
if="$(arg enable_pointcloud)">
<param name="queue_size" value="10" />

<remap from="rgb/camera_info" to="$(arg name)/$(arg color_sens_name)/camera_info" />
<remap from="rgb/image_rect_color" to="$(arg name)/$(arg color_sens_name)/image_rect" if="$(arg rectify_rgb)" />
<remap from="rgb/image_rect_color" to="$(arg name)/$(arg color_sens_name)/image_raw"
unless="$(arg rectify_rgb)" />
<remap from="depth_registered/image_rect" to="$(arg name)/$(arg stereo_sens_name)/$(arg depth_topic_suffix)" />
<remap from="depth_registered/points" to="$(arg points_topic_name)" />
</node>

</launch>
20 changes: 3 additions & 17 deletions depthai_ros_driver/launch/rgbd_pcl.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,26 +35,12 @@
<arg name="cam_roll" value="$(arg cam_roll)" />
<arg name="cam_pitch" value="$(arg cam_pitch)" />
<arg name="cam_yaw" value="$(arg cam_yaw)" />
<arg name="rectify_rgb" value="$(arg rectify_rgb)" />
<arg name="pointcloud_enable" value="true" />
</include>



<node pkg="nodelet" type="nodelet" name="rectify_color"
args="load image_proc/rectify $(arg name)_nodelet_manager" if="$(arg rectify_rgb)">
<remap from="image_mono" to="$(arg name)/rgb/image_raw"/>
<remap from="image_rect" to="$(arg name)/rgb/image_rect"/>
</node>

<node pkg="nodelet" type="nodelet" name="depth_image_to_rgb_pointcloud"
args="load depth_image_proc/point_cloud_xyzrgb $(arg name)_nodelet_manager">
<param name="queue_size" value="10"/>

<remap from="rgb/camera_info" to="$(arg name)/rgb/camera_info"/>
<remap from="rgb/image_rect_color" to="$(arg name)/rgb/image_rect" if="$(arg rectify_rgb)"/>
<remap from="rgb/image_rect_color" to="$(arg name)/rgb/image_raw" unless="$(arg rectify_rgb)"/>
<remap from="depth_registered/image_rect" to="$(arg name)/stereo/image_raw"/>
<remap from="depth_registered/points" to="$(arg name)/points"/>
</node>


</launch>
</launch>
2 changes: 1 addition & 1 deletion depthai_ros_driver/src/dai_nodes/sensors/mono.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ void Mono::setupQueues(std::shared_ptr<dai::Device> device) {

utils::ImgPublisherConfig pubConf;
pubConf.daiNodeName = getName();
pubConf.topicName = "~/" + getName();
pubConf.topicName = getName();
pubConf.lazyPub = ph->getParam<bool>("i_enable_lazy_publisher");
pubConf.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
pubConf.calibrationFile = ph->getParam<std::string>("i_calibration_file");
Expand Down
2 changes: 1 addition & 1 deletion depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ void RGB::setupQueues(std::shared_ptr<dai::Device> device) {

utils::ImgPublisherConfig pubConfig;
pubConfig.daiNodeName = getName();
pubConfig.topicName = "~/" + getName();
pubConfig.topicName = getName();
pubConfig.lazyPub = ph->getParam<bool>("i_enable_lazy_publisher");
pubConfig.socket = static_cast<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
pubConfig.calibrationFile = ph->getParam<std::string>("i_calibration_file");
Expand Down
2 changes: 1 addition & 1 deletion depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,7 @@ const std::unordered_map<NodeNameEnum, std::string> NodeNameMap = {

bool rsCompabilityMode(ros::NodeHandle node) {
bool compat = false;
node.getParam("camera.i_rs_compat", compat);
node.getParam("camera_i_rs_compat", compat);
return compat;
}
std::string getNodeName(ros::NodeHandle node, NodeNameEnum name) {
Expand Down
7 changes: 7 additions & 0 deletions depthai_ros_driver/src/dai_nodes/sensors/sensor_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,13 @@ void SensorWrapper::link(dai::Node::Input in, int linkType) {
}
}

std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>> SensorWrapper::getPublishers() {
if(ph->getParam<bool>("i_disable_node")) {
return std::vector<std::shared_ptr<sensor_helpers::ImagePublisher>>();
}
return sensorNode->getPublishers();
}

void SensorWrapper::updateParams(parametersConfig& config) {
sensorNode->updateParams(config);
}
Expand Down
8 changes: 4 additions & 4 deletions depthai_ros_driver/src/dai_nodes/sensors/stereo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,7 @@ void Stereo::setupRectQueue(std::shared_ptr<dai::Device> device,
pubConfig.rectified = true;
pubConfig.width = ph->getOtherNodeParam<int>(sensorName, "i_width");
pubConfig.height = ph->getOtherNodeParam<int>(sensorName, "i_height");
pubConfig.topicName = "~/" + sensorName;
pubConfig.topicName = sensorName;
pubConfig.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw";
pubConfig.maxQSize = ph->getOtherNodeParam<int>(sensorName, "i_max_q_size");
pubConfig.socket = sensorInfo.socket;
Expand Down Expand Up @@ -191,7 +191,7 @@ void Stereo::setupStereoQueue(std::shared_ptr<dai::Device> device) {

utils::ImgPublisherConfig pubConf;
pubConf.daiNodeName = getName();
pubConf.topicName = "~/" + getName();
pubConf.topicName = getName();
pubConf.topicSuffix = rsCompabilityMode() ? "/image_rect_raw" : "/image_raw";
pubConf.rectified = !convConfig.alphaScalingEnabled;
pubConf.width = ph->getParam<int>("i_width");
Expand Down Expand Up @@ -224,10 +224,10 @@ void Stereo::setupQueues(std::shared_ptr<dai::Device> device) {
if(ph->getParam<bool>("i_publish_topic")) {
setupStereoQueue(device);
}
if(ph->getParam<bool>("i_publish_left_rect") || ph->getParam<bool>("i_publish_synced_rect_pair")) {
if(ph->getParam<bool>("i_left_rect_publish_topic") || ph->getParam<bool>("i_publish_synced_rect_pair")) {
setupLeftRectQueue(device);
}
if(ph->getParam<bool>("i_publish_right_rect") || ph->getParam<bool>("i_publish_synced_rect_pair")) {
if(ph->getParam<bool>("i_right_rect_publish_topic") || ph->getParam<bool>("i_publish_synced_rect_pair")) {
setupRightRectQueue(device);
}
if(ph->getParam<bool>("i_publish_synced_rect_pair")) {
Expand Down
2 changes: 2 additions & 0 deletions depthai_ros_driver/src/param_handlers/camera_param_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ void CameraParamHandler::declareParams() {
declareAndLogParam<std::string>("i_external_calibration_path", "");
declareAndLogParam("i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200));
declareAndLogParam("i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500));
declareAndLogParam<bool>("i_restart_on_diagnostics_error", false);
declareAndLogParam<bool>("i_rs_compat", false);
declareAndLogParam<bool>("i_restart_on_diagnostics_error", false);
declareAndLogParam<bool>("i_publish_tf_from_calibration", false);
declareAndLogParam<std::string>("i_tf_camera_name", "oak");
Expand Down
6 changes: 3 additions & 3 deletions depthai_ros_driver/src/param_handlers/stereo_param_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,8 @@ StereoParamHandler::StereoParamHandler(ros::NodeHandle node, const std::string&
}

void StereoParamHandler::updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right, dai::CameraBoardSocket& align) {
int newLeftS = getParam<int>("i_left_socket_id", static_cast<int>(left));
int newRightS = getParam<int>("i_right_socket_id", static_cast<int>(right));
int newLeftS = declareAndLogParam<int>("i_left_socket_id", static_cast<int>(left));
int newRightS = declareAndLogParam<int>("i_right_socket_id", static_cast<int>(right));
alignSocket = static_cast<dai::CameraBoardSocket>(declareAndLogParam<int>("i_board_socket_id", static_cast<int>(align)));
if(newLeftS != static_cast<int>(left) || newRightS != static_cast<int>(right)) {
ROS_WARN("Left or right socket changed, updating stereo node");
Expand Down Expand Up @@ -117,7 +117,7 @@ void StereoParamHandler::declareParams(std::shared_ptr<dai::node::StereoDepth> s
height = declareAndLogParam<int>("i_height", height);
stereo->setOutputSize(width, height);
stereo->setDefaultProfilePreset(depthPresetMap.at(declareAndLogParam<std::string>("i_depth_preset", "HIGH_ACCURACY")));
if(declareAndLogParam<bool>("i_enable_distortion_correction", false)) {
if(declareAndLogParam<bool>("i_enable_distortion_correction", true)) {
stereo->enableDistortionCorrection(true);
}
if(declareAndLogParam<bool>("i_set_disparity_to_depth_use_spec_translation", false)) {
Expand Down

0 comments on commit 92ba383

Please sign in to comment.