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

fixed #175

Browse Source
This commit is contained in:
matlabbe
2017-06-01 13:08:28 -04:00
parent 47e6c8dfdc
commit ba7c4c50ca
2 changed files with 17 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/CoreWrapper.h
+1
View File
@@ -178,6 +178,7 @@ private:
float rotVariance_;
float transVariance_;
rtabmap::Transform currentMetricGoal_;
rtabmap::Transform lastPublishedMetricGoal_;
bool latestNodeWasReached_;
rtabmap::ParametersMap parameters_;
src/CoreWrapper.cpp
+16 -4
View File
@@ -1248,6 +1248,7 @@ void CoreWrapper::process(
goalReachedPub_.publish(result);
}
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
}
else
@@ -1287,6 +1288,7 @@ void CoreWrapper::process(
goalReachedPub_.publish(result);
}
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
}
}
@@ -1377,6 +1379,7 @@ void CoreWrapper::goalCommonCallback(
const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
if(poses.size() == 0)
{
@@ -1548,6 +1551,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
lastPose_.setIdentity();
lastPoseIntermediate_ = false;
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
mapsManager_.clear();
previousStamp_ = ros::Time(0);
@@ -1604,6 +1608,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
transVariance_ = 0;
lastPose_.setIdentity();
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
userData_ = cv::Mat();
@@ -1967,6 +1972,7 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
NODELET_WARN("Goal cancelled!");
rtabmap_.clearPath(0);
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
if(goalReachedPub_.getNumSubscribers())
{
@@ -1976,9 +1982,9 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
}
}
if(mbClient_.isServerConnected())
{
mbClient_.cancelGoal();
}
{
mbClient_.cancelGoal();
}
return true;
}
@@ -2149,7 +2155,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
{
if(!currentMetricGoal_.isNull())
if(!currentMetricGoal_.isNull() && currentMetricGoal_ != lastPublishedMetricGoal_)
{
NODELET_INFO("Publishing next goal: %d -> %s",
rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str());
@@ -2175,6 +2181,7 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
boost::bind(&CoreWrapper::goalDoneCb, this, _1, _2),
boost::bind(&CoreWrapper::goalActiveCb, this),
boost::bind(&CoreWrapper::goalFeedbackCb, this, _1));
lastPublishedMetricGoal_ = currentMetricGoal_;
}
else
{
@@ -2184,6 +2191,10 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
if(nextMetricGoalPub_.getNumSubscribers())
{
nextMetricGoalPub_.publish(poseMsg);
if(!useActionForGoal_)
{
lastPublishedMetricGoal_ = currentMetricGoal_;
}
}
}
}
@@ -2228,6 +2239,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
{
rtabmap_.clearPath(1);
currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false;
}
}