一起做ROS-DEMO:控制移動機器人自主導航並停到標籤(AR Marker)之前

原文連結：

https://blog.csdn.net/qq_23670601/article/details/91612289

在觀看某機器人的視頻時，我們看到了這樣一個場景：

可以看到機器人在檢測到物體的標籤後，自主導航至物體之前，並對標籤物體進行抓取。那麼接下來我想以自己的一個demo個大家分解一下這個任務在ROS下的實現方式。

文末有完整代碼。

文章目錄1. 通過AR Tracker識別標籤。2. 將標籤的坐標轉換到地圖（Map）坐標系下，並加入偏移。3.發送坐標，控制機器人自主導航至目標位置點。4. 記錄一下最終的代碼：1. 通過AR Tracker識別標籤。

<launch>  <ar    g 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話題可視化：
得益於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.14159const 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;//是否開始移動的flagint 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;}
做機器人研發為什麼要用ROS？