ROS navigation调试基础（实现真实机器人导航）

一叶执念

发布时间 2022.03.03阅读数 6928 评论数 0

最近使用了一下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导航功能调优指南中查看。下载地址：

https://download.csdn.net/download/YiYeZhiNian/74781706

ROS navigation 建模仿真路径规划

打赏 0

上一篇：ORB_SLAM2跑笔记本电脑摄像头以及gazebo下单目、双目、RGBD相机仿真

下一篇：Dijkstra算法图文详解

ROS navigation调试基础（实现真实机器人导航）

一叶执念

1、导航包安装

2、仿真环境调试

3、真实机器人调试

为你推荐

FAST-LIO2代码解析(三)

基于Frenet参考路径的高速公路轨迹规划

KITTI数据集Raw Data与Ground Truth序列00-10的对应关系，以及对应的标定参数

激光slam学习笔记１－－RTK组合惯导、激光雷达传感器一些经验知识分享

视觉slam学习之——ch6 非线性曲线拟合（centos系统）

aloam 代码阅读与总结

关于作者

一叶执念

8

0

0

1

如何知道ROS中的话题频率以及如何知道一段代码的运行时间

ORB_SLAM2跑笔记本电脑摄像头以及gazebo下单目、双目、RGBD相机仿真

Dijkstra算法图文详解

相关推荐

四足机器人雷达-视觉导航3：Ego Planning 面向四足机器人的修改

自动驾驶|车道线检测：End-to-end Lane Detection（一）

cartographer 前端两种匹配

精选主流VIO/VSLAM系统改造与工程化落地

经典文献阅读之--Fast-BEV(实时鸟瞰图感知)

精选代码浅析DLIO(一)---整体框架梳理

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

ROS navigation调试基础（实现真实机器人导航）

一叶执念

1、导航包安装

2、仿真环境调试

3、真实机器人调试

为你推荐

FAST-LIO2代码解析(三)

基于Frenet参考路径的高速公路轨迹规划

KITTI数据集Raw Data与Ground Truth序列00-10的对应关系，以及对应的标定参数

激光slam学习笔记１－－RTK组合惯导、激光雷达传感器一些经验知识分享

视觉slam学习之——ch6 非线性曲线拟合（centos系统）

aloam 代码阅读与总结

评论（0）

关于作者

一叶执念

8

0

0

1

如何知道ROS中的话题频率以及如何知道一段代码的运行时间

ORB_SLAM2跑笔记本电脑摄像头以及gazebo下单目、双目、RGBD相机仿真

Dijkstra算法图文详解

相关推荐

四足机器人雷达-视觉导航3：Ego Planning 面向四足机器人的修改

自动驾驶|车道线检测 ：End-to-end Lane Detection（一）

cartographer 前端两种匹配

精选主流VIO/VSLAM系统改造与工程化落地

经典文献阅读之--Fast-BEV(实时鸟瞰图感知)

精选代码浅析DLIO(一)---整体框架梳理

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

自动驾驶|车道线检测：End-to-end Lane Detection（一）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板