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

Added "odom_tf_angular_variance" and "odom_tf_linear_variance" parameters for rtabmap node when TF is used for odometry

Browse Source
This commit is contained in:
matlabbe
2017-02-08 15:16:29 -05:00
parent 938bb985f3
commit d5144f292f
3 changed files with 29 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+2
View File
@@ -186,6 +186,8 @@ private:
std::string groundTruthBaseFrameId_;
std::string configPath_;
std::string databasePath_;
float odomDefaultAngVariance_;
float odomDefaultLinVariance_;
bool waitForTransform_;
double waitForTransformDuration_;
bool useActionForGoal_;
launch/rtabmap.launch
+5
View File
@@ -66,8 +66,11 @@
<arg name="visual_odometry" default="true"/> <!-- Launch rtabmap visual odometry node -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="odom_frame_id" default=""/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="odom_tf_angular_variance" default="1"/> <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance" default="1"/> <!-- If TF is used to get odometry, this is the default linear variance -->
<arg name="odom_args" default="$(arg rtabmap_args)"/>
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
@@ -128,6 +131,8 @@
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
<param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
src/CoreWrapper.cpp
+18 -4
View File
@@ -93,6 +93,8 @@ CoreWrapper::CoreWrapper() :
groundTruthBaseFrameId_(""), // e.g., "base_link_gt"
configPath_(""),
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
odomDefaultAngVariance_(1.0),
odomDefaultLinVariance_(1.0),
waitForTransform_(true),
waitForTransformDuration_(0.2), // 200 ms
useActionForGoal_(false),
@@ -145,6 +147,8 @@ void CoreWrapper::onInit()
pnh.param("tf_delay", tfDelay, tfDelay);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("tf_tolerance", tfTolerance, tfTolerance);
pnh.param("odom_tf_angular_variance", odomDefaultAngVariance_, odomDefaultAngVariance_);
pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
@@ -951,8 +955,8 @@ void CoreWrapper::commonDepthCallbackImpl(
data,
lastPose_,
odomFrameId,
uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0);
rotVariance_,
transVariance_);
rotVariance_ = 0;
transVariance_ = 0;
}
@@ -1137,8 +1141,8 @@ void CoreWrapper::commonStereoCallback(
data,
lastPose_,
odomFrameId,
uIsFinite(rotVariance_) && rotVariance_>0?rotVariance_:1.0,
uIsFinite(transVariance_) && transVariance_>0?transVariance_:1.0);
rotVariance_,
transVariance_);
rotVariance_ = 0;
transVariance_ = 0;
@@ -1158,6 +1162,16 @@ void CoreWrapper::process(
double timeRtabmap = 0.0;
double timeUpdateMaps = 0.0;
double timePublishMaps = 0.0;
if(!uIsFinite(odomRotationalVariance) || odomRotationalVariance<=0.0f)
{
odomRotationalVariance = odomDefaultAngVariance_;
}
if(!uIsFinite(odomTransitionalVariance) || odomTransitionalVariance<=0.0f)
{
odomTransitionalVariance = odomDefaultLinVariance_;
}
if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
{
timeRtabmap = timer.ticks();