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

rtabmap node: Added initial_pose parameter

Browse Source
This commit is contained in:
matlabbe
2022-11-06 10:07:21 -08:00
parent 276a5f4ac2
commit 9c82e2ac1a
2 changed files with 20 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+18
View File
@@ -141,6 +141,7 @@ void CoreWrapper::onInit()
mapsManager_.init(nh, pnh, getName(), true);
bool publishTf = true;
std::string initialPoseStr;
double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms
std::string odomFrameIdInit;
@@ -186,6 +187,7 @@ void CoreWrapper::onInit()
pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
pnh.param("gen_scan", genScan_, genScan_);
@@ -228,6 +230,7 @@ void CoreWrapper::onInit()
groundTruthBaseFrameId_.c_str());
}
NODELET_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
NODELET_INFO("rtabmap: initial_pose = %s", initialPoseStr.c_str());
NODELET_INFO("rtabmap: use_action_for_goal = %s", useActionForGoal_?"true":"false");
NODELET_INFO("rtabmap: tf_delay = %f", tfDelay);
NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
@@ -803,6 +806,21 @@ void CoreWrapper::onInit()
}
}
// Set initial pose if set
if(!initialPoseStr.empty())
{
Transform intialPose = Transform::fromString(initialPoseStr);
if(!intialPose.isNull())
{
NODELET_INFO("Setting initial pose: \"%s\"", intialPose.prettyPrint().c_str());
rtabmap_.setInitialPose(intialPose);
}
else
{
NODELET_ERROR("Invalid initial_pose: \"%s\"", initialPoseStr.c_str());
}
}
// set private parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{