diff --git a/.devcontainer/humble/devcontainer.json b/.devcontainer/humble/devcontainer.json index c673d85d..66de040e 100644 --- a/.devcontainer/humble/devcontainer.json +++ b/.devcontainer/humble/devcontainer.json @@ -7,5 +7,6 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind", "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"] } diff --git a/.devcontainer/jazzy/devcontainer.json b/.devcontainer/jazzy/devcontainer.json index f6f7e254..fcdec049 100644 --- a/.devcontainer/jazzy/devcontainer.json +++ b/.devcontainer/jazzy/devcontainer.json @@ -7,5 +7,6 @@ }, "workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind", "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"] } diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 9c465599..25294bd7 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -2593,6 +2593,24 @@ bool convertScanMsg( double waitForTransform, 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 rtabmap::Transform tmpT = getMovingTransform( scan2dMsg.header.frame_id, diff --git a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py index e7294af6..3b6c79f6 100644 --- a/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py +++ b/rtabmap_demos/launch/turtlebot4_ignition_demo.launch.py @@ -4,7 +4,7 @@ # # Example: # 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. # @@ -77,4 +77,4 @@ def generate_launch_description(): 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(ignition) - return ld \ No newline at end of file + return ld diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 201fff0b..8605cf38 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -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; - 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); - } + odomSensorDataPub_->publish(msg); } - if(odomSensorDataCompressedPub_->get_subscription_count()>0) + if(odomSensorDataFeaturesPub_->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); + // 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(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(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(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(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(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(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(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(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(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(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(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(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at(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(