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

Merged planner node into rtabmap node

Browse Source
This commit is contained in:
Mathieu Labbe
2015-01-30 15:31:44 -05:00
parent 78307669b3
commit d764ca5ba3
4 changed files with 142 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+129
View File
@@ -30,6 +30,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Int32MultiArray.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Camera.h>
#include <rtabmap/core/Parameters.h>
@@ -129,6 +131,18 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
mapData_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
mapGraph_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
// planning topics
goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_pose", 1);
nextMetricGoalIdPub_ = nh.advertise<std_msgs::Int32>("goal_pose_id", 1);
goalReachedPub_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
pathPub_ = nh.advertise<nav_msgs::Path>("path", 1);
pathIdsPub_ = nh.advertise<std_msgs::Int32MultiArray>("path_ids", 1);
ros::Publisher nextMetricGoal_;
ros::Publisher goalReached_;
ros::Publisher path_;
configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
databasePath_ = uReplaceChar(databasePath_, '~', UDirectory::homeDir());
@@ -916,6 +930,8 @@ void CoreWrapper::process(
const Statistics & stats = rtabmap_.getStatistics();
this->publishStats(stats);
this->updateGoal();
}
}
else if(!rtabmap_.isIDsGenerated())
@@ -933,6 +949,118 @@ void CoreWrapper::process(
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
}
void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
{
currentMetricGoal_.setNull();
int id = msg->data;
ROS_INFO("Planning: set goal %d", id);
std::list<std::pair<int, Transform> > poses = rtabmap_.computePath(id);
if(poses.size())
{
currentMetricGoal_ = rtabmap_.getPose(rtabmap_.getPathGoalId());
if(currentMetricGoal_.isNull())
{
ROS_ERROR("Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
rtabmap_.clearPath();
}
else
{
ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
ros::Time now = ros::Time::now();
if(pathPub_.getNumSubscribers())
{
nav_msgs::Path path;
path.header.frame_id = mapFrameId_;
path.header.stamp = now;
path.poses.resize(poses.size());
int oi = 0;
for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
path.poses[oi].header = path.header;
rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
++oi;
}
pathPub_.publish(path);
}
if(pathIdsPub_.getNumSubscribers())
{
std_msgs::Int32MultiArray array;
array.data.resize(poses.size());
int oi = 0;
for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
array.data[oi++] = iter->first;
}
pathIdsPub_.publish(array);
}
publishGoal();
}
}
else
{
ROS_WARN("Planning: Cannot compute a path (or goal is already reached)!");
goalReachedPub_.publish(std_msgs::Empty());
}
}
void CoreWrapper::updateGoal()
{
if(!currentMetricGoal_.isNull())
{
if(rtabmap_.getPath().size() == 0)
{
// Goal reached
ROS_INFO("Planning: Publishing goal reached!");
if(goalReachedPub_.getNumSubscribers())
{
goalReachedPub_.publish(std_msgs::Empty());
}
currentMetricGoal_.setNull();
}
else
{
Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathGoalId());
if(!updatedGoalPose.isNull())
{
// detect if the goal has changed or local map
// has changed so much that current goal drifted
if(currentMetricGoal_.getDistance(updatedGoalPose) > rtabmap_.getGoalReachedRadius()/2.0f)
{
currentMetricGoal_ = updatedGoalPose;
publishGoal();
}
}
else
{
ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
}
}
}
}
void CoreWrapper::publishGoal()
{
ROS_INFO("Planning: Publishing next goal: Location %d pose=%s",
rtabmap_.getPathGoalId(), currentMetricGoal_.prettyPrint().c_str());
if(nextMetricGoalPub_.getNumSubscribers())
{
geometry_msgs::PoseStamped goalMsg;
goalMsg.header.frame_id = mapFrameId_;
goalMsg.header.stamp = ros::Time::now();
rtabmap_ros::transformToPoseMsg(currentMetricGoal_, goalMsg.pose);
nextMetricGoalPub_.publish(goalMsg);
}
if(nextMetricGoalIdPub_.getNumSubscribers())
{
std_msgs::Int32 msg;
msg.data = rtabmap_.getPathGoalId();
nextMetricGoalIdPub_.publish(msg);
}
}
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -980,6 +1108,7 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
rtabmap_.resetMemory();
_variance = 0;
lastPose_.setIdentity();
currentMetricGoal_.setNull();
return true;
}
src/CoreWrapper.h
+13
View File
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
@@ -92,6 +93,9 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg);
void goalNodeCallback(const std_msgs::Int32ConstPtr & msg);
void updateGoal();
void publishGoal();
void process(
int id,
@@ -129,6 +133,7 @@ private:
bool paused_;
rtabmap::Transform lastPose_;
float _variance;
rtabmap::Transform currentMetricGoal_;
std::string frameId_;
std::string mapFrameId_;
@@ -144,6 +149,14 @@ private:
ros::Publisher mapData_;
ros::Publisher mapGraph_;
//Planning stuff
ros::Subscriber goalNodeSub_;
ros::Publisher nextMetricGoalPub_;
ros::Publisher nextMetricGoalIdPub_;
ros::Publisher goalReachedPub_;
ros::Publisher pathPub_;
ros::Publisher pathIdsPub_;
// for loop closure detection only
image_transport::Subscriber defaultSub_;
src/PlannerNode.cpp
-318
View File
@@ -1,318 +0,0 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <nav_msgs/Path.h>
#include <tf/transform_listener.h>
#include <rtabmap_ros/MsgConversion.h>
#include <rtabmap_ros/GetMap.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/Graph.h>
#include <queue>
class Planner
{
public:
Planner() :
frameId_("base_link"),
mapFrameId_("map"),
plan2d_(true),
goalError_(1.0),
graphChangeError_(0.2),
currentGoalIndex_(0)
{
ros::NodeHandle pnh("~");
pnh.param("frame_id", frameId_, frameId_);
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("plan_2d", plan2d_, plan2d_);
pnh.param("goal_error", goalError_, goalError_);
pnh.param("graph_change_error", graphChangeError_, graphChangeError_);
ros::NodeHandle nh;
goalTopic_ = nh.subscribe("goal", 1, &Planner::goalCallback, this);
mapDataTopic_ = nh.subscribe("mapData", 1, &Planner::mapDataCallback, this);
pubNextGoal_ = nh.advertise<geometry_msgs::PoseStamped>("next_goal", 1);
pubGoalReached_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
pubPath_ = nh.advertise<nav_msgs::Path>("path", 1);
}
~Planner()
{
}
void goalCallback(const std_msgs::Int32ConstPtr & msg)
{
int id = msg->data;
ROS_INFO("planner: set goal %d", id);
rtabmap_ros::GetMap getMapSrv;
getMapSrv.request.global = true;
getMapSrv.request.optimized = true;
getMapSrv.request.graphOnly = true;
if(!ros::service::call("get_map", getMapSrv))
{
ROS_WARN("Can't call \"get_map\" service");
return;
}
std::map<int, rtabmap::Transform> poses;
std::map<int, int> mapIds;
std::multimap<int, rtabmap::Link> constraints;
rtabmap::Transform mapToOdom;
rtabmap_ros::mapGraphFromROS(getMapSrv.response.data.graph, poses, mapIds, constraints, mapToOdom);
std::multimap<int, int> links;
for(std::multimap<int, rtabmap::Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
links.insert(std::make_pair(iter->first, iter->second.to()));
links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
}
if(!uContains(poses, id))
{
ROS_WARN("Goal %d not found in the global graph!", id);
return;
}
// find the nearest node of the current location
rtabmap::Transform currentPose;
try
{
tf::StampedTransform tmp;
tfListener_.lookupTransform(mapFrameId_, frameId_, ros::Time(0), tmp);
currentPose = rtabmap_ros::transformFromTF(tmp);
if(plan2d_)
{
currentPose.z() = 0;
}
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
int nearestPose = -1;
float nearestDistance = -1.0f;
for(std::map<int, rtabmap::Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(plan2d_)
{
iter->second.z() = 0;
}
float d = currentPose.getDistance(iter->second);
if(nearestPose == -1 || d < nearestDistance)
{
nearestPose = iter->first;
nearestDistance = d;
}
}
if(nearestPose <= 0)
{
ROS_WARN("Nearest pose not found!?!?");
return;
}
ROS_INFO("Computing path from location %d to %d", nearestPose, id);
path_ = rtabmap::computePath(poses, links, nearestPose, id);
currentGoalIndex_ = 0;
currentGoal_.setNull();
if(path_.size()<2)
{
path_.clear();
ROS_WARN("Cannot compute a path (or goal is already reached)!");
}
else
{
ROS_INFO("Path generated! Size=%d", (int)path_.size());
if(pubPath_.getNumSubscribers())
{
nav_msgs::Path path;
path.header.frame_id = mapFrameId_;
path.header.stamp = ros::Time::now();
path.poses.resize(path_.size());
for(unsigned int i=0; i<path_.size(); ++i)
{
path.poses[i].header = path.header;
rtabmap_ros::transformToPoseMsg(poses[path_[i]], path.poses[i].pose);
}
pubPath_.publish(path);
}
}
}
void mapDataCallback(const rtabmap_ros::MapDataConstPtr & msg)
{
if(path_.size())
{
// Get current pose
rtabmap::Transform currentPose;
try
{
tf::StampedTransform tmp;
tfListener_.lookupTransform(mapFrameId_, frameId_, ros::Time(0), tmp);
currentPose = rtabmap_ros::transformFromTF(tmp);
if(plan2d_)
{
currentPose.z() = 0;
}
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
// Convert graph data
std::map<int, rtabmap::Transform> poses;
std::map<int, int> mapIds;
std::multimap<int, rtabmap::Link> constraints;
rtabmap::Transform mapToOdom;
rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, constraints, mapToOdom);
// Post a new goal if:
// - no current goal is set,
// - the current goal is reached,
// - the current goal has moved (graph was optimized with new constraints)
if(!currentGoal_.isNull())
{
std::map<int, rtabmap::Transform>::iterator iter=poses.find(path_[currentGoalIndex_]);
if(iter != poses.end())
{
rtabmap::Transform currentGoal = iter->second;
if(plan2d_)
{
currentGoal.z() = 0;
}
float d = currentGoal.getDistance(currentGoal_);
if(d > graphChangeError_)
{
// reset the goal
ROS_INFO("Goal reset because the local graph has changed (%f m) newCurent=%s oldCurrent=%s...",
d, currentGoal.prettyPrint().c_str(), currentGoal_.prettyPrint().c_str());
currentGoal_.setNull();
}
if(!currentGoal_.isNull())
{
float d = currentPose.getDistance(currentGoal_);
if(d < goalError_)
{
//Current goal reached! Reset it.
currentGoal_.setNull();
if(currentGoalIndex_ == path_.size()-1)
{
// last goal reached!
ROS_INFO("Goal %d reached!", path_[currentGoalIndex_]);
pubGoalReached_.publish(std_msgs::Empty());
path_.clear();
return; // EXIT
}
}
}
}
}
if(currentGoal_.isNull())
{
// find the farthest node that can be reached in the local map
for(unsigned int i=path_.size()-1; i>=currentGoalIndex_; --i)
{
std::map<int, rtabmap::Transform>::iterator iter=poses.find(path_[i]);
if(iter != poses.end())
{
currentGoalIndex_ = i;
currentGoal_ = iter->second;
if(plan2d_)
{
currentGoal_.z() = 0;
}
break;
}
}
if(currentGoal_.isNull())
{
ROS_ERROR("Cannot update current goal!");
}
else
{
ROS_INFO("Publishing next goal: Location %d (%d/%d)",
path_[currentGoalIndex_], currentGoalIndex_+1, (int)path_.size());
geometry_msgs::PoseStamped goalMsg;
goalMsg.header = msg->header;
rtabmap_ros::transformToPoseMsg(currentGoal_, goalMsg.pose);
pubNextGoal_.publish(goalMsg);
}
}
}
}
private:
std::string frameId_;
std::string mapFrameId_;
bool plan2d_;
double goalError_;
double graphChangeError_;
tf::TransformListener tfListener_;
ros::Subscriber goalTopic_;
ros::Subscriber mapDataTopic_;
ros::Publisher pubNextGoal_;
ros::Publisher pubGoalReached_;
ros::Publisher pubPath_;
std::vector<int> path_;
int currentGoalIndex_;
rtabmap::Transform currentGoal_;
};
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ros::init(argc, argv, "planner");
Planner planner;
ros::spin();
return 0;
}