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

在觀看某機器人的視頻時,我們看到了這樣一個場景:
在這裏插入圖片描述

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
  • 補充下:標籤的生成可以包含一些信息,通過 rosrun ar_track_alvar createMarker -h查看相關幫助說明。
  • 啓動你的攝像頭,啓動標籤檢測節點,就可以看到有話題發佈出來了,觀察標籤話題:rostopic echo /visualization_marker
  • 也可以在rviz中查看,將/visualization_marker話題可視化:
    +

2. 將標籤的座標轉換到地圖(Map)座標系下,並加入偏移。

得益於ros中強大的TF樹功能,我們只需要將機器人的模型配置好就可以使用其中的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;
}




發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章