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 issue #40: RtabmapGlobalPathEvent build error

Browse Source
This commit is contained in:
matlabbe
2015-10-27 09:31:05 -04:00
parent 1cff33e3fa
commit 94c7928321
3 changed files with 17 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+14 -2
View File
@@ -1344,7 +1344,8 @@ void CoreWrapper::goalCommonCallback(
int id,
const std::string & label,
const Transform & pose,
const ros::Time & stamp)
const ros::Time & stamp,
double * planningTime)
{
UTimer timer;
@@ -1362,10 +1363,19 @@ void CoreWrapper::goalCommonCallback(
ROS_INFO("Planning: set goal %s", pose.prettyPrint().c_str());
}
if(planningTime)
{
*planningTime = 0.0;
}
bool success = false;
if((id > 0 && rtabmap_.computePath(id, true)) ||
(!pose.isNull() && rtabmap_.computePath(pose)))
{
if(planningTime)
{
*planningTime = timer.elapsed();
}
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
const std::vector<std::pair<int, Transform> > & poses = rtabmap_.getPath();
@@ -1905,10 +1915,12 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res)
{
goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now());
double planningTime = 0.0;
goalCommonCallback(req.node_id, req.node_label, Transform(), ros::Time::now(), &planningTime);
const std::vector<std::pair<int, Transform> > & path = rtabmap_.getPath();
res.path_ids.resize(path.size());
res.path_poses.resize(path.size());
res.planning_time = planningTime;
for(unsigned int i=0; i<path.size(); ++i)
{
res.path_ids[i] = path[i].first;
src/CoreWrapper.h
+1 -1
View File
@@ -173,7 +173,7 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp);
void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalNodeCallback(const rtabmap_ros::GoalConstPtr & msg);
void updateGoal(const ros::Time & stamp);
src/GuiWrapper.cpp
+2 -2
View File
@@ -292,7 +292,7 @@ void GuiWrapper::goalPathCallback(
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));
this->post(new RtabmapGlobalPathEvent(goalMsg->node_id, goalMsg->node_label, poses, 0.0));
}
void GuiWrapper::goalReachedCallback(
@@ -441,7 +441,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
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));
this->post(new RtabmapGlobalPathEvent(setGoalSrv.request.node_id, setGoalSrv.request.node_label, poses, setGoalSrv.response.planning_time));
}
}
else if(cmd == rtabmap::RtabmapEventCmd::kCmdCancelGoal)