Skip to content

Commit

Permalink
Updated output topic name
Browse files Browse the repository at this point in the history
  • Loading branch information
@matlabbe
matlabbe committed Oct 20, 2023
1 parent f81dbe7 commit 0de2200
Show file tree
Hide file tree
Showing 3 changed files with 44 additions and 36 deletions.
31 changes: 15 additions & 16 deletions rtabmap_examples/launch/test_sensor_data.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@

<launch>

<arg name="pub_sensor_data_image" default="true"/>
<arg name="features_only" default="false"/>
<arg name="compression_format" default=".jpg"/> <!-- ".jpg" or ".png" -->
<arg name="parallel_compression" default="true"/>

Expand Down Expand Up @@ -98,9 +98,8 @@
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="pub_sensor_data_image" value="$(arg pub_sensor_data_image)"/>
<param name="pub_sensor_data_image_compression_format" value="$(arg compression_format)"/>
<param name="pub_sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_odom/rgbd_odometry manager $(arg odom_args)">
Expand All @@ -110,17 +109,17 @@
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="pub_sensor_data_image" value="$(arg pub_sensor_data_image)"/>
<param name="pub_sensor_data_image_compression_format" value="$(arg compression_format)"/>
<param name="pub_sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>

<!-- RTAB-Map -->
<node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_slam/rtabmap manager $(arg rtabmap_args)">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_sensor_data" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="sensor_data" to="odom_sensor_data"/>
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
</node>
</group>

Expand Down Expand Up @@ -152,9 +151,8 @@
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="pub_sensor_data_image" value="$(arg pub_sensor_data_image)"/>
<param name="pub_sensor_data_image_compression_format" value="$(arg compression_format)"/>
<param name="pub_sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>
<!-- RGB-D Odometry -->
<node unless="$(arg stereo)" pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" args="$(arg odom_args)" output="screen">
Expand All @@ -164,17 +162,17 @@
<param name="keep_color" type="bool" value="true"/>
<param name="wait_imu_to_init" type="bool" value="$(arg wait_imu_to_init)"/>
<remap from="imu" to="$(arg imu_topic)"/>
<param name="pub_sensor_data_image" value="$(arg pub_sensor_data_image)"/>
<param name="pub_sensor_data_image_compression_format" value="$(arg compression_format)"/>
<param name="pub_sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
<param name="sensor_data_compression_format" value="$(arg compression_format)"/>
<param name="sensor_data_parallel_compression" value="$(arg parallel_compression)"/>
</node>

<!-- RTAB-Map -->
<node pkg="rtabmap_slam" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="subscribe_sensor_data" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="sensor_data" to="odom_sensor_data"/>
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
</node>
</group>

Expand All @@ -184,7 +182,8 @@
<param name="subscribe_sensor_data" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="false"/>
<remap from="sensor_data" to="odom_sensor_data"/>
<remap if="$(arg features_only)" from="sensor_data" to="odom_sensor_data/features"/>
<remap unless="$(arg features_only)" from="sensor_data" to="odom_sensor_data/raw"/>
</node>

</group>
Expand Down
2 changes: 1 addition & 1 deletion rtabmap_odom/include/rtabmap_odom/OdometryROS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,7 @@ class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
ros::Publisher odomLastFrame_;
ros::Publisher odomRgbdImagePub_;
ros::Publisher odomSensorDataPub_;
ros::Publisher odomSensorDataFeaturesPub_;
ros::Publisher odomSensorDataCompressedPub_;
ros::ServiceServer resetSrv_;
ros::ServiceServer resetToPoseSrv_;
Expand All @@ -149,7 +150,6 @@ class OdometryROS : public nodelet::Nodelet, public rtabmap_sync::SyncDiagnostic
double expectedUpdateRate_;
double maxUpdateRate_;
double minUpdateRate_;
bool pubSensorDataImage_;
std::string compressionImgFormat_;
bool compressionParallelized_;
int odomStrategy_;
Expand Down
47 changes: 28 additions & 19 deletions rtabmap_odom/src/OdometryROS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,6 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
expectedUpdateRate_(0.0),
maxUpdateRate_(0.0),
minUpdateRate_(0.0),
pubSensorDataImage_(true),
compressionImgFormat_(".jpg"),
compressionParallelized_(true),
odomStrategy_(Parameters::defaultOdomStrategy()),
Expand All @@ -110,8 +109,9 @@ void OdometryROS::onInit()
odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
odomRgbdImagePub_ = nh.advertise<rtabmap_msgs::RGBDImage>("odom_rgbd_image", 1);
odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data", 1);
odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data_compressed", 1);
odomSensorDataPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/raw", 1);
odomSensorDataFeaturesPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/features", 1);
odomSensorDataCompressedPub_ = nh.advertise<rtabmap_msgs::SensorData>("odom_sensor_data/compressed", 1);

Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
Expand Down Expand Up @@ -150,9 +150,8 @@ void OdometryROS::onInit()
pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);

pnh.param("pub_sensor_data_image", pubSensorDataImage_, pubSensorDataImage_);
pnh.param("pub_sensor_data_image_compression_format", compressionImgFormat_, compressionImgFormat_);
pnh.param("pub_sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);
pnh.param("sensor_data_compression_format", compressionImgFormat_, compressionImgFormat_);
pnh.param("sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);

pnh.param("wait_imu_to_init", waitIMUToinit_, waitIMUToinit_);

Expand Down Expand Up @@ -188,9 +187,8 @@ void OdometryROS::onInit()
NODELET_INFO("Odometry: max_update_rate = %f Hz", maxUpdateRate_);
NODELET_INFO("Odometry: min_update_rate = %f Hz", minUpdateRate_);
NODELET_INFO("Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
NODELET_INFO("Odometry: pub_sensor_data_image = %s", pubSensorDataImage_?"true":"false");
NODELET_INFO("Odometry: pub_sensor_data_image_compression_format = %s", compressionImgFormat_.c_str());
NODELET_INFO("Odometry: pub_sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
NODELET_INFO("Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str());
NODELET_INFO("Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");

configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
Expand Down Expand Up @@ -936,12 +934,26 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header

if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
{
if(odomSensorDataPub_.getNumSubscribers())
if(odomSensorDataPub_.getNumSubscribers() || odomSensorDataFeaturesPub_.getNumSubscribers())
{
rtabmap_msgs::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, pubSensorDataImage_);
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_.getNumSubscribers());
msg.header.stamp = header.stamp; // use corresponding time stamp to image
odomSensorDataPub_.publish(msg);
if(odomSensorDataPub_.getNumSubscribers())
{
odomSensorDataPub_.publish(msg);
}
if(odomSensorDataFeaturesPub_.getNumSubscribers())
{
// remove data
msg.left = sensor_msgs::Image();
msg.right = sensor_msgs::Image();
msg.laser_scan = sensor_msgs::PointCloud2();
msg.grid_ground.clear();
msg.grid_obstacles.clear();
msg.grid_empty_cells.clear();
odomSensorDataFeaturesPub_.publish(msg);
}
}
if(odomSensorDataCompressedPub_.getNumSubscribers())
{
Expand All @@ -953,11 +965,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_);
rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data());
if(pubSensorDataImage_ && !data.imageRaw().empty())
if(!data.imageRaw().empty())
{
ctImage.start();
}
if(pubSensorDataImage_ && !data.depthOrRightRaw().empty())
if(!data.depthOrRightRaw().empty())
{
ctDepth.start();
}
Expand All @@ -975,11 +987,8 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
}
else
{
if(pubSensorDataImage_)
{
compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
}
compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
compressedScan = compressData2(data.laserScanRaw().data());
}
if(!compressedImage.empty() && !data.stereoCameraModels().empty())
Expand Down

0 comments on commit 0de2200

Please sign in to comment.