使用ros actionlib向机器人发送目标点

当我们已有现成地图的时候，ros的Navigation程序包可以根据已有地图进行定位导航，其中的move_base程序包应用ros的actionlib机制接收全局目标点，进行路径规划，然后对移动机器人进行控制行进至目标点。本文将展示用c++和python如何应用actionlib来向move_base发送目标点。

参考：

• Map-Based Navigation 【1】
• ROS学习 Python读写文本文件 【2】
• ROS节点的初始化及退出详解（ros::init、SIGINT、ros::ok、ros::NodeHandle） 【3】
• jihoonl/waypoint/ 【4】

注：以下程序涉及的目标点均是一个二维的位置点，不包含orientation。根据geometry_msgs中PoseStamped的定义可以很容易拓展成包含姿态的目标点。

C++

以下是代码框架（主要来源于参考【1】和【3】）：

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>   // 引用move_base的信息
#include <actionlib/client/simple_action_client.h>   // 引用actionlib库
#include "std_msgs/String.h"
#include <sstream>
#include <iostream>
#include <signal.h>  // 引用signal头文件，为了做节点退出操作

using namespace std;
// 定义一个SimpleActionClient，用来给move_base一个目标点：
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

void DoShutdown(int sig)
{
	//这里主要进行退出前的数据保存、内存清理、告知其他节点等工作
	ROS_INFO("shutting down!");
	ros::shutdown(); 
    exit(sig); //为了更完整，处理一下退出的signal
}

int main(int argc, char** argv){
    ros::init(argc, argv, "a_goals_sender"); //初始化ros节点的常规操作
    
    // 声明一个SimpleActionClient：
    //tell the action client that we want to spin a thread by default
    MoveBaseClient ac("move_base", true);

  //wait for the action server to come up
    while(!ac.waitForServer(ros::Duration(5.0))){
        ROS_INFO("Waiting for the move_base action server to come up");
    }
    // 声明一个目标点goal，注意MoveBaseGoal的格式：
    move_base_msgs::MoveBaseGoal goal;
    
    //在ctrl+c时有效执行退出操作，方便扩展（参见参考【3】）
    signal(SIGINT, DoShutdown);
    
    ros::NodeHandle n;
    ros::Rate loop_rate(100); 
    
    while (ros::ok())
    {
        // Details here: http://edu.gaitech.hk/turtlebot/map-navigation.html 参见参考【1】
        /*goal.target_pose.header.frame_id = "map" specifies the reference frame for that location. In this example, it is specified as the map frame, which simply means that the coordinates will be considered in the global reference frame related to the map itself. In other words, it is the absolute position on the map. In case where the reference frame is set with respect to the robot, namely goal.target_pose.header.frame_id = "base_link" (like in this tutorial), the coordinate will have a completely other meaning. In fact, in this case the coordinate will represent the (x,y) coordinate with respect to robot base frame attached to the robot, so it is a relative position rather than a absolute position as in the case of using the map frame.
        //goal.target_pose.header.frame_id = "base_link";*/
        goal.target_pose.header.frame_id = "map";
        goal.target_pose.header.stamp = ros::Time::now();
		// 以下是一个随意取的二维目标点：
        goal.target_pose.pose.position.x = 1.3;
        goal.target_pose.pose.position.y = 7.56;
        goal.target_pose.pose.orientation.w = 1.0;

        ROS_INFO("Sending goal");
        // 定义好了goal，就可以调用SimpleActionClient的现成方法sendGoal()，非常方便：
        ac.sendGoal(goal);

        ac.waitForResult();
        // 判断是否执行成功：
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
          ROS_INFO("Hooray, the base moved to the goal");
        else
          ROS_INFO("The base failed to move for some reason");
      
        ros::spinOnce();

        loop_rate.sleep();
    }
    return 0;
}

整个流程并不复杂，按照格式定义好目标点goal即可。以上只是发送一个目标点的操作，有了单次操作，就可以很方便地进行扩展，例如按照一个目标点列表依次发送给机器人（在下一节python实现中会涉及）。

需要注意一点：

goal.target_pose.header.frame_id = “map” specifies the reference frame for that location. In this example, it is specified as the map frame, which simply means that the coordinates will be considered in the global reference frame related to the map itself. In other words, it is the absolute position on the map. In case where the reference frame is set with respect to the robot, namely goal.target_pose.header.frame_id = “base_link” (like in this tutorial), the coordinate will have a completely other meaning. In fact, in this case the coordinate will represent the (x,y) coordinate with respect to robot base frame attached to the robot, so it is a relative position rather than a absolute position as in the case of using the map frame.

也就是说目标点的frame_id决定了目标点的坐标是在全局坐标系下还是在以机器人为参考的坐标系下，“map”（或者其他名字的固定坐标系）就是在全局坐标系下，“base_link”一般就是以机器人当前位置为参考的局部坐标系下。

Python

本节代码来源于参考【2】和【4】，实现的是从一个文件中读取一系列目标点并发送给机器人。

import random
import rospy
import actionlib # 引用actionlib库
import waypoint_touring.utils as utils # 代码附在后面

import move_base_msgs.msg as move_base_msgs
import visualization_msgs.msg as viz_msgs

class TourMachine(object):

    def __init__(self, filename, random_visits=False, repeat=False):
        self._waypoints = utils.get_waypoints(filename) # 获取一系列目标点的值

        action_name = 'move_base'
        self._ac_move_base = actionlib.SimpleActionClient(action_name, move_base_msgs.MoveBaseAction) # 创建一个SimpleActionClient
        rospy.loginfo('Wait for %s server' % action_name)
        self._ac_move_base.wait_for_server
        self._counter = 0
        self._repeat = repeat
        self._random_visits = random_visits

        if self._random_visits:
            random.shuffle(self._waypoints)
        # 以下为了显示目标点：
        self._pub_viz_marker = rospy.Publisher('viz_waypoints', viz_msgs.MarkerArray, queue_size=1, latch=True)
        self._viz_markers = utils.create_viz_markers(self._waypoints)

    def move_to_next(self):
        p = self._get_next_destination()

        if not p:
            rospy.loginfo("Finishing Tour")
            return True
        # 把文件读取的目标点信息转换成move_base的goal的格式：
        goal = utils.create_move_base_goal(p)
        rospy.loginfo("Move to %s" % p['name'])
        # 这里也是一句很简单的send_goal:
        self._ac_move_base.send_goal(goal)
        self._ac_move_base.wait_for_result()
        result = self._ac_move_base.get_result()
        rospy.loginfo("Result : %s" % result)

        return False

    def _get_next_destination(self):
        """
        根据是否循环，是否随机访问等判断，决定下一个目标点是哪个
        """
        if self._counter == len(self._waypoints):
            if self._repeat:
                self._counter = 0
                if self._random_visits:
                    random.shuffle(self._waypoints)
            else:
                next_destination = None
        next_destination = self._waypoints[self._counter]
        self._counter = self._counter + 1
        return next_destination

    def spin(self):
        rospy.sleep(1.0)
        self._pub_viz_marker.publish(self._viz_markers)
        finished = False
        while not rospy.is_shutdown() and not finished:
            finished = self.move_to_next()
            rospy.sleep(2.0)

if __name__ == '__main__':
    rospy.init_node('tour')
    # 使用了ros的get_param读取文件名：
    filename = rospy.get_param('~filename')
    random_visits = rospy.get_param('~random', False)
    repeat = rospy.get_param('~repeat', False)

    m = TourMachine(filename, random_visits, repeat)
    rospy.loginfo('Initialized')
    m.spin()
    rospy.loginfo('Bye Bye')

下面是utils的实现：

import yaml
import rospy
import move_base_msgs.msg as move_base_msgs
import geometry_msgs.msg as geometry_msgs
import visualization_msgs.msg as viz_msgs
import std_msgs.msg as std_msgs

id_count = 1

def get_waypoints(filename):
    # 目标点文件是yaml格式的：
    with open(filename, 'r') as f:
        data = yaml.load(f)

    return data['waypoints']

def create_geo_pose(p):
    pose = geometry_msgs.Pose()

    pose.position.x = p['pose']['position']['x']
    pose.position.y = p['pose']['position']['y']
    pose.position.z = p['pose']['position']['z']
    pose.orientation.x = p['pose']['orientation']['x']
    pose.orientation.y = p['pose']['orientation']['y']
    pose.orientation.z = p['pose']['orientation']['z']
    pose.orientation.w = p['pose']['orientation']['w']
    return pose

def create_move_base_goal(p):
    target = geometry_msgs.PoseStamped()
    target.header.frame_id = p['frame_id']
    target.header.stamp = rospy.Time.now()
    target.pose = create_geo_pose(p)
    goal = move_base_msgs.MoveBaseGoal(target)
    return goal

def create_viz_markers(waypoints):
    marray= viz_msgs.MarkerArray()
    for w in waypoints:
        m_arrow = create_arrow(w)
        m_text = create_text(w)
        marray.markers.append(m_arrow)
        marray.markers.append(m_text)
    return marray

def create_marker(w):
    global id_count
    m = viz_msgs.Marker()
    m.header.frame_id = w['frame_id']
    m.ns = w['name']
    m.id = id_count
    m.action = viz_msgs.Marker.ADD
    m.pose = create_geo_pose(w)
    m.scale = geometry_msgs.Vector3(1.0,0.3,0.3)
    m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)

    id_count = id_count + 1
    return m

def create_arrow(w):
    m = create_marker(w)
    m.type = viz_msgs.Marker.ARROW
    m.color = std_msgs.ColorRGBA(0.0,1.0,0.0,1.0)
    return m

def create_text(w):
    m = create_marker(w)
    m.type = viz_msgs.Marker.TEXT_VIEW_FACING
    m.pose.position.z = 2.5
    m.text = w['name']
    return m

其中，目标点文件的格式如下：

waypoints:
- frame_id: map
  name: 68.809387207_10.8609399796
  pose:
    orientation:
      w: 1
      x: 0
      y: 0
      z: 0
    position:
      x: 68.80938720703125
      y: 10.860939979553223
      z: 0.0

最后补充一个相关的很有用的python程序：从rviz中获取鼠标点击的目标点，并保存成文件。

import yaml
import rospy
import geometry_msgs.msg as geometry_msgs

class WaypointGenerator(object):

    def __init__(self, filename):
        # 通过订阅rviz发出的/clicked_point的rostopic，就可以获取鼠标点击的目标点了：
        # Subscriber订阅/clicked_point的同时，交给作为callback函数的_process_pose去处理路标点：
        self._sub_pose = rospy.Subscriber('clicked_point', geometry_msgs.PointStamped, self._process_pose, queue_size=1)
        self._waypoints = []
        self._filename = filename

    def _process_pose(self, msg):
        p = msg.point

        data = {}
        data['frame_id'] = msg.header.frame_id
        data['pose'] = {}
        # 因为rviz输出的是2D Nav Goal所以只处理2维：
        data['pose']['position'] = {'x': p.x, 'y': p.y, 'z': 0.0}
        data['pose']['orientation'] = {'x': 0, 'y': 0, 'z': 0, 'w':1}
        data['name'] = '%s_%s' % (p.x, p.y)

        self._waypoints.append(data)
        rospy.loginfo("Clicked : (%s, %s, %s)" % (p.x, p.y, p.z))

    def _write_file(self):
        ways = {}
        ways['waypoints'] = self._waypoints
        # 把目标点输出成yaml文件：
        with open(self._filename, 'w') as f:
            f.write(yaml.dump(ways, default_flow_style=False))

    def spin(self):
        rospy.spin()
        self._write_file()


if __name__ == '__main__':

    rospy.init_node('waypoint_generator')
    filename = rospy.get_param('~filename')

    g = WaypointGenerator(filename)
    rospy.loginfo('Initialized')
    g.spin()
    rospy.loginfo('ByeBye')

这样，我们就可以在rviz上选取一系列的目标点并保存成文件，然后每次发送给机器人同样的目标点做重复测试了。