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

Odometry: added ground_truth_base_frame_id parameter, added in rtabmap.launch

Browse Source
This commit is contained in:
matlabbe
2017-05-17 15:40:29 -04:00
parent 797571cd26
commit 4d67004016
2 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/rtabmap.launch
+4
View File
@@ -105,6 +105,8 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
@@ -125,6 +127,8 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
src/OdometryROS.cpp
+4 -2
View File
@@ -63,6 +63,7 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
groundTruthBaseFrameId_(""),
guessFrameId_(""),
publishTf_(true),
waitForTransform_(true),
@@ -124,6 +125,7 @@ void OdometryROS::onInit()
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("ground_truth_base_frame_id", groundTruthBaseFrameId_, frameId_);
pnh.param("config_path", configPath, configPath);
pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_);
pnh.param("guess_from_tf", guessFromTf_, guessFromTf_);
@@ -351,7 +353,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
!groundTruthFrameId_.empty())
{
// sync with the first value of the ground truth
Transform initialPose = getTransform(groundTruthFrameId_, frameId_, stamp);
Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(initialPose.isNull())
{
return;
@@ -360,7 +362,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
frameId_.c_str());
groundTruthBaseFrameId_.c_str());
odometry_->reset(initialPose);
}