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

Updated to 0.10.8: rtabmapviz can receive goals (new topic /rtabmap/goal_node, ne msg rtabmap_ros::Goal) synchronized with global path

Browse Source
This commit is contained in:
matlabbe
2015-09-21 16:37:35 -04:00
parent aae95d323c
commit da58edf73a
7 changed files with 106 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+47 -30
View File
@@ -193,6 +193,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
// planning topics
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
@@ -1322,7 +1323,7 @@ void CoreWrapper::process(
}
}
void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform> > & poses)
void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform> > & poses, const ros::Time & stamp)
{
currentMetricGoal_.setNull();
latestNodeWasReached_ = false;
@@ -1343,14 +1344,13 @@ void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform>
else
{
ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
ros::Time now = ros::Time::now();
// Global path
if(globalPathPub_.getNumSubscribers())
{
nav_msgs::Path path;
path.header.frame_id = mapFrameId_;
path.header.stamp = now;
path.header.stamp = stamp;
path.poses.resize(poses.size());
std::stringstream stream;
for(unsigned int i=0; i<poses.size(); ++i)
@@ -1381,8 +1381,8 @@ void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform>
}
}
publishCurrentGoal(now);
publishLocalPath(now);
publishCurrentGoal(stamp);
publishLocalPath(stamp);
}
}
else
@@ -1412,7 +1412,37 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
UTimer timer;
rtabmap_.computePath(targetPose);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath());
goalCommonCallback(rtabmap_.getPath(), msg->header.stamp);
}
void CoreWrapper::goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg)
{
int id = msg->node_id;
if(id == 0 && !msg->node_label.empty() && rtabmap_.getMemory())
{
id = rtabmap_.getMemory()->getSignatureIdByLabel(msg->node_label);
}
if(id > 0)
{
ROS_INFO("Planning: set goal %d", id);
UTimer timer;
rtabmap_.computePath(id, true);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath(), msg->header.stamp);
if(currentMetricGoal_.isNull())
{
ROS_ERROR("Planning: Node id %d not found or goal already reached!", id);
}
}
else if(!msg->node_label.empty())
{
ROS_ERROR("Planning: Node with label \"%s\" not found!", msg->node_label.c_str());
}
else
{
ROS_ERROR("Planning: Node id should be > 0 !");
}
}
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -1851,31 +1881,18 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
{
int id = req.node_id;
if(id == 0 && !req.node_label.empty() && rtabmap_.getMemory())
rtabmap_ros::GoalPtr msg(new rtabmap_ros::Goal());
msg->header.stamp = ros::Time::now();
msg->node_id = req.node_id;
msg->node_label = req.node_label;
goalNodeCallback(msg);
const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
res.path_ids.resize(path.size());
res.path_poses.resize(path.size());
for(unsigned int i=0; i<path.size(); ++i)
{
id = rtabmap_.getMemory()->getSignatureIdByLabel(req.node_label);
}
if(id > 0)
{
ROS_INFO("Planning: set goal %d", id);
UTimer timer;
rtabmap_.computePath(id, true);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath());
if(currentMetricGoal_.isNull())
{
ROS_ERROR("Planning: Node id %d not found or goal already reached!", id);
}
}
else if(!req.node_label.empty())
{
ROS_ERROR("Planning: Node with label \"%s\" not found!", req.node_label.c_str());
}
else
{
ROS_ERROR("Planning: Node id should be > 0 !");
res.path_ids[i] = path[i].first;
rtabmap_ros::transformToPoseMsg(path[i].second, res.path_poses[i]);
}
return true;
}
src/CoreWrapper.h
+4 -1
View File
@@ -53,6 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/PublishMap.h"
#include "rtabmap_ros/SetGoal.h"
#include "rtabmap_ros/SetLabel.h"
#include "rtabmap_ros/Goal.h"
#include "MapsManager.h"
@@ -172,8 +173,9 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses);
void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses, const ros::Time & stamp);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
void updateGoal(const ros::Time & stamp);
void process(
@@ -251,6 +253,7 @@ private:
//Planning stuff
ros::Subscriber goalSub_;
ros::Subscriber goalNodeSub_;
ros::Publisher nextMetricGoalPub_;
ros::Publisher goalReachedPub_;
ros::Publisher globalPathPub_;
src/GuiWrapper.cpp
+33
View File
@@ -168,6 +168,14 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
infoTopic_,
mapDataTopic_);
infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
goalTopic_.subscribe(nh, "goal_node", 1);
pathTopic_.subscribe(nh, "global_path", 1);
goalPathSync_ = new message_filters::Synchronizer<MyGoalPathSyncPolicy>(
MyGoalPathSyncPolicy(queueSize),
goalTopic_,
pathTopic_);
goalPathSync_->registerCallback(boost::bind(&GuiWrapper::goalPathCallback, this, _1, _2));
}
GuiWrapper::~GuiWrapper()
@@ -243,6 +251,20 @@ void GuiWrapper::infoMapCallback(
this->post(new RtabmapEvent(stat));
}
void GuiWrapper::goalPathCallback(
const rtabmap_ros::GoalConstPtr & goalMsg,
const nav_msgs::PathConstPtr & pathMsg)
{
// we don't have the node ids, just generate fake ones.
std::vector<std::pair<int, Transform> > poses(pathMsg->poses.size());
for(unsigned int i=0; i<pathMsg->poses.size(); ++i)
{
poses[i].first = -int(i)-1;
poses[i].second = rtabmap_ros::transformFromPoseMsg(pathMsg->poses[i].pose);
}
this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses));
}
void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
{
std::map<int, Signature> signatures;
@@ -374,6 +396,17 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
{
ROS_ERROR("Can't call \"set_goal\" service");
}
else
{
UASSERT(setGoalSrv.response.path_ids.size() == setGoalSrv.response.path_poses.size());
std::vector<std::pair<int, Transform> > poses(setGoalSrv.response.path_poses.size());
for(unsigned int i=0; i<setGoalSrv.response.path_poses.size(); ++i)
{
poses[i].first = setGoalSrv.response.path_ids[i];
poses[i].second = rtabmap_ros::transformFromPoseMsg(setGoalSrv.response.path_poses[i]);
}
this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses));
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)
{
src/GuiWrapper.h
+11 -1
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/Info.h"
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/OdomInfo.h"
#include "rtabmap_ros/Goal.h"
#include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/core/Transform.h"
@@ -43,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
@@ -72,6 +74,7 @@ protected:
private:
void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
void goalPathCallback(const rtabmap_ros::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg);
void setupCallbacks(
bool subscribeDepth,
@@ -214,7 +217,9 @@ private:
message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
ros::Subscriber globalPathTopic_;
message_filters::Subscriber<rtabmap_ros::Goal> goalTopic_;
message_filters::Subscriber<nav_msgs::Path> pathTopic_;
ros::Subscriber defaultSub_; // odometry only
std::vector<image_transport::SubscriberFilter*> imageSubs_;
@@ -234,6 +239,11 @@ private:
rtabmap_ros::MapData> MyInfoMapSyncPolicy;
message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
typedef message_filters::sync_policies::ExactTime<
rtabmap_ros::Goal,
nav_msgs::Path> MyGoalPathSyncPolicy;
message_filters::Synchronizer<MyGoalPathSyncPolicy> * goalPathSync_;
// with odom msg
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::LaserScan,