ROS 机器人导航个人资料

参考链接

1. http://wiki.ros.org/move_base#Published_Topics
2. http://wiki.ros.org/navfn
3. ROS与navigation教程
4. ROS Navigation之move_base完全详解

5. 一些关于ROS中move_base的理解

6. ROS探索总结-15.amcl（导航与定位）

7. ROS机器人底盘(18)-如何在实际项目中应用ROS导航相关（1）

8. Why AMCL requires to subscribe to /initialpose topic?

9. C++实现rviz 2D Pose Estimate 功能设置机器人初始座标

10. amcl定位在初始位姿的自动定位

11. 深入AMCL(三):AMCL手动初始化后如何自动定位

12. amcl中手动定位自主定位问题

13. ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom座标转换的方法

14. ros中如何根据map.yaml和tf数据确定地图中机器人的位置

15. ROS及SLAM进阶教程（二）AMCL算法原理讲解

 

 

1、ROS RVIZ的几种plan

本文链接：https://blog.csdn.net/luohuiwu/article/details/93859257

引言：rviz里可以显示几种路径曲线，这里对每种plan的意义进行简单阐释。

rviz中各种plan的来源

• NavFn Plan

  • 显示由机器人的全局规划器产生的路径，如上图中的红色部分。
  • Topic为: /move_base_node/NavfnROS/plan

• Global Plan

  • 显示由局部规划器追踪的路径，为全局路径的一部分。如上图中的黄色部分。
  • Topic: /move_base_node/TrajectoryPlannerROS/global_plan

• Local Plan

  • 显示速度指令为底座生成的执行路径。如上图中的蓝色部分。
  • Topic: /move_base_node/TrajectoryPlannerROS/local_plan

•                                              导航
• ROS

2、(ros/navigation) navigation导航中一些重要话题

topic1:  cmd_vel
       Type: geometry_msgs/Twist
   Publishers:  * /move_base  
Subscribers:  * none

topic2:  /move_base/NavfnROS/plan 
Type: nav_msgs/Path
Publishers:  * /move_base  
Subscribers:  * /rviz  

topic3:   /move_base/DWAPlannerROS/global_plan 
Type: nav_msgs/Path
Publishers:  * /move_base  
Subscribers:  * /rviz  

topic4:  /move_base/DWAPlannerROS/local_plan 
Type: nav_msgs/Path
Publishers:  * /move_base  
Subscribers:  * /rviz  

topic5:  /move_base/goal 
Type: move_base_msgs/MoveBaseActionGoal
Publishers:  * /move_base  
Subscribers:  * /move_base  

topic6:   /move_base_simple/goal 
Type: geometry_msgs/PoseStamped
Publishers:  * /rviz  
Subscribers:  * /move_base   * /rviz  
  
topic7:   /move_base/status 
Type: actionlib_msgs/GoalStatusArray
Publishers:  * /move_base  

topic8:   /tf
Type: tf2_msgs/TFMessage
Publishers: 
 * /odometry  
 * /robot_tf_publisher 
 * /amcl  

Subscribers: 
 * /amcl  
 * /move_base  
 * /rviz  

3、make_plan (路线规划)

geometry_msgs::PoseStamped Start;
    Start.header.seq = 0;
    Start.header.stamp = Time(0);
    Start.header.frame_id = "map";
    Start.pose.position.x = x1;
    Start.pose.position.y = y1;
    Start.pose.position.z = 0.0;
    Start.pose.orientation.x = 0.0;
    Start.pose.orientation.y = 0.0;
    Start.pose.orientation.w = 1.0;

geometry_msgs::PoseStamped Goal;
    Goal.header.seq = 0;
    Goal.header.stamp = Time(0);
    Goal.header.frame_id = "map";
    Goal.pose.position.x = x2;
    Goal.pose.position.y = y2;
    Goal.pose.position.z = 0.0;
    Goal.pose.orientation.x = 0.0;
    Goal.pose.orientation.y = 0.0;
    Goal.pose.orientation.w = 1.0;

ServiceClient check_path = nh_.serviceClient<nav_msgs::GetPlan>("make_plan");
    nav_msgs::GetPlan srv;
    srv.request.start = Start;
    srv.request.goal = Goal;
    srv.request.tolerance = 1.5;

    ROS_INFO("Make plan: %d", (check_path.call(srv) ? 1 : 0));
    ROS_INFO("Plan size: %d", srv.response.plan.poses.size());



move_base_msgs::MoveBaseGoal move_goal;
move_goal.target_pose.header.frame_id = "map";
move_goal.target_pose.header.stamp = Time(0);

move_goal.target_pose.pose.position.x = x;
move_goal.target_pose.pose.position.y = y;
move_goal.target_pose.pose.position.z = 0.0;
move_goal.target_pose.pose.orientation.x = 0.0;
move_goal.target_pose.pose.orientation.y = 0.0;
move_goal.target_pose.pose.orientation.w = 1.0;

ROS_INFO("Sending goal");
ac_.sendGoal(move_goal,boost::bind(&ExploreAction::doneCb, this, _1, _2));
 

/*
 * make_plan.cpp
 *
 *  Created on: Aug 10, 2016
 *      Author: unicorn
 */
//路线规划代码
#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH
void fillPathRequest(nav_msgs::GetPlan::Request &request)
{
request.start.header.frame_id ="map";
request.start.pose.position.x = 12.378;//初始位置x座标
request.start.pose.position.y = 28.638;//初始位置y座标
request.start.pose.orientation.w = 1.0;//方向
request.goal.header.frame_id = "map";
request.goal.pose.position.x = 18.792;//终点座标
request.goal.pose.position.y = 29.544;
request.goal.pose.orientation.w = 1.0;
request.tolerance = 0.5;//如果不能到达目标，最近可到的约束
}
//路线规划结果回调
void callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv)
{
// Perform the actual path planner call
//执行实际路径规划器
if (serviceClient.call(srv)) {
//srv.response.plan.poses 为保存结果的容器，遍历取出
if (!srv.response.plan.poses.empty()) {
forEach(const geometry_msgs::PoseStamped &p, srv.response.plan.poses) {
ROS_INFO("x = %f, y = %f", p.pose.position.x, p.pose.position.y);
}
}
else {
ROS_WARN("Got empty plan");
}
}
else {
ROS_ERROR("Failed to call service %s - is the robot moving?",
serviceClient.getService().c_str());
}
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "make_plan_node");
ros::NodeHandle nh;
// Init service query for make plan
//初始化路径规划服务，服务名称为"move_base_node/make_plan"
std::string service_name = "move_base_node/make_plan";
//等待服务空闲，如果已经在运行这个服务，会等到运行结束。
while (!ros::service::waitForService(service_name, ros::Duration(3.0))) {
ROS_INFO("Waiting for service move_base/make_plan to become available");
}
/*初始化客户端，(nav_msgs/GetPlan)
Allows an external user to ask for a plan to a given pose from move_base without causing move_base to execute that plan.
允许用户从move_base 请求一个plan，并不会导致move_base 执行此plan
*/ 
ros::ServiceClient serviceClient = nh.serviceClient<nav_msgs::GetPlan>(service_name, true);
if (!serviceClient) {
ROS_FATAL("Could not initialize get plan service from %s",
serviceClient.getService().c_str());
return -1;
}
nav_msgs::GetPlan srv;
//请求服务：规划路线
fillPathRequest(srv.request);
if (!serviceClient) {
ROS_FATAL("Persistent service connection to %s failed",
serviceClient.getService().c_str());
return -1;
}
ROS_INFO("conntect to %s",serviceClient.getService().c_str());
callPlanningService(serviceClient, srv);
}

/*
 * send_goal.cpp
 *
 *  Created on: Aug 10, 2016
 *      Author: unicorn
 */

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
/*move_base_msgs::MoveBaseAction
 move_base在world中的目标
*/ 
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>  MoveBaseClient;
int main(int argc, char** argv) {
ros::init(argc, argv, "send_goals_node");

/*
// create the action client
// true causes the client to spin its own thread
//don't need ros::spin()
创建action客户端，参数1：action名，参数2：true，不需要手动调用ros::spin()，会在它的线程中自动调用。
*/
MoveBaseClient ac("move_base", true);
// 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_base
//目标的属性设置
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = 21.174;
goal.target_pose.pose.position.y = 10.876;
goal.target_pose.pose.orientation.w = 1;
ROS_INFO("");
ROS_INFO("Sending goal");
ac.sendGoal(goal);
// Wait for the action to return
ac.waitForResult();
if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("You have reached the goal!");
else
ROS_INFO("The base failed for some reason");
return 0;
}

4、关于amcl

amcl的英文全称是adaptive Monte Carlo localization，其实就是蒙特卡洛定位方法的一种升级版，使用自适应的KLD方法来更新粒子，这里不再多说（主要我也不熟），有兴趣的可以去看：KLD。 
而mcl（蒙特卡洛定位）法使用的是粒子滤波的方法来进行定位的。而粒子滤波很粗浅的说就是一开始在地图空间很均匀的撒一把粒子，然后通过获取机器人的motion来移动粒子，比如机器人向前移动了一米，所有的粒子也就向前移动一米，不管现在这个粒子的位置对不对。使用每个粒子所处位置模拟一个传感器信息跟观察到的传感器信息（一般是激光）作对比，从而赋给每个粒子一个概率。之后根据生成的概率来重新生成粒子，概率越高的生成的概率越大。这样的迭代之后，所有的粒子会慢慢地收敛到一起，机器人的确切位置也就被推算出来了。 

这幅图模拟了一个一维机器人的粒子更新，机器人下面那些想条形码一样的竖条就是粒子的分布了，可以看到粒子随着机器人的移动与更新会逐渐的收敛到机器人的正确位置上。 
mcl算法步骤图： 

wiki链接

ros中的amcl

ros中使用的就是自适应的蒙特卡洛定位法。

订阅的主题

1. scan (sensor_msgs/LaserScan) 
   Laser scans.
2. tf (tf/tfMessage) 
   Transforms.
3. initialpose (geometry_msgs/PoseWithCovarianceStamped) 
   Mean and covariance with which to (re-)initialize the particle filter.
4. map (nav_msgs/OccupancyGrid) 
   When the use_map_topic parameter is set, AMCL subscribes to this topic to retrieve the map used for laser-based localization. New in navigation 1.4.2. 
   实际上初始位姿可以通过参数提供也可以使用默认初始值，我们主要是要将scan（激光）、tf和map主题提供给amcl。

发布的主题

1. amcl_pose (geometry_msgs/PoseWithCovarianceStamped) 
   Robot’s estimated pose in the map, with covariance.
2. particlecloud (geometry_msgs/PoseArray) 
   The set of pose estimates being maintained by the filter.
3. tf (tf/tfMessage) 
   Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map. 
   如果纯粹是为了显示一下机器人的位姿（in rviz）我们只需要tf主题就够了。

tf tree

我们需要将base_link，odom，map三个frame连接起来。 

上图是我自己的tf连接图

example

<?xml version="1.0"?>
<launch>
   <!-->
  <node pkg="beginner_tutorials" type="talker" name="talker"/>
   <-->
  <node pkg="map_server" type="map_server" name="map_server" args="/home/zqq/map.yaml"/>

  <!-- amcl node -->
  <node pkg="amcl" type="amcl" name="amcl" output="screen">

  <remap from="scan" to="scan"/>
  <!-- Publish scans from best pose at a max of 10 Hz -->
  <param name="use_map_topic" value="true"/>
  <param name="odom_model_type" value="omni"/>
  <param name="odom_alpha5" value="0.1"/>
  <param name="transform_tolerance" value="0.5" />
  <param name="gui_publish_rate" value="10.0"/>
  <param name="laser_max_beams" value="300"/>
  <param name="min_particles" value="500"/>
  <param name="max_particles" value="5000"/>
  <param name="kld_err" value="0.1"/>
  <param name="kld_z" value="0.99"/>
  <param name="odom_alpha1" value="0.1"/>
  <param name="odom_alpha2" value="0.1"/>
  <!-- translation std dev, m -->
  <param name="odom_alpha3" value="0.1"/>
  <param name="odom_alpha4" value="0.1"/>
  <param name="laser_z_hit" value="0.9"/>
  <param name="laser_z_short" value="0.05"/>
  <param name="laser_z_max" value="0.05"/>
  <param name="laser_z_rand" value="0.5"/>
  <param name="laser_sigma_hit" value="0.2"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_lambda_short" value="0.1"/>
  <param name="laser_model_type" value="likelihood_field"/>
  <!-- <param name="laser_model_type" value="beam"/> -->
 <param name="laser_min_range" value="1"/>
<param name="laser_max_range" value="5"/>
  <param name="laser_likelihood_max_dist" value="2.0"/>
  <param name="update_min_d" value="0.2"/>
  <param name="update_min_a" value="0.5"/>
  <param name="resample_interval" value="1"/>
  <param name="transform_tolerance" value="0.1"/>
  <param name="recovery_alpha_slow" value="0.0"/>
  <param name="recovery_alpha_fast" value="0.0"/>


</node>

</launch>

launch文件，分别调用了map_server节点和amcl节点，map_server节点读取了一个我自己之前用gmapping制作的地图（详细教程：戳这里），之后调用amcl节点，订阅了scan激光主题，设置了一些参数，参数详细作用：戳这里。 
注意一定要将tf tree设置好！！坑了大部分人的都是这个tf。 

tf显示的就是当前的机器人位姿。 
注意：要将rviz的fixed frame设成map，因为map才是global_frame_id。

---

rviz居然连粒子都可以显示，显示让我对amcl粒子更新有了更深刻的理解。 
首先在参数表里面有几个比较重要的参数。 
~initial_pose_x (double, default: 0.0 meters) 
Initial pose mean (x), used to initialize filter with Gaussian distribution. 
~initial_pose_y (double, default: 0.0 meters) 
Initial pose mean (y), used to initialize filter with Gaussian distribution. 
~initial_pose_a (double, default: 0.0 radians) 
Initial pose mean (yaw), used to initialize filter with Gaussian distribution. 
~initial_cov_xx (double, default: 0.5*0.5 meters) 
Initial pose covariance (x*x), used to initialize filter with Gaussian distribution. 
~initial_cov_yy (double, default: 0.5*0.5 meters) 
Initial pose covariance (y*y), used to initialize filter with Gaussian distribution. 
~initial_cov_aa (double, default: (π/12)*(π/12) radian) 
Initial pose covariance (yaw*yaw), used to initialize filter with Gaussian distribution.

这个代表了你初始化粒子时粒子分布的一个状态，注意要把方差设的大一些，要不所有例子上来就是一坨的就没法玩了。 

上图是粒子的一个初始状态~我设的方差比较大~所以分布很大。 

上图是更新了一段时间之后~粒子逐渐趋于稳定 

上图为最终状态，趋于稳定