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

updated to latest planning changes from the rtabmap's library

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-17 17:31:03 -05:00
parent ca9ff6ea2b
commit dcee7ef2e2
2 changed files with 23 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+22 -13
View File
@@ -1005,7 +1005,7 @@ void CoreWrapper::process(
if(!updatedGoalPose.isNull()) if(!updatedGoalPose.isNull())
{ {
// Adjust the target pose relative to last node // Adjust the target pose relative to last node
if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back()) if(rtabmap_.getPathCurrentGoalId() == rtabmap_.getPath().back().first)
{ {
updatedGoalPose *= rtabmap_.getPathTransformToGoal(); updatedGoalPose *= rtabmap_.getPathTransformToGoal();
} }
@@ -1046,7 +1046,7 @@ void CoreWrapper::process(
} }
} }
void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> > & poses) void CoreWrapper::goalCommonCallback(const std::vector<std::pair<int, Transform> > & poses)
{ {
currentMetricGoal_.setNull(); currentMetricGoal_.setNull();
if(poses.size()) if(poses.size())
@@ -1075,15 +1075,21 @@ void CoreWrapper::goalCommonCallback(const std::list<std::pair<int, Transform> >
path.header.frame_id = mapFrameId_; path.header.frame_id = mapFrameId_;
path.header.stamp = now; path.header.stamp = now;
path.poses.resize(poses.size()); path.poses.resize(poses.size());
int oi = 0;
std::stringstream stream; std::stringstream stream;
for(std::list<std::pair<int, Transform> >::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(unsigned int i=0; i<poses.size(); ++i)
{ {
path.poses[oi].header = path.header; path.poses[i].header = path.header;
rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose); rtabmap_ros::transformToPoseMsg(poses[i].second, path.poses[i].pose);
++oi; stream << poses[i].first << " ";
stream << iter->first << " ";
} }
if(!rtabmap_.getPathTransformToGoal().isIdentity())
{
path.poses.resize(poses.size()+1);
Transform t = poses.back().second*rtabmap_.getPathTransformToGoal();
rtabmap_ros::transformToPoseMsg(t, path.poses[path.poses.size()-1].pose);
stream << "G";
}
ROS_INFO("Publishing global path: [%s]", stream.str().c_str()); ROS_INFO("Publishing global path: [%s]", stream.str().c_str());
globalPathPub_.publish(path); globalPathPub_.publish(path);
} }
@@ -1120,7 +1126,8 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
return; return;
} }
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str()); ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
goalCommonCallback(rtabmap_.computePath(targetPose, false)); rtabmap_.computePath(targetPose, false);
goalCommonCallback(rtabmap_.getPath());
} }
void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg) void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
@@ -1132,7 +1139,8 @@ void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr &
return; return;
} }
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str()); ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
goalCommonCallback(rtabmap_.computePath(targetPose, true)); rtabmap_.computePath(targetPose, true);
goalCommonCallback(rtabmap_.getPath());
} }
bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&) bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
@@ -1471,7 +1479,8 @@ bool CoreWrapper::setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ro
{ {
int id = req.target_node_id; int id = req.target_node_id;
ROS_INFO("Planning: set goal %d", id); ROS_INFO("Planning: set goal %d", id);
goalCommonCallback(rtabmap_.computePath(id, req.in_global_graph)); rtabmap_.computePath(id, req.in_global_graph);
goalCommonCallback(rtabmap_.getPath());
return !currentMetricGoal_.isNull(); return !currentMetricGoal_.isNull();
} }
@@ -2002,7 +2011,7 @@ void CoreWrapper::publishLocalPath(const ros::Time & stamp)
{ {
if(rtabmap_.getPath().size()) if(rtabmap_.getPath().size())
{ {
std::list<std::pair<int, Transform> > poses = rtabmap_.getPathNextPoses(); std::vector<std::pair<int, Transform> > poses = rtabmap_.getPathNextPoses();
if(poses.size()) if(poses.size())
{ {
if(localPathPub_.getNumSubscribers()) if(localPathPub_.getNumSubscribers())
@@ -2013,7 +2022,7 @@ void CoreWrapper::publishLocalPath(const ros::Time & stamp)
path.poses.resize(poses.size()); path.poses.resize(poses.size());
int oi = 0; int oi = 0;
std::stringstream stream; std::stringstream stream;
for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::vector<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
path.poses[oi].header = path.header; path.poses[oi].header = path.header;
rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose); rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
src/CoreWrapper.h
+1 -1
View File
@@ -107,7 +107,7 @@ private:
const sensor_msgs::LaserScanConstPtr& scanMsg, const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg); const nav_msgs::OdometryConstPtr & odomMsg);
void goalCommonCallback(const std::list<std::pair<int, rtabmap::Transform> > & poses); void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg); void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg); void goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void updateGoal(const ros::Time & stamp); void updateGoal(const ros::Time & stamp);