ROS目标跟随（路径规划、雷达、slam、定位）

最终效果展示

ros

这里的效果还是可以继续改进的，比如落点的位置可以进行调整，这样可以实现始终跟随在屁股背后，我已经实现可以在我的主页中找到，这里实现的仅仅是最简单的能跟上。后续想改进的话可以继续调整机器人的结构，参数等等，因为我发现有时候连直线都走不好，这应该和结构参数设置有关系，另一方面还可以跟换路径规划的算法以实现最短路径什么的。
改善后的在本篇博客：https://blog.csdn.net/m0_71523511/article/details/135675446

一、总体launch文件

1、打开已有地图

上述launch文件中包含了其它许多文件，放在下面：
nav_demo/launch/nav03_readmap.launch，打开已保存地图（创建和保存地图可以直接看文档教程7.2.1，7.2.2）：

2、组合小车的各个部分

urdf02_gazebo/urdf/car.urdf.xacro，组合小车的各个部分（这里只给出机器人1的，即进行跟随的小车）：

2.1惯性矩阵设置

head.xacro（这下面这几个全都在urdf02_gazebo/gazebo目录下）：

2.2小车底盘

demo05_car_base.urdf.xacro：

    
    
    
    
    
        
    
    
     
     
     
     
     

    
    
      
        
          
        
      
    

    
      
        
          
        
        
        
          
        
      
      
        
          
        
        
      
      
    

    
      
      
      
    
    
        Gazebo/Yellow
    

    
    
    
    
     

    
    
      
        
          
            
          
          
          
        
        
          
            
          
          
        
        

      

      
        
        
        
        
      

      
        Gazebo/Red
      

    
    
    
    
    
     
     

    
    
      
        
            
                
            
            
            
        
        
            
                
            
            
        
        
      

      
          
          
          
          
      
      
        Gazebo/Red
      
    

    
    

2.3摄像头

demo06_car_camera.urdf.xacro：

    
     
     
     
     
     
     

     

    
    
        
            
                
            
            
            
        
        
            
                
            
            
        
        
    

    
        
        
        
    
    
        Gazebo/Blue
    

2.4雷达

demo07_car_laser.urdf.xacro：

    
     
     
     
     
     

     

    
        
            
                
            
            
            
                
            
        

        
            
                
            
            
        

        

    

    
        
        
        
    

    
        Gazebo/White
    

    
     
     
     
     
     

     

    
    
        
            
                
            
            
            
        
        
            
                
            
            
        
        
    

    
        
        
        
    
    
        Gazebo/Black
    

2.5、gazebo/move.xacro：


    
    
        
        
            transmission_interface/SimpleTransmission
            
                hardware_interface/VelocityJointInterface
            
            
                hardware_interface/VelocityJointInterface
                1
            
        
    

    
    
    

    
    
        
            Debug
            true
            /
            1
            true
            true
            10.0
            true
            left_wheel2base_link 
            right_wheel2base_link 
            ${base_link_radius * 2} 
            ${wheel_radius * 2} 
            1
            30
            1.8
            robot1/cmd_vel 
            robot1/odom 
            robot1/odom 
            robot1/base_footprint 
        
    

2.6为机器人模型添加雷达配置

gazebo/laser.xacro：

  
  
    
      0 0 0 0 0 0
      true
      5.5
      
        
          
            360
            1
            -3
            3
          
        
        
          0.10
          30.0
          0.01
        
        
          gaussian
          0.0
          0.01
        
      
      
        /robot1/scan
        laser
      
    
  

2.7为机器人模型添加摄像头配置

gazebo/camera.xacro：

  
  
    
    
      30.0 
      
      
        1.3962634
        
          1280
          720
          R8G8B8
        
        
          0.02
          300
        
        
          gaussian
          0.0
          0.007
        
      
      
      
        true
        0.0
        /camera
        image_raw
        camera_info
        camera
        0.07
        0.0
        0.0
        0.0
        0.0
        0.0
      
    
  
    

2.8为机器人模型添加kinect摄像头配置

gazebo/kinect.xacro：

      
      
        true
        20.0
        
          ${60.0*PI/180.0}
          
            R8G8B8
            640
            480
          
          
            0.05
            8.0
          
        
        
          camera
          true
          10
          rgb/image_raw
          depth/image_raw
          depth/points
          rgb/camera_info
          depth/camera_info
          support_depth
          0.1
          0.0
          0.0
          0.0
          0.0
          0.0
          0.4
        
      
    

3、定位系统（amcl）

所谓定位就是推算机器人自身在全局地图中的位置，当然，SLAM中也包含定位算法实现，不过SLAM的定位是用于构建全局地图的，是属于导航开始之前的阶段，而当前定位是用于导航中，导航中，机器人需要按照设定的路线运动，通过定位可以判断机器人的实际轨迹是否符合预期。在ROS的导航功能包集navigation中提供了 amcl 功能包，用于实现导航中的机器人定位。AMCL(adaptive Monte Carlo Localization) 是用于2D移动机器人的概率定位系统，它实现了自适应（或KLD采样）蒙特卡洛定位方法，可以根据已有地图使用粒子滤波器推算机器人位置。只有机器人1需要使用定位系统。
nav_demo/launch/nav04_amcl_t.launch：

4、路径规划（move_base）

路径规划是导航中的核心功能之一，在ROS的导航功能包集navigation中提供了 move_base 功能包，用于实现此功能。move_base 功能包提供了基于动作(action)的路径规划实现，move_base 可以根据给定的目标点，控制机器人底盘运动至目标位置，并且在运动过程中会连续反馈机器人自身的姿态与目标点的状态信息。只有机器人1需要用到路径规划。

4.1全局路径规划与本地路径规划调用的通用参数

nav_demo/param/costmap_common_params_t.yaml：

#机器人几何参，如果机器人是圆形，设置 robot_radius,如果是其他形状设置 footprint
robot_radius: 0.12 #圆形
# footprint: [[-0.12, -0.12], [-0.12, 0.12], [0.12, 0.12], [0.12, -0.12]] #其他形状

obstacle_range: 3.0 # 用于障碍物探测，比如: 值为 3.0，意味着检测到距离小于 3 米的障碍物时，就会引入代价地图
raytrace_range: 3.5 # 用于清除障碍物，比如：值为 3.5，意味着清除代价地图中 3.5 米以外的障碍物


#膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物
inflation_radius: 0.2
#代价比例系数，越大则代价值越小
cost_scaling_factor: 3.0

#地图类型
map_type: costmap
#导航包所需要的传感器
observation_sources: robot1/scan
#对传感器的坐标系和数据进行配置。这个也会用于代价地图添加和清除障碍物。例如，你可以用激光雷达传感器用于在代价地图添加障碍物，再添加kinect用于导航和清除障碍物。
scan: {sensor_frame: robot1/laser, data_type: LaserScan, topic: robot1/scan, marking: true, clearing: true}

4.2全局代价地图参数设置

nav_demo/param/global_costmap_params_t.yaml:

global_costmap:
  global_frame: map #地图坐标系
  robot_base_frame: robot1/base_footprint #机器人坐标系
  # 以此实现坐标变换

  update_frequency: 1.0 #代价地图更新频率
  publish_frequency: 1.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: true # 是否使用一个地图或者地图服务器来初始化全局代价地图，如果不使用静态地图，这个参数为false.

4.3局部代价地图参数设置

nav_demo/param/local_costmap_params_t.yaml：

local_costmap:
  global_frame: robot1/odom #里程计坐标系
  robot_base_frame: robot1/base_footprint #机器人坐标系

  update_frequency: 10.0 #代价地图更新频率
  publish_frequency: 10.0 #代价地图的发布频率
  transform_tolerance: 0.5 #等待坐标变换发布信息的超时时间

  static_map: false  #不需要静态地图，可以提升导航效果
  rolling_window: true #是否使用动态窗口，默认为false，在静态的全局地图中，地图不会变化
  width: 3 # 局部地图宽度 单位是 m
  height: 3 # 局部地图高度 单位是 m
  resolution: 0.05 # 局部地图分辨率 单位是 m，一般与静态地图分辨率保持一致

4.4基本的局部规划器参数配置

nav_demo/param/base_local_planner_params_t.yaml：

TrajectoryPlannerROS:

# Robot Configuration Parameters
  max_vel_x: 0.5 # X 方向最大速度
  min_vel_x: 0.1 # X 方向最小速速

  max_vel_theta:  1.0 # 
  min_vel_theta: -1.0
  min_in_place_vel_theta: 1.0

  acc_lim_x: 1.0 # X 加速限制
  acc_lim_y: 0.0 # Y 加速限制
  acc_lim_theta: 0.6 # 角速度加速限制

# Goal Tolerance Parameters，目标公差
  xy_goal_tolerance: 0.10
  yaw_goal_tolerance: 0.05

# Differential-drive robot configuration
# 是否是全向移动机器人
  holonomic_robot: false

# Forward Simulation Parameters，前进模拟参数
  sim_time: 1.5
  vx_samples: 18
  vtheta_samples: 20
  sim_granularity: 0.05

二、目标跟随节点（坐标变换）

监听机器人2base_link2与map的坐标变换，并将其发布至/robot1/move_base_simple/goal，即可控制机器人1运动（这里监听map与base_link2的原因是/robot2/base_footprint坐标系会随着机器人运动而变换位置，经过观察发现base_link2不会乱跳，这里没找到原因，应该是某些地方没配置好或者冲突了）：

#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/TransformStamped.h"
#include"geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"test03_control_turtle2");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    ros::Publisher pub = nh.advertise("/robot1/move_base_simple/goal",100);

    ros::Rate rate(0.5);
    while (ros::ok())
    {
        try
        {
            geometry_msgs::TransformStamped son1toson2 = buffer.lookupTransform("map","base_link2",ros::Time(0));
            ROS_INFO("son1xiangduiyuson2 : pianyiliang(%.2f,%.2f)",son1toson2.transform.translation.x,son1toson2.transform.translation.y);
            geometry_msgs::PoseStamped odometry;
            odometry.header.frame_id = "robot1/odom";
            odometry.pose.position.x = (son1toson2.transform.translation.x-0.5);
            odometry.pose.position.y = (son1toson2.transform.translation.y);
            odometry.pose.position.z = 0;
            odometry.pose.orientation.w = 1.0;

            ROS_INFO("msg = (%.2f,%.2f)",odometry.pose.position.x,odometry.pose.position.y);
            pub.publish(odometry);
        }
        catch(const std::exception& e)
        {
            ROS_ERROR("Exception: %s", e.what());
        }

        

        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

三、键盘控制机器人2运动节点

机器人2控制节点，也可以使用ros自带的（rosrun teleop_twist_keyboard teleop_twist_keyboard.py，它发布到/cmd_vel话题）。

#include 
#include 
#include 
#include 
#include 
#include 
#ifndef _WIN32
# include 
# include 
#else
# include 
#endif

#define KEYCODE_RIGHT 0x43
#define KEYCODE_LEFT 0x44
#define KEYCODE_UP 0x41
#define KEYCODE_DOWN 0x42
#define KEYCODE_Q 0x71


class KeyboardReader
{
public:
  KeyboardReader()
#ifndef _WIN32
    : kfd(0)
#endif
  {
#ifndef _WIN32
    // get the console in raw mode
    tcgetattr(kfd, &cooked);
    struct termios raw;
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &=~ (ICANON | ECHO);
    // Setting a new line, then end of file
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);
#endif
  }
  void readOne(char * c)
  {
#ifndef _WIN32
      // 使用select来实现非阻塞读取
      fd_set readfds;
      struct timeval tv;
      FD_ZERO(&readfds);
      FD_SET(kfd, &readfds);

      // 设置超时为0，这使得select立即返回
      tv.tv_sec = 1;
      tv.tv_usec = 0;

      // 检查是否有输入
      int retval = select(kfd + 1, &readfds, NULL, NULL, &tv);
      if (retval == -1)
      {
          perror("select()");
          return;
      }
      else if (retval)
      {
          // 数据可读
          int rc = read(kfd, c, 1);
          if (rc < 0)
          {
              throw std::runtime_error("read failed");
          }
      }
      else
      {
          // 没有数据
          *c = 0; // 无字符
      }
#else
    for(;;)
    {
      HANDLE handle = GetStdHandle(STD_INPUT_HANDLE);
      INPUT_RECORD buffer;
      DWORD events;
      PeekConsoleInput(handle, &buffer, 1, &events);
      if(events > 0)
      {
        ReadConsoleInput(handle, &buffer, 1, &events);
        if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_LEFT)
        {
          *c = KEYCODE_LEFT;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_UP)
        {
          *c = KEYCODE_UP;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_RIGHT)
        {
          *c = KEYCODE_RIGHT;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == VK_DOWN)
        {
          *c = KEYCODE_DOWN;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x42)
        {
          *c = KEYCODE_B;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x43)
        {
          *c = KEYCODE_C;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x44)
        {
          *c = KEYCODE_D;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x45)
        {
          *c = KEYCODE_E;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x46)
        {
          *c = KEYCODE_F;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x47)
        {
          *c = KEYCODE_G;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x51)
        {
          *c = KEYCODE_Q;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x52)
        {
          *c = KEYCODE_R;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x54)
        {
          *c = KEYCODE_T;
          return;
        }
        else if (buffer.Event.KeyEvent.wVirtualKeyCode == 0x56)
        {
          *c = KEYCODE_V;
          return;
        }
      }
    }
#endif
  }
  void shutdown()
  {
#ifndef _WIN32
    tcsetattr(kfd, TCSANOW, &cooked);
#endif
  }
private:
#ifndef _WIN32
  int kfd;
  struct termios cooked;
#endif
};

KeyboardReader input;

class TeleopTurtle
{
public:
  TeleopTurtle();
  void keyLoop();
  void getKeyPress(char& c);

private:

  
  ros::NodeHandle nh_;
  double linear_, angular_, l_scale_, a_scale_;
  ros::Publisher twist_pub_;
  
};

TeleopTurtle::TeleopTurtle():
  linear_(0),
  angular_(0),
  l_scale_(2.0),
  a_scale_(2.0)
{
  nh_.param("scale_angular", a_scale_, a_scale_);
  nh_.param("scale_linear", l_scale_, l_scale_);

  twist_pub_ = nh_.advertise("/cmd_vel22", 1);
}

void quit(int sig)
{
  (void)sig;
  input.shutdown();
  ros::shutdown();
  exit(0);
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "keyboard");
  // ros::Rate rate(10);  // 设置频率为1Hz
  TeleopTurtle teleop_turtle;

  signal(SIGINT,quit);

  teleop_turtle.keyLoop();
  // rate.sleep(); // 等待以保持1Hz频率
  
  return(0);
}


void TeleopTurtle::getKeyPress(char& c)
{
  try
  {
    input.readOne(&c);
  }
  catch (const std::runtime_error &)
  {
    perror("read():");
    return;
  }

  linear_=angular_=0;
  ROS_DEBUG("value: 0x%02X\n", c);
}


void TeleopTurtle::keyLoop()
{
  char c;

  bool dirty=false;

  puts("Reading from keyboard");
  puts("----------------------------------------------");
  puts(" ");
  puts("Use up and down keys to move the left  turtle.");
  puts("Use w  and s    keys to move the right turtle.");
  puts(" ");
  puts("----------------------------------------------");

  while (ros::ok())
  {
    // get the next event from the keyboard  
    getKeyPress(c);
    linear_ = angular_ = 0;
    switch(c)
    {
      case KEYCODE_UP:
        linear_ = 0.3;
        dirty = true;
        break;
      case KEYCODE_DOWN:
        linear_ = -0.3;
        dirty = true;
        break;
      case KEYCODE_LEFT:
        angular_ = 1.0;
        dirty = true;
        break;
      case KEYCODE_RIGHT:
        angular_ = -1.0;
        dirty = true;
        break;
      default:
        angular_ = 0;
        linear_ = 0;
        break;
    }
   
    geometry_msgs::Twist twist2;
    twist2.angular.z = a_scale_*angular_;
    twist2.linear.x = l_scale_*linear_;
    twist_pub_.publish(twist2);
  }
}