參考鏈接

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中使用的就是自適應的蒙特卡洛定位法。

訂閱的主題

scan (sensor_msgs/LaserScan)
Laser scans.
tf (tf/tfMessage)
Transforms.
initialpose (geometry_msgs/PoseWithCovarianceStamped)
Mean and covariance with which to (re-)initialize the particle filter.
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。

發佈的主題

amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
Robot’s estimated pose in the map, with covariance.
particlecloud (geometry_msgs/PoseArray)
The set of pose estimates being maintained by the filter.
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.

這個代表了你初始化粒子時粒子分佈的一個狀態，注意要把方差設的大一些，要不所有例子上來就是一坨的就沒法玩了。

上圖是粒子的一個初始狀態~我設的方差比較大~所以分佈很大。

上圖是更新了一段時間之後~粒子逐漸趨於穩定

上圖爲最終狀態，趨於穩定

ROS 機器人導航個人資料

1、ROS RVIZ的幾種plan

rviz中各種plan的來源

2、(ros/navigation) navigation導航中一些重要話題

3、make_plan (路線規劃)

4、關於amcl

ros中的amcl

訂閱的主題

發佈的主題

tf tree

example

卡爾曼濾波 -- 從推導到應用轉發

卡爾曼濾波及其他

OPEN DNN Tensorflow ROS

基於深度學習的2D圖像目標檢測

機器人視覺抓取論文及代碼資源

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結