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

removed goal_global topic subscription (to send a global goal, use set_goal service instead)

Browse Source
This commit is contained in:
matlabbe
2015-06-21 18:30:29 -04:00
parent de70790e5a
commit fae3e2fd6d
2 changed files with 1 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+1 -17
View File
@@ -188,7 +188,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
// planning topics // planning topics
goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this); goalSub_ = nh.subscribe("goal", 1, &CoreWrapper::goalCallback, this);
goalGlobalSub_ = nh.subscribe("goal_global", 1, &CoreWrapper::goalGlobalCallback, this);
nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1); nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_out", 1);
goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1); goalReachedPub_ = nh.advertise<std_msgs::Bool>("goal_reached", 1);
globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1); globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
@@ -1396,22 +1395,7 @@ void CoreWrapper::goalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
} }
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str()); ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
UTimer timer; UTimer timer;
rtabmap_.computePath(targetPose, false); rtabmap_.computePath(targetPose);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath());
}
void CoreWrapper::goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg)
{
Transform targetPose = rtabmap_ros::transformFromPoseMsg(msg->pose);
if(targetPose.isNull())
{
ROS_ERROR("Pose received is null!");
return;
}
ROS_INFO("Planning: set goal %s", targetPose.prettyPrint().c_str());
UTimer timer;
rtabmap_.computePath(targetPose, true);
ROS_INFO("Planning: Time computing path = %f s", timer.ticks()); ROS_INFO("Planning: Time computing path = %f s", timer.ticks());
goalCommonCallback(rtabmap_.getPath()); goalCommonCallback(rtabmap_.getPath());
} }
src/CoreWrapper.h
-2
View File
@@ -174,7 +174,6 @@ private:
void goalCommonCallback(const std::vector<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 updateGoal(const ros::Time & stamp); void updateGoal(const ros::Time & stamp);
void process( void process(
@@ -249,7 +248,6 @@ private:
//Planning stuff //Planning stuff
ros::Subscriber goalSub_; ros::Subscriber goalSub_;
ros::Subscriber goalGlobalSub_;
ros::Publisher nextMetricGoalPub_; ros::Publisher nextMetricGoalPub_;
ros::Publisher goalReachedPub_; ros::Publisher goalReachedPub_;
ros::Publisher globalPathPub_; ros::Publisher globalPathPub_;