使用move_base做4个点循环跑的导航

突然间接到个任务，要用ROS做一个循环跑的导航小车。这有点难住我了，以前用move_base跑导航的时候只是给一个目标点然后等它走过去，但是怎么才能循环的跑起来呢？第一反应百度一下， 好吧，貌似网上没有用move_base跑循环的。。。那就自己写一个吧。

以前记得《ros by example 1》里有个用move_base跑4个目标点的程序，先去看了下源码，发现它仅仅是把目标点发送出去，然后等待move_base 60s，如果这段时间内成功完成导航任务，则继续发布下个目标点，否则退出。程序源码在这！https://github.com/pirobot/rbx1/blob/indigo-devel/rbx1_nav/nodes/move_base_square.py

    def move(self, goal):
            # Send the goal pose to the MoveBaseAction server
            self.move_base.send_goal(goal)
            
            # Allow 1 minute to get there
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 
            
            # If we don't get there in time, abort the goal
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                # We made it!
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")

当时觉得好无语，哪有做控制是靠等待时间来判断的啊，通过当前位姿与目标点位姿的距离才能判断出导航任务是否完成啊。所以，只要把这段代码中添加一个距离的判断就可以实现基于move_base的循环导航了。即：

if(distance < 1.0)//米
{
    move_base.send_goal(goal);
}

只要当前位姿与目标点位姿的距离小于1米了，我就重新发送一个目标点给move_base，这样就可以实现循环导航了（在实验时试过：在导航过程中重新发送目标点，机器人会重新规划处一条到新目标点的路径及轨迹）。那当前时刻的位姿怎么确定呢？有2种方式，一种是订阅 定位 节点发布的 位姿 话题；感觉这种有点low，就pass掉了，因为我觉得应该会有一种更优越的方法。第二种稍后揭晓。

Actionlib

为了寻找这种更优的方法，只有去看move_base的源码了！以前只知道move_base加载全局路径规划和局部路径规划，然后使用局部路径规划来计算出cmd_vel，一直都没去看move_base的源码。。。

看了代码发现move_base其实是actionlib的服务端的实现，好吧，不知道actionlib是啥。后来又去看actionlib的概念。action也是一种类似于service的问答通讯机制，不一样的地方是action还带有一个反馈机制，可以不断反馈任务的实施进度，而且可以在任务实施过程中，中止运行。哇，带反馈，这不就是我要的更优越的方法么。之后通过下面2位大神的文章以及ROS官网中的教程简单学习了一下actionlib的使用。

http://www.guyuehome.com/908

https://www.cnblogs.com/cv-pr/p/6005013.html

http://wiki.ros.org/actionlib/Tutorials

如下图可以看出，action分为服务端和客户端，服务端会不断的向客服端发送反馈信息，而move_base为服务端，上述《ros by example 1 》中的例子为客服端。所以只需要在客户端中使用这个反馈就可以了。那么，怎么实现呢？

想要使用这个反馈需要确定3件事情：

1 确定move_base是否将这个反馈信息发出来了；

2 通过学习actionlib知道了这个消息传递的消息类型是要自己定义的，那么move_base反馈的消息类型是什么呢？

3 怎么才能接受到这个反馈消息呢？

问题1：是否发出了反馈消息

我们先从第一个问题开始：通过下面这个文章，知道了move_base确实是发布了反馈信息，而且反馈信息就是当前的位姿。ok，成功解决第一个问题。注意，这个反馈消息不是move_base发出来的，而是actionlib发出来的！

https://www.cnblogs.com/shhu1993/p/6323699.html

bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){
    //发布速度topic
    geometry_msgs::Twist cmd_vel;

    //push the feedback out
    //发布一些反馈信息
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position = current_position;
    as_->publishFeedback(feedback);
    
    //省略。。。

}

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal){

//we have a goal so start the planner(通知planner线程进行路径规划)
    boost::unique_lock<boost::mutex> lock(planner_mutex_);
    planner_goal_ = goal;
    runPlanner_ = true;
    #通知规划路径线程
    planner_cond_.notify_one();
    lock.unlock();
   
    ros::NodeHandle n;
    while(n.ok())
    {
        #被抢占了(可能是发出新的goal，也可能是取消了)
        if(as_->isPreemptRequested()){
            if(as_->isNewGoalAvailable()){
                #发布新的goal，通知planner线程工作。
                planner_cond_.notify_one();
            }
            else{
                //if we've been preempted explicitly we need to shut things down
                //强制退出
                return;
            }
        }
        //省略。。。
    }
}

与此同时，我也看到了在新的目标点到来时，新的任务抢占了之前的任务，move_base会将新的目标点发送到actionlib中并重新规划路径。 

问题2：消息类型

move_base的action通讯的消息类型在安装move_base时会自动安装，ROS中（http://wiki.ros.org/move_base_msgs）提供了这个消息的说明。简单说明一下，消息类型是由2个“---”分隔开的三种数据类型组成。第一条为目标点的消息类型；第二条为当此次动作执行完成时向客户端发送的消息类型，仅在动作结束时发送一次；第三条为反馈的消息类型，这个是在动作执行过程中一直发送的。

在这里可以看到，move_base的反馈消息类型为 geometry_msgs/PoseStamped ，消息的名字为 base_position。

#goal definition
geometry_msgs/PoseStamped target_pose

---
#result definition

---
#feedback
geometry_msgs/PoseStamped base_position

 这个反馈消息是发送在/move_base命名空间下的/feedback话题上，也就是/move_base/feedback。通过接收这个话题得到的消息如下所示：

---
header: 
  seq: 640
  stamp: 
    secs: 1547635103
    nsecs: 592393429
  frame_id: ''
status: 
  goal_id: 
    stamp: 
      secs: 1547635097
      nsecs: 258239984
    id: "/nav_test-4-1547635097.258"
  status: 1
  text: "This goal has been accepted by the simple action server"
feedback: 
  base_position: 
    header: 
      seq: 0
      stamp: 
        secs: 1547635103
        nsecs: 550548076
      frame_id: "map"
    pose: 
      position: 
        x: 0.00729128714117
        y: 0.0192558593724
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: -0.0199257022983
        w: 0.999801463486
---

OK，第二个问题成功解决掉。 

问题3：怎么接收反馈消息

在学习actionlib时知道了actionlib的使用方式，即，一方为服务端，负责发送动作执行的状态与结果；一方为客户端，负责发送动作目标并监听动作状态。反馈消息是从 服务端 按照着 自己定义的消息类型 向客户端发出。所以，反馈消息的处理是在客服端内进行，通过回调函数的方式进行处理。ROS中有例子如下：

http://wiki.ros.org/actionlib_tutorials/Tutorials/Writing%20a%20Callback%20Based%20Simple%20Action%20Client

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib_tutorials/FibonacciAction.h>

using namespace actionlib_tutorials;
typedef actionlib::SimpleActionClient<FibonacciAction> Client;

// Called once when the goal completes
void doneCb(const actionlib::SimpleClientGoalState& state,
            const FibonacciResultConstPtr& result)
{
  ROS_INFO("Finished in state [%s]", state.toString().c_str());
  ROS_INFO("Answer: %i", result->sequence.back());
  ros::shutdown();
}

// Called once when the goal becomes active
void activeCb()
{
  ROS_INFO("Goal just went active");
}

// Called every time feedback is received for the goal
void feedbackCb(const FibonacciFeedbackConstPtr& feedback)
{
  ROS_INFO("Got Feedback of length %lu", feedback->sequence.size());
}

int main (int argc, char **argv)
{
  ros::init(argc, argv, "test_fibonacci_callback");

  // Create the action client
  Client ac("fibonacci", true);

  ROS_INFO("Waiting for action server to start.");
  ac.waitForServer();
  ROS_INFO("Action server started, sending goal.");

  // Send Goal
  FibonacciGoal goal;
  goal.order = 20;
  ac.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

  ros::spin();
  return 0;
}

 可以看到，通过在发送目标点时 将回调函数注册进去，从而在每次反馈消息到来时将自动调用回调函数，也就是上文中的feedbackCb（）函数。在actionlib的源码中可以找到sendGoal()的声明，它为后面三个回调函数给了默认的初始值，一个指向空函数的函数指针。

  /**
   * \brief Sends a goal to the ActionServer, and also registers callbacks
   *
   * If a previous goal is already active when this is called. We simply forget
   * about that goal and start tracking the new goal. No cancel requests are made.
   * \param done_cb     Callback that gets called on transitions to Done
   * \param active_cb   Callback that gets called on transitions to Active
   * \param feedback_cb Callback that gets called whenever feedback for this goal is received
   */
  void sendGoal(const Goal & goal,
    SimpleDoneCallback done_cb = SimpleDoneCallback(),
    SimpleActiveCallback active_cb = SimpleActiveCallback(),
    SimpleFeedbackCallback feedback_cb = SimpleFeedbackCallback());

知道了回调函数的使用方式就可以接受反馈消息了。不完整的几行代码如下：在下面的代码中，每次反馈消息的到来都会调用feedbackCb()，并将base_position赋值到全局变量current_point当中。

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> Client;

geometry_msgs::Point current_point;

// Called once when the goal becomes active
void activeCb()
{
	ROS_INFO("Goal Received");
}

// Called every time feedback is received for the goal
void feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
{
//	ROS_INFO("Got base_position of Feedback");
	current_point.x = feedback->base_position.pose.position.x;
	current_point.y = feedback->base_position.pose.position.y;
	current_point.z = feedback->base_position.pose.position.z;
}


int main(int argc, char** argv)
{
	ros::init(argc, argv, "nav_move_base");
	ros::NodeHandle node;

	//Subscribe to the move_base action server
	Client ac("move_base", true);

	//Publisher to manually control the robot (e.g. to stop it, queue_size=5)
	cmdVelPub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 5);

	move_base_msgs::MoveBaseGoal goal;

	//Use the map frame to define goal poses
	goal.target_pose.header.frame_id = "map";

	//Set the time stamp to "now"
	goal.target_pose.header.stamp = ros::Time::now();

	//Set the goal pose to the i-th waypoint
	goal.target_pose.pose = pose_list[count];

	//Start the robot moving toward the goal
	ac.sendGoal(goal, Client::SimpleDoneCallback(), &activeCb, &feedbackCb);

	return 0;
}

可以看到，在sendGoal()函数中，第一个回调函数按照默认的声明给定了空值，并将在客户端中实现的后两个回调函数注册到actionlib中。

ac.sendGoal(goal, Client::SimpleDoneCallback(), &activeCb, &feedbackCb);

这个方法使用了全局函数，但是假如我想使用类该怎么做呢？这就需要boost库中的bind了，用法如下：

    actionlib::SimpleActionClient<FibonacciAction> ac;

    // Need boost::bind to pass in the 'this' pointer
    ac.sendGoal(goal,
                boost::bind(&MyNode::doneCb, this, _1, _2),
                Client::SimpleActiveCallback(),
                Client::SimpleFeedbackCallback());

OK，至此，我们已经成功解决上述3个问题并且成功的接受到了作为反馈消息的当前位姿。有了当前位姿我们就可以做很多事情了，包括编写一个基于move_base的循环跑的小程序。

虽然上述代码不完整，但是我想聪明的你肯定能够依照这上述的例子实现自己的程序。当然，如果你想下载我的完整代码也是可以的，不过我很不厚道的收费了，嘿嘿。下载链接如下：

https://download.csdn.net/download/tiancailx/10935014

程序用法：

roslaunch rbx1_bringup fake_turtlebot.launch

roslaunch hit_nav  fake_move_base_blank_map.launch

rviz

rosrun hit_nav move_base_square_hit

 第一步为启动基于arbotix模拟的仿真模型，使用的是《ros by example 1 》的代码，github链接如下：https://github.com/pirobot/rbx1，并且需要安装arbotix 以及 move_base。相信聪明的你一定能够运行成功的。

rviz的配置如下：

运行时的截图。由于目标点间距设置的比较近，所以有时即使改变了目标点他也会先完成之前的一个任务，把目标点设置远些、距离目标点的阈值设置的大些，就可以解决这个问题。