一起做ROS-DEMO系列（1）：控制移动机器人自主导航并停到标签(AR Marker)之前

在观看某机器人的视频时，我们看到了这样一个场景：

• 可以看到机器人在检测到物体的标签后，自主导航至物体之前，并对标签物体进行抓取。那么接下来我想以自己的一个demo个大家分解一下这个任务在ROS下的实现方式。
  文末有完整代码。
  

1. 通过AR Tracker识别标签。

ar_track_alvar官方说明：http://wiki.ros.org/ar_track_alvar
主要步骤如下：

• 安装ar-track-alvar功能包，$ sudo apt-get install ros-kinectic-ar-track-alvar
• 配置launch启动文件：

<launch>
	<arg name="marker_size" default="5" />  <!--定义marker最外框的尺寸，注意单位是厘米-->
	<arg name="max_new_marker_error" default="0.01" /> 
	<arg name="max_track_error" default="0.2" />
	<arg name="cam_image_topic" default="/camera/color/image_raw" /> <!--修改为自己发布的图像话题-->
	<arg name="cam_info_topic" default="/camera/color/camera_info" /> <!--修改为自己发布的标定参数话题-->
	<arg name="output_frame" default="/camera_color_optical_frame" /> <!--修改为图片所在的坐标系，关系到后续的坐标系自动转换-->

	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen"
	 args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
</launch>

• 打印一张标签：可以通过此代码生成 rosrun ar_track_alvar createMarker
• 补充下：标签的生成可以包含一些信息，相关帮助说明如下。
• 
• 启动你的摄像头，启动标签检测节点，就可以看到有话题发布出来了，观察标签话题：rostopic echo /visualization_marker
• 也可以在rviz中查看，将/visualization_marker话题可视化：
  

2. 将标签的坐标转换到地图（Map）坐标系下，并加入偏移。

得益于ros中强大的TF树功能，我们只需要将机器人的模型配置好就可以使用其中的TF转换，自动将某个想要的坐标从一个坐标系转换到另一个坐标系下：

• 先看一下效果：图中的红色方块即为识别到的标签坐标，地上的红色大箭头即为经过转换的，想要机器人移动到的目标点坐标，箭头的起始位目标点位置，箭头的朝向为机器人停的朝向。
  
  TF类说明：http://docs.ros.org/lunar/api/tf/html/c++/classtf_1_1TransformListener.html
  TF转换代码：

try{    //采用try-catch的方式可以防止一些意外的崩溃
    listener.transformPose("/map",target_odom_point_tmp,target_odom_point);//将视觉坐标系下的target_odom_point_tmp坐标点转换到/map坐标系
    getflagsuccess = 1;
  }
  catch(tf::TransformException &ex){
    // who care the fuck!
    ros::Duration(0.5).sleep();
    getflagsuccess = 0;
  }
  if(getflagsuccess)//成功转换判断
  {
    tf::Quaternion quat;
    double roll, pitch, yaw;//定义存储r\p\y的容器
    tf::quaternionMsgToTF(target_odom_point.pose.orientation, quat);
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//将四元数姿态表示转换到易于理解的RPY坐标系表示
    yaw +=1.5708;//旋转90
    target_odom_point.pose.position.x -=keep_distance*cos(yaw);//keep_distance表示移动的目标点距离标签的垂直距离，即将当前的x值进行一下偏移
    target_odom_point.pose.position.y -=keep_distance*sin(yaw);
     target_odom_point.pose.position.z = 0;
    target_odom_point.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);//将rpy表示转换为四元数表示
   
 
  odom_point_pub.publish(target_odom_point);//发布一个可视化的目标坐标点，用于debug

3.发送坐标，控制机器人自主导航至目标位置点。

调用 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;动作服务器移动机器人，实现自主导航。

if(global_flag)//判断是否进行移动操作
  {
      global_flag = 0;
      goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position = target_odom_point.pose.position;
    goal.target_pose.pose.orientation = target_odom_point.pose.orientation;//坐标赋值
    ROS_INFO("Sending goal");
     ac.sendGoal(goal);
     // 等待动作服务器完成
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("You have reached the goal1!");
        std_msgs::Bool flag_pub_tmp;
        flag_pub_tmp.data = true;

        int pubcout1=0;
        while(pubcout1 < 100)//在到达指定位置后，发送100次下一段任务指令
        {
           flag_pub.publish(flag_pub_tmp);//发布下一步指令
          pubcout1++;
          ros::Duration(0.01).sleep();
        }
    }
    else
    ROS_INFO("The base move failed for some reason");
  }

4. 记录一下最终的代码：

• 接收了两个话题/visualization_marke(标签坐标) /lets_move（开始移动flag）
• 发布了两个话题/odom_target_point(根据标签计算出的最终移动目标点) /lets_move_finished（移动完成flag）
• 调用了一个动作服务器MoveBaseClient，通过movebase包控制机器人自主导航（成熟的机器人ros产品都会配置好）。

/*
 * go_to_the_marker.cpp
 *
 *  Created on: May 8, 2019
 *      Author: Wxw
 */
#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Bool.h>
#include "tf/transform_datatypes.h"
#include <math.h>

const float PI = 3.14159
const float keep_distance = 1.2;//与目标物体沿其轴向方向的距离

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>  MoveBaseClient;
move_base_msgs::MoveBaseGoal goal;
geometry_msgs::PoseStamped target_odom_point_tmp,target_odom_point;

bool global_flag = 0;//是否开始移动的flag
int cout = 0;//执行记数，即只执行一次移动，在置1后对后续的指示忽略掉


void marker_sub_CB(const visualization_msgs::Marker &marker_tmp)
{
target_odom_point_tmp.header=marker_tmp.header;
target_odom_point_tmp.pose.position=marker_tmp.pose.position;
target_odom_point_tmp.pose.orientation=marker_tmp.pose.orientation;
}
void flag_sub_CB(const std_msgs::Bool &flag_tmp)
{
  if(cout == 0)
  {
    global_flag = flag_tmp.data;
    cout = 1;
  }    
}
    
int main (int argc, char **argv)
{
    ros::init(argc, argv, "go_to_the_marker");
    ros::NodeHandle n;
    MoveBaseClient ac("move_base", true);
    tf::TransformListener listener;
    ros::Rate rate(10);
    ros::Subscriber marker_sub=n.subscribe("/visualization_marker",10,marker_sub_CB);
    ros::Subscriber flag_sub=n.subscribe("/lets_move",1,flag_sub_CB);

    ros::Publisher odom_point_pub = n.advertise<geometry_msgs::PoseStamped>("/odom_target_point",10);
    ros::Publisher flag_pub = n.advertise<std_msgs::Bool>("/lets_move_finished",1);

    // Wait 60 seconds for the action server to become available
    ROS_INFO("Waiting for the move_base action server");
    ac.waitForServer(ros::Duration(60));
    ROS_INFO("Connected to move base server");
    // Send a goal to move_base1
    //目标的属性设置
    bool getflagsuccess = 1;
    ros::Rate r(10);
    
while(ros::ok())
{
  ros::spinOnce();
  try{
    listener.transformPose("/map",target_odom_point_tmp,target_odom_point);
    getflagsuccess = 1;
  }
  catch(tf::TransformException &ex){
    // who care the fuck!
    ros::Duration(0.5).sleep();
    getflagsuccess = 0;
    std::cout<<getflagsuccess<<std::endl;
  }
  if(getflagsuccess)
  {
    tf::Quaternion quat;
    double roll, pitch, yaw;//定义存储r\p\y的容器
    target_odom_point.pose.orientation.x=0;
    target_odom_point.pose.orientation.y=0;

    tf::quaternionMsgToTF(target_odom_point.pose.orientation, quat);
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
    yaw +=1.5708;//旋转90
    target_odom_point.pose.position.x -=keep_distance*cos(yaw);
    target_odom_point.pose.position.y -=keep_distance*sin(yaw);

    target_odom_point.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
    target_odom_point.pose.position.z = 0;
 
  odom_point_pub.publish(target_odom_point);//发布一个可视化的目标坐标点

  if(global_flag)//lets go!
  {
      global_flag = 0;
      goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();
    goal.target_pose.pose.position = target_odom_point.pose.position;
    goal.target_pose.pose.orientation = target_odom_point.pose.orientation;
    ROS_INFO("Sending goal");
     ac.sendGoal(goal);
     // 等待自主导航完成
    ac.waitForResult();
    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("You have reached the goal1!");
        std_msgs::Bool flag_pub_tmp;
        flag_pub_tmp.data = true;

        int pubcout1=0;
        while(pubcout1<100)
        {
           flag_pub.publish(flag_pub_tmp);//发布下一步指令
          pubcout1++;
          ros::Duration(0.01).sleep();
        }
    }
    else
    ROS_INFO("The base failed for some reason");
  }

 
        
  r.sleep();
  }
      
}
    
  return 0;
}