i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 12:09:51 +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
launch/rtabmap.launch
+2
View File
@@ -26,6 +26,7 @@
<!-- Localization-only mode --> <!-- Localization-only mode -->
<arg name="localization" default="false"/> <arg name="localization" default="false"/>
<arg name="initial_pose" default=""/> <!-- Format: "x y z roll pitch yaw" or "x y z qx qy qz qw". Default: see "rtabmap --params | grep RGBD/StartAtOrigin" -->
<!-- sim time for convenience, if playing a rosbag --> <!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/> <arg name="use_sim_time" default="false"/>
@@ -362,6 +363,7 @@
<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="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="initial_pose" type="string" value="$(arg initial_pose)"/>
<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)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/> <param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
src/CoreWrapper.cpp
+18
View File
@@ -141,6 +141,7 @@ void CoreWrapper::onInit()
mapsManager_.init(nh, pnh, getName(), true); mapsManager_.init(nh, pnh, getName(), true);
bool publishTf = true; bool publishTf = true;
std::string initialPoseStr;
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; std::string odomFrameIdInit;
@@ -186,6 +187,7 @@ void CoreWrapper::onInit()
pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_); pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); 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_action_for_goal", useActionForGoal_, useActionForGoal_);
pnh.param("use_saved_map", useSavedMap_, useSavedMap_); pnh.param("use_saved_map", useSavedMap_, useSavedMap_);
pnh.param("gen_scan", genScan_, genScan_); pnh.param("gen_scan", genScan_, genScan_);
@@ -228,6 +230,7 @@ void CoreWrapper::onInit()
groundTruthBaseFrameId_.c_str()); groundTruthBaseFrameId_.c_str());
} }
NODELET_INFO("rtabmap: map_frame_id = %s", mapFrameId_.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: use_action_for_goal = %s", useActionForGoal_?"true":"false");
NODELET_INFO("rtabmap: tf_delay = %f", tfDelay); NODELET_INFO("rtabmap: tf_delay = %f", tfDelay);
NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance); 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 // set private parameters
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter) for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{ {