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

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/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)
{