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 "wait_for_transform_duration" (default 0.1 s) parameter to rtabmap, rtabmapviz and odometry nodes

Browse Source
This commit is contained in:
matlabbe
2015-08-14 15:01:51 -04:00
parent cc5203127f
commit 0dd1d5721b
11 changed files with 105 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_turtlebot_mapping.launch
+19 -3
View File
@@ -26,6 +26,8 @@
<arg name="rgbd_odometry" default="false"/>
<arg name="args" default=""/>
<arg name="version083" default="false"/>
<arg name="rtabmapviz" default="false"/>
<arg name="wait_for_transform" default="0.1"/>
<!-- Navigation stuff (move_base) -->
<include file="$(find turtlebot_bringup)/launch/3dsensor.launch"/>
@@ -38,12 +40,12 @@
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<!-- inputs -->
<remap from="scan" to="/scan"/>
<remap from="scan" to="/scan"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
@@ -74,8 +76,9 @@
<!-- Odometry : ONLY for testing without the actual robot! /odom TF should not be already published. -->
<node if="$(arg rgbd_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Odom/Force2D" type="string" value="true"/>
<param name="Odom/InlierDistance" type="string" value="0.05"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
@@ -86,5 +89,18 @@
<remap from="grid_map" to="/map"/>
</node>
<!-- visualization with rtabmapviz -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="scan" to="/scan"/>
</node>
</group>
</launch>
launch/rgbd_mapping.launch
+4 -4
View File
@@ -31,7 +31,7 @@
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="true"/>
<arg name="wait_for_transform" default="0.1"/>
<!-- Odometry parameters: -->
<arg name="strategy" default="0" /> <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
@@ -52,7 +52,7 @@
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="wait_for_transform_duration" type="bool" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
@@ -69,7 +69,7 @@
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="wait_for_transform_duration" type="bool" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
@@ -96,7 +96,7 @@
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="wait_for_transform_duration" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
launch/stereo_mapping.launch
+4 -4
View File
@@ -32,7 +32,7 @@
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="true"/>
<arg name="wait_for_transform" default="0.1"/>
<!-- Odometry parameters: -->
<arg name="strategy" default="0" /> <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
@@ -56,7 +56,7 @@
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="wait_for_transform_duration" type="bool" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approximate_sync)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
@@ -76,7 +76,7 @@
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="wait_for_transform_duration" type="bool" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approximate_sync)"/>
@@ -106,7 +106,7 @@
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="wait_for_transform_duration" type="bool" value="$(arg wait_for_transform)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
src/CoreWrapper.cpp
+11 -26
View File
@@ -82,6 +82,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
configPath_(""),
databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
useActionForGoal_(false),
genScan_(false),
genScanMaxDepth_(4.0),
@@ -147,6 +148,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
pnh.param("tf_delay", tfDelay, tfDelay);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
pnh.param("gen_scan", genScan_, genScan_);
pnh.param("gen_scan_max_depth", genScanMaxDepth_, genScanMaxDepth_);
@@ -660,26 +662,9 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
if(!paused_)
{
// Odom TF ready?
Transform odom;
try
Transform odom = getTransform(odomFrameId_, frameId_, stamp);
if(odom.isNull())
{
if(waitForTransform_)
{
//if(!tfBuffer_.canTransform(odomFrameId_, frameId_, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(odomFrameId_, frameId_, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", odomFrameId_.c_str(), frameId_.c_str());
return false;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(odomFrameId_, frameId_, stamp, tmp);
odom = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return false;
}
@@ -710,28 +695,28 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
Transform CoreWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
// TF ready?
Transform localTransform;
Transform transform;
try
{
if(waitForTransform_)
if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_>0.0)
{
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", fromFrameId.c_str(), toFrameId.c_str());
return localTransform;
ROS_WARN("rtabmap: Could not get transform from %s to %s after %f second!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_);
return transform;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
transform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
}
return localTransform;
return transform;
}
void CoreWrapper::commonDepthCallback(
src/CoreWrapper.h
+1
View File
@@ -234,6 +234,7 @@ private:
std::string configPath_;
std::string databasePath_;
bool waitForTransform_;
double waitForTransformDuration_;
bool useActionForGoal_;
bool genScan_;
double genScanMaxDepth_;
src/GuiWrapper.cpp
+18 -25
View File
@@ -73,6 +73,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
mainWindow_(0),
frameId_("base_link"),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
cameraNodeName_(""),
lastOdomInfoUpdateTime_(0),
depthScanSync_(0),
@@ -133,6 +134,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
pnh.param("queue_size", queueSize, queueSize);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process
if(!tfPrefix.empty())
@@ -409,28 +411,28 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
Transform GuiWrapper::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
// TF ready?
Transform localTransform;
Transform transform;
try
{
if(waitForTransform_ && !stamp.isZero())
if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
{
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", fromFrameId.c_str(), toFrameId.c_str());
return localTransform;
ROS_WARN("rtabmapviz: Could not get transform from %s to %s after %f seconds!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_);
return transform;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
transform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
}
return localTransform;
return transform;
}
void GuiWrapper::commonDepthCallback(
@@ -515,12 +517,6 @@ void GuiWrapper::commonDepthCallback(
return;
}
//for sync transform
if(odomT.isNull())
{
return;
}
cv::Mat rgb;
cv::Mat depth;
std::vector<CameraModel> cameraModels;
@@ -684,7 +680,7 @@ void GuiWrapper::commonDepthCallback(
cameraModels,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomT,
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
@@ -763,12 +759,6 @@ void GuiWrapper::commonStereoCallback(
return;
}
//for sync transform
if(odomT.isNull())
{
return;
}
Transform localTransform = getTransform(frameId_, leftCamInfoMsg->header.frame_id, leftCamInfoMsg->header.stamp);
if(localTransform.isNull())
{
@@ -777,12 +767,15 @@ void GuiWrapper::commonStereoCallback(
// sync with odometry stamp
if(odomHeader.stamp != leftCamInfoMsg->header.stamp)
{
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp);
if(sensorT.isNull())
if(!odomT.isNull())
{
return;
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp);
if(sensorT.isNull())
{
return;
}
localTransform = odomT.inverse() * sensorT * localTransform;
}
localTransform = odomT.inverse() * sensorT * localTransform;
}
image_geometry::StereoCameraModel model;
@@ -860,7 +853,7 @@ void GuiWrapper::commonStereoCallback(
stereoModel,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomT,
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
src/GuiWrapper.h
+1
View File
@@ -209,6 +209,7 @@ private:
std::string frameId_;
std::string odomFrameId_;
bool waitForTransform_;
double waitForTransformDuration_;
tf::TransformListener tfListener_;
message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
src/OdometryROS.cpp
+35 -18
View File
@@ -57,6 +57,7 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
groundTruthFrameId_(""),
publishTf_(true),
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
paused_(false)
{
this->processArguments(argc, argv);
@@ -78,6 +79,7 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
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_);
@@ -302,35 +304,50 @@ void OdometryROS::processArguments(int argc, char * argv[])
}
}
Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
// TF ready?
Transform transform;
try
{
if(waitForTransform_ && !stamp.isZero() && waitForTransformDuration_ > 0.0)
{
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
{
ROS_WARN("odometry: Could not get transform from %s to %s after %f seconds!", fromFrameId.c_str(), toFrameId.c_str(), waitForTransformDuration_);
return transform;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(fromFrameId, toFrameId, stamp, tmp);
transform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
}
return transform;
}
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(odometry_->getPose().isNull() &&
!groundTruthFrameId_.empty())
{
tf::StampedTransform initialPose; // sync with the first value of the ground truth
try
// sync with the first value of the ground truth
Transform initialPose = getTransform(groundTruthFrameId_, frameId_, stamp);
if(initialPose.isNull())
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str());
return;
}
}
this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, stamp, initialPose);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
rtabmap::Transform pose = rtabmap_ros::transformFromTF(initialPose);
ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
pose.prettyPrint().c_str(),
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
frameId_.c_str());
odometry_->reset(pose);
odometry_->reset(initialPose);
}
// process data
src/OdometryROS.h
+2 -1
View File
@@ -66,7 +66,7 @@ public:
const tf::TransformListener & tfListener() const {return tfListener_;}
bool isPaused() const {return paused_;}
bool isOdometryBOW() const;
bool waitForTransform() const {return waitForTransform_;}
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
private:
rtabmap::Odometry * odometry_;
@@ -77,6 +77,7 @@ private:
std::string groundTruthFrameId_;
bool publishTf_;
bool waitForTransform_;
double waitForTransformDuration_;
rtabmap::ParametersMap parameters_;
ros::Publisher odomPub_;
src/RGBDOdometryNode.cpp
+6 -32
View File
@@ -190,22 +190,9 @@ public:
ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
tf::StampedTransform localTransform;
try
Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
if(localTransform.isNull())
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), image->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
@@ -218,7 +205,7 @@ public:
model.fy(),
model.cx(),
model.cy(),
rtabmap_ros::transformFromTF(localTransform));
localTransform);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
@@ -290,22 +277,9 @@ public:
higherStamp = stamp;
}
tf::StampedTransform localTransform;
try
Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
if(localTransform.isNull())
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageMsgs[i]->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
@@ -374,7 +348,7 @@ public:
model.fy(),
model.cx(),
model.cy(),
rtabmap_ros::transformFromTF(localTransform)));
localTransform));
}
rtabmap::SensorData data(
src/StereoOdometryNode.cpp
+4 -18
View File
@@ -134,23 +134,9 @@ public:
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
tf::StampedTransform localTransform;
try
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
if(localTransform.isNull())
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageRectLeft->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
@@ -174,14 +160,14 @@ public:
model.left().cx(),
model.left().cy(),
model.baseline(),
rtabmap_ros::transformFromTF(localTransform));
localTransform);
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", rtabmap_ros::transformFromTF(localTransform).prettyPrint().c_str());
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,