i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master -> ros2

Browse Source
This commit is contained in:
matlabbe
2024-07-21 16:29:49 -07:00
parent 57dd787a58 a97efff760
commit 3aacddb3d6
5 changed files with 133 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.devcontainer/humble/devcontainer.json
+2 -1
View File
@@ -7,5 +7,6 @@
}, },
"workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind", "workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
"workspaceFolder": "/ros2_ws", "workspaceFolder": "/ros2_ws",
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/humble/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'" "postAttachCommand": "echo 'Initialize colcon: source /opt/ros/humble/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'",
"runArgs": ["--privileged"]
} }
.devcontainer/jazzy/devcontainer.json
+2 -1
View File
@@ -7,5 +7,6 @@
}, },
"workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind", "workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
"workspaceFolder": "/ros2_ws", "workspaceFolder": "/ros2_ws",
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'" "postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'",
"runArgs": ["--privileged"]
} }
rtabmap_conversions/src/MsgConversion.cpp
+18
View File
@@ -2593,6 +2593,24 @@ bool convertScanMsg(
double waitForTransform, double waitForTransform,
bool outputInFrameId) bool outputInFrameId)
{ {
// scan message validation check
if(scan2dMsg.angle_increment == 0.0f) {
UERROR("convertScanMsg: angle_increment should not be 0!");
return false;
}
if(scan2dMsg.range_min > scan2dMsg.range_max) {
UERROR("convertScanMsg: range_min (%f) should be smaller than range_max (%f)!", scan2dMsg.range_min, scan2dMsg.range_max);
return false;
}
if(scan2dMsg.angle_increment > 0 && scan2dMsg.angle_max < scan2dMsg.angle_min) {
UERROR("convertScanMsg: Angle increment (%f) should be negative if angle_min(%f) > angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max);
return false;
}
else if (scan2dMsg.angle_increment < 0 && scan2dMsg.angle_max > scan2dMsg.angle_min) {
UERROR("convertScanMsg: Angle increment (%f) should positive if angle_min(%f) < angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max);
return false;
}
// make sure the frame of the laser is updated during the whole scan time // make sure the frame of the laser is updated during the whole scan time
rtabmap::Transform tmpT = getMovingTransform( rtabmap::Transform tmpT = getMovingTransform(
scan2dMsg.header.frame_id, scan2dMsg.header.frame_id,
rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py
+2 -2
View File
@@ -4,7 +4,7 @@
# #
# Example: # Example:
# 1) Launch simulator (turtlebot4, nav2 and rtabmap): # 1) Launch simulator (turtlebot4, nav2 and rtabmap):
# $ ros2 launch rtabmap_demos turtlebot4_ignition.launch.py # $ ros2 launch rtabmap_demos turtlebot4_ignition_demo.launch.py
# #
# 2) Click on "Play" button on bottom-left of gazebo. # 2) Click on "Play" button on bottom-left of gazebo.
# #
@@ -77,4 +77,4 @@ def generate_launch_description():
ld = LaunchDescription(ARGUMENTS) ld = LaunchDescription(ARGUMENTS)
ld.add_action(rtabmap) # put it first so that localization arg is not overwritten by the same used by ignition ld.add_action(rtabmap) # put it first so that localization arg is not overwritten by the same used by ignition
ld.add_action(ignition) ld.add_action(ignition)
return ld return ld
rtabmap_odom/src/OdometryROS.cpp
+109 -112
View File
@@ -929,124 +929,121 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
} }
} }
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()) if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0)
{ {
if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0) rtabmap_msgs::msg::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0);
msg.header.stamp = header.stamp; // use corresponding time stamp to image
if(odomSensorDataPub_->get_subscription_count()>0)
{ {
rtabmap_msgs::msg::SensorData msg; odomSensorDataPub_->publish(msg);
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0);
msg.header.stamp = header.stamp; // use corresponding time stamp to image
if(odomSensorDataPub_->get_subscription_count()>0)
{
odomSensorDataPub_->publish(msg);
}
if(odomSensorDataFeaturesPub_->get_subscription_count()>0)
{
// remove data
msg.left = sensor_msgs::msg::Image();
msg.right = sensor_msgs::msg::Image();
msg.laser_scan = sensor_msgs::msg::PointCloud2();
msg.grid_ground.clear();
msg.grid_obstacles.clear();
msg.grid_empty_cells.clear();
odomSensorDataFeaturesPub_->publish(msg);
}
} }
if(odomSensorDataCompressedPub_->get_subscription_count()>0) if(odomSensorDataFeaturesPub_->get_subscription_count()>0)
{ {
cv::Mat compressedImage; // remove data
cv::Mat compressedDepth; msg.left = sensor_msgs::msg::Image();
cv::Mat compressedScan; msg.right = sensor_msgs::msg::Image();
if(compressionParallelized_) msg.laser_scan = sensor_msgs::msg::PointCloud2();
{ msg.grid_ground.clear();
rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_); msg.grid_obstacles.clear();
rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_); msg.grid_empty_cells.clear();
rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data()); odomSensorDataFeaturesPub_->publish(msg);
if(!data.imageRaw().empty())
{
ctImage.start();
}
if(!data.depthOrRightRaw().empty())
{
ctDepth.start();
}
if(!data.laserScanRaw().isEmpty())
{
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
compressedImage = ctImage.getCompressedData();
compressedDepth = ctDepth.getCompressedData();
compressedScan = ctLaserScan.getCompressedData();
}
else
{
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())
{
data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false);
}
else if(!compressedImage.empty() && !data.cameraModels().empty())
{
data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false);
}
if(!compressedScan.empty())
{
data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f?
LaserScan(compressedScan,
data.laserScanRaw().maxPoints(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().format(),
data.laserScanRaw().localTransform()):
LaserScan(compressedScan,
data.laserScanRaw().format(),
data.laserScanRaw().rangeMin(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().angleMin(),
data.laserScanRaw().angleMax(),
data.laserScanRaw().angleIncrement(),
data.laserScanRaw().localTransform()), false);
}
rtabmap_msgs::msg::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false);
msg.header.stamp = header.stamp; // use corresponding time stamp to image
odomSensorDataCompressedPub_->publish(msg);
} }
if(visParams_)
{
if(icpParams_)
{
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}
else
{
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}
}
else // if(icpParams_)
{
RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}
statusDiagnostic_.setStatus(pose.isNull());
if(syncDiagnostic_.get() && !pose.isNull())
{
double curentRate = 1.0/(this->now()-timeStart).seconds();
syncDiagnostic_->tick(header.stamp,
maxUpdateRate_>0 ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
}
previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
} }
if(odomSensorDataCompressedPub_->get_subscription_count()>0)
{
cv::Mat compressedImage;
cv::Mat compressedDepth;
cv::Mat compressedScan;
if(compressionParallelized_)
{
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(!data.imageRaw().empty())
{
ctImage.start();
}
if(!data.depthOrRightRaw().empty())
{
ctDepth.start();
}
if(!data.laserScanRaw().isEmpty())
{
ctLaserScan.start();
}
ctImage.join();
ctDepth.join();
ctLaserScan.join();
compressedImage = ctImage.getCompressedData();
compressedDepth = ctDepth.getCompressedData();
compressedScan = ctLaserScan.getCompressedData();
}
else
{
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())
{
data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false);
}
else if(!compressedImage.empty() && !data.cameraModels().empty())
{
data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false);
}
if(!compressedScan.empty())
{
data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f?
LaserScan(compressedScan,
data.laserScanRaw().maxPoints(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().format(),
data.laserScanRaw().localTransform()):
LaserScan(compressedScan,
data.laserScanRaw().format(),
data.laserScanRaw().rangeMin(),
data.laserScanRaw().rangeMax(),
data.laserScanRaw().angleMin(),
data.laserScanRaw().angleMax(),
data.laserScanRaw().angleIncrement(),
data.laserScanRaw().localTransform()), false);
}
rtabmap_msgs::msg::SensorData msg;
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false);
msg.header.stamp = header.stamp; // use corresponding time stamp to image
odomSensorDataCompressedPub_->publish(msg);
}
if(visParams_)
{
if(icpParams_)
{
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}
else
{
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}
}
else // if(icpParams_)
{
RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
}
statusDiagnostic_.setStatus(pose.isNull());
if(syncDiagnostic_.get())
{
double curentRate = 1.0/(this->now()-timeStart).seconds();
syncDiagnostic_->tick(header.stamp,
maxUpdateRate_>0 ? maxUpdateRate_:
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
}
previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
} }
void OdometryROS::resetOdom( void OdometryROS::resetOdom(