i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-13 06:40:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Backporting ros2 commit 0ad034680b

Browse Source
This commit is contained in:
matlabbe
2025-09-14 23:44:46 +00:00
parent e355deca43
commit 3fa20f56fb
4 changed files with 80 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_launch/launch/rtabmap.launch
+4
View File
@@ -127,6 +127,7 @@
<arg name="odom_guess_min_rotation" default="0"/>
<arg name="odom_max_rate" default="0"/>
<arg name="odom_expected_rate" default="0"/>
<arg name="odom_always_process_most_recent_frame" default="true"/>
<arg name="imu_topic" default="/imu/data"/> <!-- only used with VIO approaches -->
<arg name="wait_imu_to_init" default="false"/>
<arg name="use_odom_features" default="false"/>
@@ -264,6 +265,7 @@
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="always_process_most_recent_frame" type="bool" value="$(arg odom_always_process_most_recent_frame)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="keep_color" type="bool" value="$(arg use_odom_features)"/>
</node>
@@ -295,6 +297,7 @@
<param name="guess_min_translation" type="double" value="$(arg odom_guess_min_translation)"/>
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="always_process_most_recent_frame" type="bool" value="$(arg odom_always_process_most_recent_frame)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="keep_color" type="bool" value="$(arg use_odom_features)"/>
</node>
@@ -323,6 +326,7 @@
<param name="guess_min_rotation" type="double" value="$(arg odom_guess_min_rotation)"/>
<param name="scan_cloud_max_points" type="int" value="$(arg scan_cloud_max_points)"/>
<param name="expected_update_rate" type="double" value="$(arg odom_expected_rate)"/>
<param name="always_process_most_recent_frame" type="bool" value="$(arg odom_always_process_most_recent_frame)"/>
<param name="max_update_rate" type="double" value="$(arg odom_max_rate)"/>
<param name="deskewing" type="bool" value="$(arg scan_deskewing)"/>
<param name="deskewing_slerp" type="bool" value="$(arg scan_deskewing_slerp)"/>
rtabmap_odom/include/rtabmap_odom/OdometryROS.h
+4
View File
@@ -95,6 +95,7 @@ private:
virtual void onOdomInit() = 0;
virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
void processData();
virtual void mainLoop();
virtual void mainLoopKill();
@@ -160,9 +161,12 @@ private:
rtabmap::Transform guessPreviousPose_;
ros::Time previousStamp_;
ros::Time previousClockTime_;
double lastReceivedTopicClock_;
double lastReceivedTopicStamp_;
double expectedUpdateRate_;
double maxUpdateRate_;
double minUpdateRate_;
bool alwaysProcessMostRecentFrame_;
std::string compressionImgFormat_;
bool compressionParallelized_;
int odomStrategy_;
rtabmap_odom/src/OdometryROS.cpp
+59 -12
View File
@@ -78,9 +78,12 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
stereoParams_(stereoParams),
visParams_(visParams),
icpParams_(icpParams),
lastReceivedTopicClock_(0.0),
lastReceivedTopicStamp_(0.0),
expectedUpdateRate_(0.0),
maxUpdateRate_(0.0),
minUpdateRate_(0.0),
alwaysProcessMostRecentFrame_(true),
compressionImgFormat_(".jpg"),
compressionParallelized_(true),
odomStrategy_(Parameters::defaultOdomStrategy()),
@@ -148,6 +151,7 @@ void OdometryROS::onInit()
pnh.param("expected_update_rate", expectedUpdateRate_, expectedUpdateRate_); // expected sensor rate
pnh.param("max_update_rate", maxUpdateRate_, maxUpdateRate_);
pnh.param("min_update_rate", minUpdateRate_, minUpdateRate_);
pnh.param("always_process_most_recent_frame", alwaysProcessMostRecentFrame_, alwaysProcessMostRecentFrame_);
pnh.param("sensor_data_compression_format", compressionImgFormat_, compressionImgFormat_);
pnh.param("sensor_data_parallel_compression", compressionParallelized_, compressionParallelized_);
@@ -373,10 +377,11 @@ void OdometryROS::onInit()
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_)
{
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
imuSub_ = nh.subscribe("imu", queueSize*5, &OdometryROS::callbackIMU, this);
NODELET_INFO("odometry: Subscribing to IMU topic %s", imuSub_.getTopic().c_str());
int queueSize = 50;
pnh.param("imu_queue_size", queueSize, queueSize);
imuSub_ = nh.subscribe("imu", queueSize, &OdometryROS::callbackIMU, this);
NODELET_INFO("odometry: Subscribing to IMU topic %s (imu_queue_size=%d)",
imuSub_.getTopic().c_str(), queueSize);
}
this->start();
@@ -460,22 +465,43 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
void OdometryROS::processData(SensorData & data, const std_msgs::Header & header)
{
//NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec());
double clockNow = ros::Time::now().toSec();
if(dataMutex_.lockTry() == 0)
{
if(bufferedDataToProcess_) {
NODELET_ERROR("We didn't receive IMU newer than previous image (%f) and we just received a new image (%f). The previous image is dropped!",
NODELET_ERROR("We didn't receive IMU newer than previous image/scan (%f) and we just received a new image/scan (%f). The previous image/scan is dropped! Make sure IMU is published faster and with less delay than the image/scan.",
dataHeaderToProcess_.stamp.toSec(), header.stamp.toSec());
}
dataToProcess_ = data;
dataHeaderToProcess_ = header;
bufferedDataToProcess_ = false;
dataReady_.release();
if(alwaysProcessMostRecentFrame_) {
dataReady_.release();
}
dataMutex_.unlock();
if(!alwaysProcessMostRecentFrame_) {
processData();
}
}
else
{
NODELET_DEBUG("Dropping image/scan data");
double estimatedPeriod = clockNow - lastReceivedTopicClock_;
double topicPeriod = header.stamp.toSec() - lastReceivedTopicStamp_;
if(estimatedPeriod>0.0 && topicPeriod>0.0 && estimatedPeriod < topicPeriod*0.9) {
NODELET_WARN("Dropping image/scan data with stamp %f (delay=%f). Something is wrong "
"because the clock difference with the previous topic received (%fs) is much lower than the "
"expected one (%fs) estimated from the topic stamps (previous stamp=%f). If you are processing "
"a large bag with flaky replaying delay, consider setting parameter \"always_process_most_recent_frame:=false\" "
"to avoid aggressively dropping data.",
header.stamp.toSec(),
clockNow - header.stamp.toSec(),
estimatedPeriod,
topicPeriod,
lastReceivedTopicStamp_);
}
}
lastReceivedTopicStamp_ = header.stamp.toSec();
lastReceivedTopicClock_ = clockNow;
}
void OdometryROS::mainLoopKill()
@@ -493,7 +519,11 @@ void OdometryROS::mainLoop()
// thread killed
return;
}
processData();
}
void OdometryROS::processData()
{
UScopeMutex lock(dataMutex_);
// aliases
@@ -512,21 +542,36 @@ void OdometryROS::mainLoop()
if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < header.stamp.toSec()))
{
NODELET_WARN("Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f). Buffering the image until an imu with same or greater stamp is received.",
data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
if(imus_.empty()) {
// If empty, it is an error!
NODELET_ERROR("Make sure IMU is published faster than data rate! (last image/scan stamp=%f and imu buffer is empty). Buffering the image/scan until an imu with same or greater stamp is received.",
data.stamp());
}
bufferedDataToProcess_ = true;
return;
}
// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(header.stamp.toSec());
std::map<double, rtabmap::IMU>::iterator iterLast = iterEnd;
if(iterEnd!= imus_.end())
{
++iterEnd;
}
for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
std::map<double, rtabmap::IMU>::iterator iterFirst = imus_.begin();
for(std::map<double, rtabmap::IMU>::iterator iter=iterFirst; iter!=iterEnd;)
{
imus.push_back(*iter);
imus_.erase(iter++);
// Because we always keep the last processed imu in the buffer, skip the first
// one when processing again the buffer unless its time is lower/equal to image
// current stamp (could happen on initialization).
if(iter!=iterFirst || iter->first <= header.stamp.toSec()) {
imus.push_back(*iter);
}
if(iter!=iterLast) {
imus_.erase(iter++);
}
else {
++iter;
}
}
}
@@ -1193,6 +1238,8 @@ void OdometryROS::reset(const Transform & pose)
guessPreviousPose_.setNull();
previousStamp_ = ros::Time();
previousClockTime_ = ros::Time();
lastReceivedTopicClock_ = 0.0;
lastReceivedTopicStamp_ = 0.0;
resetCurrentCount_ = resetCountdown_;
imuProcessed_ = false;
dataToProcess_ = SensorData();
rtabmap_odom/src/nodelets/icp_odometry.cpp
+13 -1
View File
@@ -91,7 +91,19 @@ private:
ros::NodeHandle & pnh = getPrivateNodeHandle();
int queueSize = 1;
pnh.param("queue_size", queueSize, queueSize);
pnh.param("topic_queue_size", queueSize, queueSize);
if(pnh.hasParam("queue_size") && !pnh.hasParam("topic_queue_size"))
{
pnh.param("queue_size", queueSize, queueSize);
ROS_WARN("Parameter \"queue_size\" has been renamed "
"to \"topic_queue_size\" and will be removed "
"in future versions! The value (%d) is still copied to "
"\"topic_queue_size\".", queueSize);
}
else
{
pnh.param("topic_queue_size", queueSize, queueSize);
}
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_cloud_is_2d", scanCloudIs2d_, scanCloudIs2d_);
pnh.param("scan_downsampling_step", scanDownsamplingStep_, scanDownsamplingStep_);