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

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 rotVariance_;
float transVariance_; float transVariance_;
rtabmap::Transform currentMetricGoal_; rtabmap::Transform currentMetricGoal_;
rtabmap::Transform lastPublishedMetricGoal_;
bool latestNodeWasReached_; bool latestNodeWasReached_;
rtabmap::ParametersMap parameters_; rtabmap::ParametersMap parameters_;
src/CoreWrapper.cpp
+16 -4
View File
@@ -1248,6 +1248,7 @@ void CoreWrapper::process(
goalReachedPub_.publish(result); goalReachedPub_.publish(result);
} }
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
} }
else else
@@ -1287,6 +1288,7 @@ void CoreWrapper::process(
goalReachedPub_.publish(result); goalReachedPub_.publish(result);
} }
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
} }
} }
@@ -1377,6 +1379,7 @@ void CoreWrapper::goalCommonCallback(
const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath(); const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
if(poses.size() == 0) if(poses.size() == 0)
{ {
@@ -1548,6 +1551,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
lastPose_.setIdentity(); lastPose_.setIdentity();
lastPoseIntermediate_ = false; lastPoseIntermediate_ = false;
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
mapsManager_.clear(); mapsManager_.clear();
previousStamp_ = ros::Time(0); previousStamp_ = ros::Time(0);
@@ -1604,6 +1608,7 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
transVariance_ = 0; transVariance_ = 0;
lastPose_.setIdentity(); lastPose_.setIdentity();
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
userData_ = cv::Mat(); userData_ = cv::Mat();
@@ -1967,6 +1972,7 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
NODELET_WARN("Goal cancelled!"); NODELET_WARN("Goal cancelled!");
rtabmap_.clearPath(0); rtabmap_.clearPath(0);
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
if(goalReachedPub_.getNumSubscribers()) if(goalReachedPub_.getNumSubscribers())
{ {
@@ -1976,9 +1982,9 @@ bool CoreWrapper::cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Em
} }
} }
if(mbClient_.isServerConnected()) if(mbClient_.isServerConnected())
{ {
mbClient_.cancelGoal(); mbClient_.cancelGoal();
} }
return true; return true;
} }
@@ -2149,7 +2155,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
void CoreWrapper::publishCurrentGoal(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", NODELET_INFO("Publishing next goal: %d -> %s",
rtabmap_.getPathCurrentGoalId(), currentMetricGoal_.prettyPrint().c_str()); 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::goalDoneCb, this, _1, _2),
boost::bind(&CoreWrapper::goalActiveCb, this), boost::bind(&CoreWrapper::goalActiveCb, this),
boost::bind(&CoreWrapper::goalFeedbackCb, this, _1)); boost::bind(&CoreWrapper::goalFeedbackCb, this, _1));
lastPublishedMetricGoal_ = currentMetricGoal_;
} }
else else
{ {
@@ -2184,6 +2191,10 @@ void CoreWrapper::publishCurrentGoal(const ros::Time & stamp)
if(nextMetricGoalPub_.getNumSubscribers()) if(nextMetricGoalPub_.getNumSubscribers())
{ {
nextMetricGoalPub_.publish(poseMsg); nextMetricGoalPub_.publish(poseMsg);
if(!useActionForGoal_)
{
lastPublishedMetricGoal_ = currentMetricGoal_;
}
} }
} }
} }
@@ -2228,6 +2239,7 @@ void CoreWrapper::goalDoneCb(const actionlib::SimpleClientGoalState& state,
{ {
rtabmap_.clearPath(1); rtabmap_.clearPath(1);
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
lastPublishedMetricGoal_.setNull();
latestNodeWasReached_ = false; latestNodeWasReached_ = false;
} }
} }