天天看點
傳回

學習move_base包

系統版本：Ubuntu14.04，ROS indigo

機器人底盤：kobuki

攝像頭：Asus Xtion

程式設計語言：C++

參考書本：《ROS by Example》 《Learning ROS for Robotics Programming》

　　在上一篇博文，我們監控/odom坐标和/base_footprint坐标之間的tf變換，進而跟蹤機器人移動的距離和旋轉的角度。ROS提供更加簡潔的方法即是使用move_base包也能實作一樣的效果。其中move_base包使用ROS action工具來達到一個給定的導航目标點。當選擇機器人行走的路徑時，move_base包合并base_local_planner基本局部規劃器、全局代價地圖和局部代價地圖的裡程資料。

　　參考資料：

move_base Wiki：http://wiki.ros.org/move_base

actionlib Wiki：http://wiki.ros.org/actionlib

　　為了使用move_base制定一個導航目标，我們提供一個相對特定參考坐标的機器人的目标姿态（位置和方向）。move_base包使用MoveBaseActionGoal消息類型來指定目标。運作一下指令檢視MoveBaseActionGoal消息類,如圖1。

rosmsg show MoveBaseActionGoal
學習move_base包

圖1

　　可見，目标由标準ROS header、goal_id和goal本身組成。goal裡的PoseStamped消息類型依次由header和pose組成，其中pose包括了position和orientation。

　　接下來使用move_base導航一個正方形。

建立功能包 

catkin_create_pkg robot_navigation actionlib geometry_msgs move_base_msgs visualization_msgs roscpp tf

建立機器人配置

　　在robot_navigation/launch檔案夾下fake_move_base_blank_map.launch為名建立一個檔案，并添加一下代碼：

<launch>
  <!-- Run the map server with a blank map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/blank_map.yaml"/>

  <include file="$(find robot_navigation)/launch/fake_move_base.launch" />

  <!-- Run a static transform between /odom and /map -->
  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />
</launch>

　　此.launch檔案的解釋：首先我們先運作帶有參數blank_map.yaml空白地圖的ROS map_server節點。.yaml檔案描述地圖本身的大小和分辨率。接下來的第二行指令包含可 fake_move_base.launch檔案，此檔案是運作move_base節點和加載所有必要配置參數以使仿真機器人工作得更好。最後，因為我們使用一個空白的地圖和我們的仿真機器人沒有傳感器，機器人無法為定位擷取掃描資料。取而代之，假設量程是正确的，我們簡單設定一個靜态身份轉換，使用機器人的量程坐标轉換成地圖map坐标。

　　以下是fake_move_base.launch檔案内容：

<launch>
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <rosparam file="$(find robot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find robot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find robot_navigation)/config/fake/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_navigation)/config/fake/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_navigation)/config/fake/base_local_planner_params.yaml" command="load" />
  </node>
</launch>

　　 此launch檔案運作move_base節點并五次回調rosparam加載.yaml檔案。加載兩次costmap_common_params.yaml檔案的原因是使用global_costmap命名空間和local_costmap命名空間來設定那些共同參數。

參數配置

　　根據fake_move_base.launch檔案内容，在robot_navigation/config/fake檔案夾下分别建立costmap_common_params.yaml，local_costmap_params.yaml，global_costmap_params.yaml，base_local_planner_params.yaml這四個配置檔案。

　　costmap_common_params.yaml檔案内容如下：

obstacle_range: 2.5
raytrace_range: 3.0

robot_radius: 0.175
inflation_radius: 0.2
max_obstacle_height: 0.6
min_obstacle_height: 0.0
observation_sources: scan
scan: {data_type: LaserScan, topic: /scan, marking: true, clearing: true, expected_update_rate: 0}

　　 這個檔案主要用于配置基本參數。這些參數會被用于local_costmap和global_costmap。obstacle_range和raytrace_range參數用于表示傳感器的最大探測距離并在代價地圖中引入探測的障礙物。前者主要用于障礙的探測。在此示例中，如果機器人檢測到一個距離小于2.5的障礙物，就會将這個障礙物引入到代價地圖中。後者則用于在機器人運動過程中，實時清除代價地圖中的障礙物，并更新可移動的自由空間資料。

　　robot_radius表示機器人的半徑大小。inflation_radius表示機器人與障礙物之間必須保持的最小距離。max_obstacle_height表示傳感器讀取最大高度被視為有效，如果此參數的值比全局max_obstacle_height的值低，則會過濾掉此高度以上的點。min_obstacle_height表示傳感器讀取最小高度被視為有效。observation_sources設定導航功能包集所使用的傳感器。

　　local_costmap_params.yaml檔案内容如下：

local_costmap:
   global_frame: /odom
   robot_base_frame: /base_footprint
   update_frequency: 
   publish_frequency: 
   static_map: true
   rolling_window: false
   width: 
   height: 
   resolution: 
   transform_tolerance:

　　global_frame: /odom表示對于局部代價地圖，使用量程坐标作為全局坐标。對于TurtleBot或者kobuki的機器人參考坐标robot_base_frame是/base_footprint。update_frequency表示基于傳感器資料更新局部地圖的頻率。publish_frequency表示釋出資訊的頻率。rolling_window用于配置在機器人運動過程中，代價地圖始終以機器人為中心。width、height和resolution參數來配置代價地圖的尺寸和分辨率，以米為機關。transform_tolerance表示在tf樹坐标之間轉換的允許延時或者繪圖過程臨時被中止的允許延時，以秒為機關。

　　global_costmap_params.yaml檔案内容如下：

global_costmap:
   global_frame: /map
   robot_base_frame: /base_footprint
   update_frequency: 
   publish_frequency: 
   static_map: true
   rolling_window: false
   resolution: 
   transform_tolerance: 
   map_type: costmap

　　global_frame: /map表示對于全局代價地圖，使用/map作為全局坐标。map_type有兩種類型：“voxel”和“costmap”，“voxel”表示3D視圖，“costmap”表示2D視圖。

　　base_local_planner_params.yaml檔案内容如下：

controller_frequency: 3.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false

TrajectoryPlannerROS:
   max_vel_x: 
   min_vel_x: 
   max_vel_y:   # zero for a differential drive robot
   min_vel_y: 
   max_vel_theta: 
   min_vel_theta: -
   min_in_place_vel_theta: 
   escape_vel: -
   acc_lim_x: 
   acc_lim_y:   # zero for a differential drive robot
   acc_lim_theta: 

   holonomic_robot: false
   yaw_goal_tolerance:  # about 6 degrees
   xy_goal_tolerance:   # 5 cm
   latch_xy_goal_tolerance: false
   pdist_scale: 
   gdist_scale: 
   meter_scoring: true

   heading_lookahead: 
   heading_scoring: false
   heading_scoring_timestep: 
   occdist_scale: 
   oscillation_reset_dist: 
   publish_cost_grid_pc: false
   prune_plan: true

   sim_time: 
   sim_granularity: 
   angular_sim_granularity: 
   vx_samples: 
   vy_samples:   # zero for a differential drive robot
   vtheta_samples: 
   dwa: true
   simple_attractor: false

　　controller_frequency表示更新規劃程序的頻率，每秒多少次。max_vel_x表示機器人最大線速度，機關是m/s。min_vel_x表示機器人最小線速度。max_vel_y和min_vel_y對于兩輪差分驅動機器人是無效的。max_vel_theta表示機器人最大轉動速度，機關是rad/s。min_vel_theta表示機器人最小轉動速度。min_in_place_vel_theta表示機器人最小原地旋轉速度。escape_vel表示機器人的逃離速度，即背向相反方向行走速度，機關是m/s。acc_lim_x表示在x方向上最大的線加速度，機關是m/s^2。 acc_lim_theta表示最大角加速度，機關是rad/s^2。holonomic_robot：除非擁有一個全方位的機器人，否則設定為false。yaw_goal_tolerance表示接近目标方向（就弧度來說）允許的誤差（rad），此值太小會造成機器人在目标附近震蕩。xy_goal_tolerance表示接近目标允許的誤差（m），此值設定太小，機器人會沒完沒了在目标位置周圍做小調整，而且此值不能小于地圖的分辨率。pdist_scale表示緊貼全局路徑比到達目标的相對重要性，如果此值比gdist_scale大，那麼機器人會更緊靠全局路徑行走。gdist_scale表示到達目标比緊靠全局路徑的相對重要性，如果此值比pdist_scale大，那麼機器人會采取任意路徑優先到達目标。meter_scoring表示gdist_scale和pdist_scale參數是否假設goal_distance和path_distance以米或者單元來表達。occdist_scale表示控制器嘗試避開障礙物的權重。sim_time表示規劃器能看到未來多少秒。dwa表示當模拟軌迹走向未來，是否使用動态視窗法。

　　在robot_navigation/launch檔案夾建立名為view_nav.launch，用于啟動rviz，内容如下：

<launch>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_navigation)/rviz/nav.rviz" />
</launch>

使用move_base導航一個正方形

1.在robot_navigation/src檔案夾下建立move_base_square.cpp為名的檔案，并複制以下代碼： 

#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <visualization_msgs/Marker.h>
#include <cmath>

ros::Publisher cmdVelPub;
ros::Publisher marker_pub;

void shutdown(int sig)
{
  cmdVelPub.publish(geometry_msgs::Twist());
  ros::Duration().sleep(); // sleep for  a second
  ROS_INFO("nav_square.cpp ended!");
  ros::shutdown();
}

void init_markers(visualization_msgs::Marker *marker)
{
  marker->ns       = "waypoints";
  marker->id       = ;
  marker->type     = visualization_msgs::Marker::CUBE_LIST;
  marker->action   = visualization_msgs::Marker::ADD;
  marker->lifetime = ros::Duration();//0 is forever
  marker->scale.x  = ;
  marker->scale.y  = ;
  marker->color.r  = ;
  marker->color.g  = ;
  marker->color.b  = ;
  marker->color.a  = ;

  marker->header.frame_id = "odom";
  marker->header.stamp = ros::Time::now();

}
int main(int argc, char** argv)
{
  ros::init(argc, argv, "nav_move_base");
  std::string topic = "/cmd_vel";
  ros::NodeHandle node;
  //Subscribe to the move_base action server
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

  //Define a marker publisher.
  marker_pub = node.advertise<visualization_msgs::Marker>("waypoint_markers", );

  //for init_markers function
  visualization_msgs::Marker  line_list;

  signal(SIGINT, shutdown);
  ROS_INFO("move_base_square.cpp start...");

  //How big is the square we want the robot to navigate?
  double square_size = ;

  //Create a list to hold the target quaternions (orientations)
  geometry_msgs::Quaternion quaternions[];

  //convert the angles to quaternions
  double angle = M_PI/;
  int angle_count = ;
  for(angle_count = ; angle_count < ;angle_count++ )
  {
      quaternions[angle_count] = tf::createQuaternionMsgFromRollPitchYaw(, , angle);
      angle = angle + M_PI/;
  }

  //a pose consisting of a position and orientation in the map frame.
  geometry_msgs::Point point;
  geometry_msgs::Pose pose_list[];
  point.x = square_size;
  point.y = ;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

  point.x = square_size;
  point.y = square_size;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

  point.x = ;
  point.y = square_size;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

  point.x = ;
  point.y = ;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

  //Initialize the visualization markers for RViz
  init_markers(&line_list);

  //Set a visualization marker at each waypoint
  for(int i = ; i < ; i++)
  {
    line_list.points.push_back(pose_list[i].position);
  }

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



  ROS_INFO("Waiting for move_base action server...");
  //Wait 60 seconds for the action server to become available
  if(!ac.waitForServer(ros::Duration()))
  {
    ROS_INFO("Can't connected to move base server");
    return ;
  }
  ROS_INFO("Connected to move base server");
  ROS_INFO("Starting navigation test");

  //Initialize a counter to track waypoints
  int count = ;
  //Cycle through the four waypoints
  while( (count < ) && (ros::ok()) )
  {
     //Update the marker display
     marker_pub.publish(line_list);

     //Intialize the waypoint goal
     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
     //Send the goal pose to the MoveBaseAction server
     ac.sendGoal(goal);

    //Allow 1 minute to get there
    bool finished_within_time = ac.waitForResult(ros::Duration());

    //If we dont get there in time, abort the goal
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
     count += ;
  }
  ROS_INFO("move_base_square.cpp end...");
  return ;
}

2.修改robot_navigation目錄下的CMakeLists.txt

在CMakeLists.txt檔案末尾加入幾條語句:

add_executable(move_base_square src/move_base_square.cpp)
target_link_libraries(move_base_square ${catkin_LIBRARIES})

3.編譯程式

在catkin_ws目錄下，執行一下指令編譯：

catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles" -DCMAKE_BUILD_TYPE=Debug -DCMAKE_ECLIPSE_MAKE_ARGUMENTS=-j8

得到move_base_square執行程式。

　　為何不直接執行catkin_make編譯？因為執行指令也可以讓Eclipse識别該工程，友善在Eclipse編寫調試代碼。執行過一次，以後隻需catkin_make編譯，Eclipse也能識别。

4.測試程式

4.1 啟動roscore 

roscore

4.2 啟動仿真機器人

4.3 啟動帶有空白地圖的map_server節點，啟動move_base節點并加載全局和局部代價地圖等配置檔案

4.4 啟動 rviz 圖形化顯示程式

　　view_nav.launch的内容如下：

<launch>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_navigation)/rviz/nav.rviz" />
</launch>

4.5 啟動move_base_square程式，效果如圖2 

rosrun robot_navigation move_base_square
學習move_base包

圖2

部分代碼解釋 

//convert the angles to quaternions
  double angle = M_PI/;
  int angle_count = ;
  for(angle_count = ; angle_count < ;angle_count++ )
  {
      quaternions[angle_count] = tf::createQuaternionMsgFromRollPitchYaw(, , angle);
      angle = angle + M_PI/;
  }

　　在正方形四個角定義了目标方向。分别将四個角度值轉換成四元數。

/a pose consisting of a position and orientation in the map frame.
  geometry_msgs::Point point;
  geometry_msgs::Pose pose_list[];
  point.x = square_size;
  point.y = ;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

  point.x = square_size;
  point.y = square_size;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

  point.x = ;
  point.y = square_size;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

  point.x = ;
  point.y = ;
  point.z = ;
  pose_list[].position = point;
  pose_list[].orientation = quaternions[];

　　 建立四個航點的姿勢，包括四個角的坐标和方向。

//Initialize the visualization markers for RViz
  init_markers(&line_list);

  //Set a visualization marker at each waypoint
  for(int i = ; i < ; i++)
  {
    line_list.points.push_back(pose_list[i].position);
  }

　　在RViz上設定虛拟标記是相當簡單的，教程位址：http://wiki.ros.org/rviz/Tutorials

　　此例程在每個目标角落放置四個紅色矩形。init_markers(&line_list)此函數在程式的前面有定義，主要設定标記形狀、大小、和顔色。然後line_list清單存放四個矩形的坐标。

//Subscribe to the move_base action server
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

　　定義一個SimpleActionClient用于發送目标給move_base動作伺服器。

//Wait 60 seconds for the action server to become available
  if(!ac.waitForServer(ros::Duration()))
  {
    ROS_INFO("Can't connected to move base server");
    return 1;
  }

　　在開始發送目标之前，我們必須等到move_base動作伺服器變成有效。這裡是等待60秒。

//Cycle through the four waypoints
  while( (count < ) && (ros::ok()) )
  {
     //Update the marker display
     marker_pub.publish(line_list);

     //Intialize the waypoint goal
     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
     //Send the goal pose to the MoveBaseAction server
     ac.sendGoal(goal);

    //Allow 1 minute to get there
    bool finished_within_time = ac.waitForResult(ros::Duration());

    //If we dont get there in time, abort the goal
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
     count += ;
  }

　　首先我們釋出标記表明四個目标姿勢，每次循環都要執行，使标記貫穿整個移動過程。接着我們初始化MoveBaseGoal動作類型的goal變量。設定目标的frame_id為map坐标和時間戳為現在時間。設定目标姿勢為現在的航點，并且發送目标給move_base動作伺服器，等待180秒，傳回true表明機器人在180秒内達到航點，傳回flase表明沒有達到航點。

仿真躲避障礙物的例子

1 啟動roscore 

roscore

2 啟動仿真機器人

3 啟動加載障礙物地圖的map_server節點，啟動move_base節點并加載全局和局部代價地圖等配置檔案

此.launch另外運作fake_move_base_obstacles.launch,

fake_move_base_obstacles.launch内容：

<launch>

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true"> 
    <rosparam file="$(find robot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find robot_navigation)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find robot_navigation)/config/fake/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_navigation)/config/fake/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find robot_navigation)/config/fake/base_local_planner_params.yaml" command="load" />
    <rosparam file="$(find robot_navigation)/config/nav_obstacles_params.yaml" command="load" />

  </node>

</launch>

其中nav_obstacles_params.yaml内容如下：

TrajectoryPlannerROS:
  max_vel_x: 
  pdist_scale: 
  gdist_scale:

　　其實這些參數已經在base_local_planner_params.yaml檔案用過，但設定這些參數為這些值為了更好地避開障礙物。此檔案将最大速度從0.5m/s降到0.3m/s，也颠倒了pdist_scale和gdist_scale的相關權重，是因為更重視跟随計劃的路徑行走。本來也可以建立一個新的base_local_planner_params.yaml包括這些新的改變，但是通過這種方法我們可以在主要的檔案保留大部分的參數，根據類似nav_obstacles_params.yaml這樣特殊檔案的需要來覆寫相應的參數。

4 啟動 rviz 圖形化顯示程式

　　view_nav_obstacles.launch的内容如下：

<launch>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find robot_navigation)/rviz/nav_obstacles.rviz" />
</launch>

5 啟動move_base_square程式，效果如圖3 

rosrun robot_navigation move_base_square
學習move_base包

圖3

在真正的機器人上測試避開障礙物

1 啟動roscore 

roscore

2 啟動機器人

3 啟動RGBD攝像頭

4 啟動帶有地圖的map_server節點，move_base節點，以及啟動配置全局和局部代價地圖

　　注意：此.launch檔案不同于之前的仿真機器人的.launch檔案。.yaml配置檔案是參考《ROS by Example》關于TurtleBot的.yaml配置檔案。在robot_navigation/config/turtlebot/目錄下，也有四個.yaml配置檔案。與《ROS by Example》的相比，主要修改一下幾個參數：

base_local_planner_params.yaml：
 pdist_scale: 
 gdist_scale:

　　我自己經過多次測試，發現如果pdist_scale參數值比gdist_scale參數值大，即是更重視跟随規劃路徑行走，有時機器人會在障礙物旁邊不斷旋轉，也總是盡可能貼近障礙物。為了避免剛才所說的情況，将pdist_scale參數值設定得比gdist_scale參數值小，即是更重視到達目的地，機器人能很順利的通過障礙物并達到目的地。原因？有待探讨。

costmap_common_params.yaml:
robot_radius: 
inflation_radius:

　　将機器人的半徑設定更大一些，機器人與障礙物之間距離設定大一些，目的為了防止筆記本電腦與機器人發生碰撞。

　　其他參數值請檢視源代碼。

5 啟動 rviz 圖形化顯示程式

6 啟動move_base_square程式 

rosrun robot_navigation move_base_square

源代碼位址：https://github.com/KeoChi/robot_navigation

個人學習筆記，歡迎交流學習。

ros move-base indigo
上一篇: datax(8):TaskGroupContainer源碼解讀
下一篇: Ajax 請求的五大步驟

繼續閱讀