Navigation ros 架构图

最近使用了一下ROS中非常经典的导航包navigation。并通过自己的激光雷达以及相机里程计驱动了自己的小车在室内进行简单的定位以及导航。在此记录一下以免后期忘记。

1、导航包安装

ROS中navigation导航包可以通过GitHub上下载：

https://github.com/ros-planning/navigation
https://github.com/ros-planning/navigation_msgs

这里要下载两个文件，前面一个是导航包后面一个是move_base中使用的一些消息类型。该文件下载后放入ROS路径下编译即可。

需要注意的一点是navigation包对应最新更新为noetic版本，kinetic版本在branches中查找：

安装过程会提示缺少一些环境，对应安装一下即可。

2、仿真环境调试

仿真环境调试时使用的是古月居的mbot_navigation下的launch文件以及对应参数。launch文件以及对应参数如下：
nav_cloister_demo.launch

<launch>

    <!-- 设置地图的配置文件 -->
    <arg name="map" default="map38.yaml" />

    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find mbot_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <include file="$(find mbot_navigation)/launch/move_base.launch"/>

    <!-- 启动AMCL节点 -->
    <include file="$(find mbot_navigation)/launch/amcl.launch" />

    <!-- 对于虚拟定位，需要设置一个/odom与/map之间的静态坐标变换 -->
    <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

    <!-- 运行rviz -->
    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find mbot_navigation)/rviz/nav.rviz"/>

</launch>

base_local_planner_params.yaml

controller_frequency: 1.0
recovery_behavior_enabled: false
clearing_rotation_allowed: false

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

   holonomic_robot: false
   yaw_goal_tolerance: 0.1 # about 6 degrees
   xy_goal_tolerance: 0.1  # 10 cm
   latch_xy_goal_tolerance: false
   pdist_scale: 12.0
   gdist_scale: 0.8
   meter_scoring: true

   heading_lookahead: 0.325
   heading_scoring: false
   heading_scoring_timestep: 0.8
   occdist_scale: 0.1
   oscillation_reset_dist: 0.05
   publish_cost_grid_pc: false
   prune_plan: true

   sim_time: 3.0
   sim_granularity: 0.025
   angular_sim_granularity: 0.05
   vx_samples: 6
   vy_samples: 0 # zero for a differential drive robot
   vtheta_samples: 15
   dwa: true
   simple_attractor: false

costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[0.175, 0.175], [0.175, -0.175], [-0.175, -0.175], [-0.175, 0.175]]
footprint_inflation: 0.01
robot_radius: 0.875
inflation_radius: 0.35
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}

global_costmap_params.yaml

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

local_costmap_params.yaml

local_costmap:
   global_frame: odom
   robot_base_frame: base_footprint
   update_frequency: 10.0
   publish_frequency: 10.0
   static_map: false
   rolling_window: true
   width: 6.0
   height: 6.0
   resolution: 0.01
   transform_tolerance: 1.0

机器人为自己搭建的一个安装了laser传感器的小机器人以及一个20x20大小的仿真环境。通过下述命令打开自己的仿真环境：

roslaunch mbot_gazebo mbot_stereo_with_laser.launch

这里根据自己设置的路径打开，这里的仿真环境还有机器人是我自己搭建的，所以可能跟每个人自己的会有所不同，需要根据自己的实际情况修改。需要这个仿真的也可以在最后的下载链接下载。然后打开move_base导航包：

roslaunch mbot_navigation nav_cloister_demo.launch

此时可以看到rviz界面下存在一个机器人以及一张地图，这地图是我之前自己建好的，然后点击rviz界面下的nav_goal给机器人设置一个目标点，此时就可以看到机器人沿着一条规划好的路线朝着终点走过去了。

这里注意一下机器人在真实运动的过程中并不是完全按照全局路径规划所规划的路径朝着终点走过去的，是因为除了全局路径规划外，move_base还存在一个局部路径规划器。机器人真正的速度不是由全局路径规划器发布的而是由局部路径规划器发布的。

全局路径规划器在规划路径时，只是考虑机器人的位置以及要到达的终点，然后使用dijkstra算法或者A*算法查找是否存在一条从起点到终点的路径。

在得到全局路径后，机器人要考虑如何运动到该目标点。因为机器人是一个物体不是一个点，它运动需要考虑一些自身的特性。比如说我现在有一个差速运动的机器人，它在坐标原点朝向Y轴方向，但是我给的目标点为X方向2m处。全局路径规划出来是一条水平长2m的直线，但是我机器人不可能水平沿着这条线运动过来。所以此时就需要在运动的过程中让机器人一边向前走一边向右边转弯，或者先旋转90°再向前运动2m。而这个策略就是来自于局部路径规划器：滑动窗口（DWA）

3、真实机器人调试

仿真的意义在于看效果，这里仿真的效果还是可以的。但是最终效果还是得在自己的机器人上试一下才知道。这里还遇到了几个问题：

首先第一个在于里程计问题，在仿真中机器人是存在一个里程计的，这个里程计不是我们通常理解的编码器信息，而是ROS中定义的一个数据消息。它包含了机器人当前的位置以及角度信息，还有机器人的运动速度和角速度信息。

通过轮子的编码器信息其实也应该能得到这些信息，但是由于之前正好搞过ORB_SLAM所以这里就打算用视觉作为算法的里程计信息来配合使用了。

3.1使用ORB_SLAM作为odom

相机的输出为三维坐标以及四元数角度表示。由于相机坐标系通常与我们使用的机器人坐标系并不相同，所以存在坐标转换的问题。

另外转换后的坐标需要以odom的形式发布出来，这里的发布有两个，一个是给到tf变换的，一个是给到订阅的。所以这里使用了两个文件来实现这两件事情，但其实这里有点问题，一个文件下应该也能完成：
tf变换：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
 
std::string turtle_name;
 
void poseCallback(const geometry_msgs::PoseStamped& msg)
{
     //TF广播器 
     static tf::TransformBroadcaster br;
     
     //根据乌龟当前的位姿。设置相对于世界坐标系的坐标变换
     tf::Transform transform;
     transform.setOrigin(tf::Vector3(msg.pose.position.z,-msg.pose.position.y,0.0));//设置平移变换
     tf::Quaternion q;
     q.setRPY(0,0,0);
     transform.setRotation(tf::Quaternion(msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z,msg.pose.orientation.w));//设置角度变换
 
     //发布坐标变换
     br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"odom",turtle_name));
}
 
 
 
int main(int argc,char** argv)
{
    ros::init(argc, argv, "my_tf_broadcaster");
    if(argc!=2)
    {
        ROS_ERROR("need turtle name as argument");
        return -1;
    }
    turtle_name=argv[1];
 
    ros::NodeHandle node;
    ros::Subscriber sub=node.subscribe("/ORB_SLAM/pose",10,&poseCallback);
    //ros::Subscriber sub=node.subscribe("/slam_out_pose",10,&poseCallback);
 
    ros::spin();
 
    return 0;
}

话题发布：

#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PointStamped.h>
#include "sensor_msgs/LaserScan.h"  	 
#define RAD2DEG(x) ((x)*180./M_PI)
ros::Publisher odom_pub;
//using namespace std;  	 
void scanCallback(const geometry_msgs::PoseStamped& slam_out_pose)
//void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan2)
{
  ros::Time current_time = ros::Time::now();
  float th=0.1;
  geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
  tf::TransformBroadcaster odom_broadcaster;
  geometry_msgs::TransformStamped odom_trans;
  odom_trans.header.stamp = current_time;
  odom_trans.header.frame_id = "odom";
  odom_trans.child_frame_id = "base_footprint";
 
  odom_trans.transform.translation.x = slam_out_pose.pose.position.z;
  odom_trans.transform.translation.y = slam_out_pose.pose.position.x;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = odom_quat;
  //std::cout<<"pose"<<odom_trans.transform.translation.x<<std::endl;
    //odom.pose.pose.orientation.x = 0.0;
    //odom.pose.pose.orientation.y = 0.0;
    //odom.pose.pose.orientation.z = 0.0;
    //odom.pose.pose.orientation.w = 1.0;
 
    //send the transform
  odom_broadcaster.sendTransform(odom_trans);

  nav_msgs::Odometry odom;
  odom.header.stamp = ros::Time::now();
  odom.header.frame_id = "odom";
  
  odom.pose.pose.position.x = slam_out_pose.pose.position.z;
  odom.pose.pose.position.y = -slam_out_pose.pose.position.x;
  odom.pose.pose.position.z = 0.0;
  std::cout<<"pose"<<odom.pose.pose.position.x<<std::endl;
  odom.pose.pose.orientation.x = slam_out_pose.pose.orientation.x;
  odom.pose.pose.orientation.y = slam_out_pose.pose.orientation.y;
  odom.pose.pose.orientation.z = slam_out_pose.pose.orientation.z;
  odom.pose.pose.orientation.w = slam_out_pose.pose.orientation.w;
  odom.child_frame_id = "base_footprint";
  odom.twist.twist.linear.x = 0.05;
  odom.twist.twist.linear.y = 0.05;
  odom.twist.twist.angular.z = 0;


  odom_pub.publish(odom);
}

 	 
int main(int argc, char **argv)
{
  ros::init(argc, argv, "odometery");
  ros::NodeHandle n;
	 
  //ros::Subscriber sub = n.subscribe("slam_out_pose", 10, scanCallback);
  ros::Subscriber sub = n.subscribe("/ORB_SLAM/pose", 10, scanCallback);
  //ros::Subscriber sub = n.subscribe<sensor_msgs::LaserScan>("/scan", 10, scanCallback);
  //ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan3", 50); 
  odom_pub=n.advertise<nav_msgs::Odometry>("odom", 50);	 
  ros::spin();
  	 
  return 0;
}

这两个其实都是通过订阅的回调函数完成的，最后输出的都是视觉的位姿估计信息。然后通过launch文件打开这两个文件：

<launch>
       
    <node pkg="exper4213" type="broadcaster" args="/base_footprint" name="broadcast"/>
    
    <node pkg="exper4213" type="talker" name="talker"/>

    <node pkg="tf" type="static_transform_publisher" name="footprint_base_broadcaster" args="0 0 0 0 0 0 /base_footprint /base_link 100" />
        
</launch>

关于tf变换这里需要注意一下，ROS中的tf变换存在两种，一种静态变换一种动态变换。使用ORB_SLAM作为odom的时候使用的是动态变换。另外在global_costmap_params.yaml文件中robot_base_frame: base_footprint设置的是base_footprint为机器人的位姿，所以这里的odom信息其实是变换到这个坐标系下的。也就是说odom相对于base_footprint进行动态变换。

这里有点不太好理解，在move_base中，map与odom两个坐标系是相对固定的，其他坐标系与机器人一同运动。不知道这里是否可以更改。

另外还有关于odom中的线速度角速度问题，在这里暂时设置为0了，该参数其实是给到DWA使用的，但是设置为0好像影响也不是特别大。

3.2更新频率的问题

这个问题其实在仿真中就存在了，当时没注意，在运动过程中move_base一直报警告：

当时频率设置的是1Hz但是实际上更新时间远远超出了这个时间。不过也能跑跑所以没有太在意，后来算法移植到自己的机器人上面之后，地图更新一次变成了20-30s，控制周期也长达十几秒，完全控制不住。这个部分是来自于可能使用的CPU运算能力问题，但是很大一部分原因是来自于参数设置，当时在上面参数设置的时候将local_costmap_params.yaml下的rolling_window参数设置为了 false。注意这里好像是把局部路径规划下的滑动窗口关闭了，可能导致了一系列奇奇怪怪的问题引起了时间的消耗。将这里修改回来之后仿真时间缩短为原来的十分之一，大概能控制在5Hz左右。

另外除了调整该参数外，其他的参数例如地图分辨率，仿真时间等参数修改也会引起时间变化。最高的时候试过将频率调整为20Hz，再高就没有修改过了。

具体的参数可以在ROS导航功能调优指南中查看。下载地址：