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

data_player: fixed build for 0.11.8, handling ~ in database path. rtabmap: added "tf_tolerance" parameter (default 0.1) for /map->/odom tf expiration

Browse Source
This commit is contained in:
matlabbe
2016-06-25 11:34:26 -04:00
parent ad832298ef
commit 42ba3b7cd1
3 changed files with 17 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+6 -3
View File
@@ -126,6 +126,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
int queueSize = 10;
bool publishTf = true;
double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms
std::string tfPrefix = "";
bool stereoApproxSync = false;
@@ -171,6 +172,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("tf_delay", tfDelay, tfDelay);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("tf_tolerance", tfTolerance, tfTolerance);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
@@ -219,6 +221,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize);
ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
@@ -424,7 +427,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
if(publishTf && optimizeIterations != 0)
{
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance));
}
else if(publishTf)
{
@@ -527,7 +530,7 @@ void CoreWrapper::saveParameters(const std::string & configFile)
}
}
void CoreWrapper::publishLoop(double tfDelay)
void CoreWrapper::publishLoop(double tfDelay, double tfTolerance)
{
if(tfDelay == 0)
return;
@@ -537,7 +540,7 @@ void CoreWrapper::publishLoop(double tfDelay)
if(!odomFrameId_.empty())
{
mapToOdomMutex_.lock();
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfTolerance);
geometry_msgs::TransformStamped msg;
msg.child_frame_id = odomFrameId_;
msg.header.frame_id = mapFrameId_;
src/CoreWrapper.h
+1 -1
View File
@@ -242,7 +242,7 @@ private:
rtabmap::ParametersMap loadParameters(const std::string & configFile);
void saveParameters(const std::string & configFile);
void publishLoop(double tfDelay);
void publishLoop(double tfDelay, double tfTolerance);
void publishStats(const ros::Time & stamp);
void publishCurrentGoal(const ros::Time & stamp);
src/DbPlayerNode.cpp
+10 -3
View File
@@ -42,6 +42,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/OdometryEvent.h>
@@ -120,18 +122,23 @@ int main(int argc, char** argv)
ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
ROS_INFO("database = %s", databasePath.c_str());
ROS_INFO("rate = %f", rate);
ROS_INFO("publish_tf = %s", publishTf?"true":"false");
rtabmap::DBReader reader(databasePath, rate, false, false, false, startId);
ROS_INFO("start_id = %d", startId);
if(databasePath.empty())
{
ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
return -1;
}
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
if(databasePath.size() && databasePath.at(0) != '/')
{
databasePath = UDirectory::currentDir(true) + databasePath;
}
ROS_INFO("database = %s", databasePath.c_str());
rtabmap::DBReader reader(databasePath, rate, false, false, false, startId);
if(!reader.init())
{
ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());