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

rtabmap: added odom_frame_id_init parameter

Browse Source
This commit is contained in:
matlabbe
2021-05-20 22:41:45 -04:00
parent 0d693bc1b6
commit f96591e030
2 changed files with 25 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/rtabmap.launch
+2
View File
@@ -37,6 +37,7 @@
<arg name="frame_id" default="camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published --> <arg name="frame_id" default="camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic --> <arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="odom_frame_id_init" default=""/> <!-- If set, TF map->odom is published even if no odometry topic has been received yet. The frame id should match the one in the topic. -->
<arg name="map_frame_id" default="map"/> <arg name="map_frame_id" default="map"/>
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" --> <arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) --> <arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
@@ -310,6 +311,7 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/> <param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/> <param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="odom_frame_id_init" type="string" value="$(arg odom_frame_id_init)"/>
<param name="publish_tf" type="bool" value="$(arg publish_tf_map)"/> <param name="publish_tf" type="bool" value="$(arg publish_tf_map)"/>
<param name="gen_scan" type="bool" value="$(arg gen_scan)"/> <param name="gen_scan" type="bool" value="$(arg gen_scan)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/> <param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
src/CoreWrapper.cpp
+23
View File
@@ -142,12 +142,14 @@ void CoreWrapper::onInit()
bool publishTf = true; bool publishTf = true;
double tfDelay = 0.05; // 20 Hz double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms double tfTolerance = 0.1; // 100 ms
std::string odomFrameIdInit;
pnh.param("config_path", configPath_, configPath_); pnh.param("config_path", configPath_, configPath_);
pnh.param("database_path", databasePath_, databasePath_); pnh.param("database_path", databasePath_, databasePath_);
pnh.param("frame_id", frameId_, frameId_); pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
pnh.param("odom_frame_id_init", odomFrameIdInit, odomFrameIdInit); // set to publish map->odom TF before receiving odom topic
pnh.param("map_frame_id", mapFrameId_, mapFrameId_); pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_); pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_);
@@ -157,6 +159,18 @@ void CoreWrapper::onInit()
"anymore! It is replaced by \"rgbd_cameras\" parameter " "anymore! It is replaced by \"rgbd_cameras\" parameter "
"used when \"subscribe_rgbd\" is true"); "used when \"subscribe_rgbd\" is true");
} }
if(!odomFrameIdInit.empty())
{
if(odomFrameId_.empty())
{
ROS_INFO("rtabmap: odom_frame_id_init = %s", odomFrameIdInit.c_str());
odomFrameId_ = odomFrameIdInit;
}
else
{
ROS_WARN("odom_frame_id_init (%s) is ignored if odom_frame_id (%s) is set.", odomFrameIdInit.c_str(), odomFrameId_.c_str());
}
}
pnh.param("publish_tf", publishTf, publishTf); pnh.param("publish_tf", publishTf, publishTf);
pnh.param("tf_delay", tfDelay, tfDelay); pnh.param("tf_delay", tfDelay, tfDelay);
@@ -2105,6 +2119,15 @@ void CoreWrapper::process(
timeRtabmap = timer.ticks(); timeRtabmap = timer.ticks();
mapToOdomMutex_.lock(); mapToOdomMutex_.lock();
mapToOdom_ = rtabmap_.getMapCorrection(); mapToOdom_ = rtabmap_.getMapCorrection();
if(!odomFrameId.empty() && !odomFrameId_.empty() && odomFrameId_.compare(odomFrameId)!=0)
{
ROS_ERROR("Odometry received doesn't have same frame_id "
"than the one previously set (old=%s, new=%s). "
"Are there multiple nodes publishing on same odometry topic name? "
"The new frame_id is now used.", odomFrameId_.c_str(), odomFrameId.c_str());
}
odomFrameId_ = odomFrameId; odomFrameId_ = odomFrameId;
mapToOdomMutex_.unlock(); mapToOdomMutex_.unlock();