搭建ROS小车编写启动功能包

3.6 ROS小车启动功能包编写

3.6.1 前言

  本篇内容应该是大家期待已久的内容，从我自身的开发经验和小伙伴的疑问来看，STM32和ROS通信、以及ROS小车启动功能包的编写是搭建ROS小车比较核心且不容易实现的技术点。多数小伙伴都是在这两个问题上，浪费了很多时间。之前的文章已经完美解决STM32和ROS通信问题。本篇文章就是结合之前内容的铺垫解决ROS小车启动功能包的编写问题。适合各种类型的ROS小车、ROS机器人平台的开发者参考。  

3.6.2 明确ROS小车完成SLAM和导航的必备条件是什么？

  这里借用深蓝学院的课件，给大家解答这个问题。  

1. 硬件要求

1. 深度信息—激光雷达，RGB-D相机有一个就可以

2D激光雷达
  RGB-D摄像机
 

1. 里程计信息

  本篇内容主要就是解决上面软件上的必备条件。  

3.6.3 ROS小车启动功能包的主要功能是什么？

  简明扼要的介绍，清楚功能，才好去设计程序。全部功能如下：  

• 订阅/cmd_vel话题 ====可能的发布者：move_base导航框架、ROS android App等等一切可以发布cmd_vel的ROS节点
• 与STM32进行串口通信 ====从STM32获取左右轮实时的真实速度和航向角、向底盘STM32发送左右轮的控制速度
• ROS小车里程计odom计算 ==== 通过从STM32获得的数据进行速度积分方式的里程计计算
• 发布里程计odom和tf ====发布建图和导航算法功能包需要的里程计和tf变换

软件流程大概是这样的，以前文章提到过。

3.6.4 ROS小车启动的launch文件要做什么事情？

  同样是简明扼要的介绍。启动launch文件主要做的事情如下：

• 启动机器人启动功能包ROS节点
• 加载机器人模型参数文件
• 发布机器人关节状态
• 发布机器人tf变换，由机器人模型文件维护的部分
• 启动激光雷达等传感器驱动功能节点

3.6.5 启动功能包代码实践

  通过3.6.3、3.6.4的简单的介绍，我们肯定清楚了一个能实现SLAM和导航的ROS小车的启动功能包和启动的launch应该具备什么基础功能，本节就带大家一起解决这个问题。   这里我先给大家介绍一下我编写好的功能包的文件结构。如下图，然后我们可以按照ROS基础学习内容，一步步的新建ROS功能包、新建源文件，修改CMakelists.txt源文件、编译、运行；也可以直接将文末分享的ROS小车启动功能包拷贝到大家的工作空间中，直接编译运行就行。  
  注意该启动功能包的通信部分，与之前分享的底层通信是配套使用的，如果没有看之前的文章，尽量看一下STM32和ROS串口通信的那一篇，这里将底层代码和机器人启动功能包一并分享给大家。   （1）新建ROS机器人启动功能包robot_bringup

cd ~/catkin_test_ws/src
catkin_create_pkg robot_bringup tf roscpp rospy std_msgs 
cd ~/catkin_test_ws/
catkin_make
source devel/setup.bash
rospack profile

（2）新建功能包源文件

cd ~/catkin_test_ws/src/robot_bringup/src
touch robot.cpp robot_bringup.cpp
cd ~/catkin_test_ws/src/robot_bringup/include/robot_bringup
touch robot.h

  将给大家分享功能包中的串口通信的源文件分别复制到/src、/include/robot_bringup文件夹下。此时ROS机器人启动功能包文件树如下所示：  
  （3）按照之前描述的功能编写源文件 robot.h文件内容如下：

#ifndef __ROBOT_H__
#define __ROBOT_H__

#include <ros/ros.h>
#include <ros/time.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <boost/asio.hpp>
#include <geometry_msgs/Twist.h>

namespace robot
{
    class robot
    {
        public:
            robot();
            ~robot();
            bool init();                  
            bool deal(double RobotV, double RobotYawRate);

        private:
            void calcOdom();               //里程计计算
            void pubOdomAndTf();           //发布Odom和tf

        private:
            ros::Time current_time_, last_time_; //时间

            double x_;                     //机器人位姿
            double y_;
            double th_;

            double vx_;                    //机器人x方向速度
            double vy_;                    //机器人y方向速度
            double vth_;                   //机器人角速度
            unsigned char sensFlag_;       //通信预留发送和接收标志位，可进行信号控制使用
            unsigned char receFlag_;

            ros::NodeHandle nh;
            ros::Publisher pub_;
            tf::TransformBroadcaster odom_broadcaster_;
    };

}

#endif

  robot.cpp 文件内容如下：  

#include <vector>
#include "robot_bringup/robot.h"
#include "robot_bringup/mbot_linux_serial.h"
using namespace std;

namespace robot
{
    boost::array<double, 36> odom_pose_covariance = {
        {1e-9, 0, 0, 0, 0, 0, 
        0, 1e-3,1e-9, 0, 0, 0, 
        0, 0, 1e6, 0, 0, 0,
        0, 0, 0, 1e6, 0, 0, 
        0, 0, 0, 0, 1e6, 0, 
        0, 0, 0, 0, 0, 1e-9}};
    boost::array<double, 36> odom_twist_covariance = {
        {1e-9, 0, 0, 0, 0, 0, 
        0, 1e-3,1e-9, 0, 0, 0, 
        0, 0, 1e6, 0, 0, 0, 
        0, 0, 0, 1e6, 0, 0, 
        0, 0, 0, 0, 1e6, 0, 
        0, 0, 0, 0, 0, 1e-9}};
    robot::robot():x_(0.0), y_(0.0), th_(0.0),vx_(0.0), vy_(0.0), vth_(0.0),sensFlag_(0),receFlag_(0) {}//构造函数
    robot::~robot(){}                                                                                   //析构函数
    /********************************************************
    函数功能：串口参数初始化、时间变量初始化、实例化发布对象
    入口参数：无
    出口参数：bool
    ********************************************************/
    bool robot::init()
    {
        // 串口初始化连接
        serialInit();

        ros::Time::init();
        current_time_ = ros::Time::now();
        last_time_ = ros::Time::now();

        //定义发布消息的名称
        pub_ = nh.advertise<nav_msgs::Odometry>("odom", 50);        

        return true;
    }

    /********************************************************
    函数功能：根据机器人线速度和角度计算机器人里程计
    入口参数：无
    出口参数：无
    ********************************************************/
    void robot::calcOdom()
    {
        ros::Time curr_time;
        curr_time = ros::Time::now();

        double dt = (curr_time - last_time_).toSec(); //间隔时间
        double delta_x = (vx_ * cos(th_)) * dt;       //th_弧度制
        double delta_y = (vx_ * sin(th_)) * dt;
        double delta_th = vth_ * dt;

        //打印时间间隔调试信息，不用的时候可以关闭
        ROS_INFO("dt:%f\n",dt);                       //s

        //里程计累加
        x_ += delta_x;
        y_ += delta_y;

        //实时角度信息,如果这里不使用IMU，也可以通过这种方式计算得出
        //th_ += delta_th;
        last_time_ = curr_time;  

        //打印位姿调试信息，不用的时候可以关闭
        ROS_INFO("x_:%f\n",x_);
        ROS_INFO("y_:%f\n",y_);
        ROS_INFO("th_:%f\n",th_);
    }
    /********************************************************
    函数功能：发布机器人里程计和TF
    入口参数：无
    出口参数：无
    ********************************************************/
    void robot::pubOdomAndTf()
    {
        current_time_ = ros::Time::now();
        // 发布TF
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time_;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id  = "base_footprint";

        geometry_msgs::Quaternion odom_quat;
        odom_quat = tf::createQuaternionMsgFromYaw(th_);
        odom_trans.transform.translation.x = x_;
        odom_trans.transform.translation.y = y_;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        odom_broadcaster_.sendTransform(odom_trans);

        // 发布里程计消息
        nav_msgs::Odometry msgl;
        msgl.header.stamp = current_time_;
        msgl.header.frame_id = "odom";

        msgl.pose.pose.position.x = x_;
        msgl.pose.pose.position.y = y_;
        msgl.pose.pose.position.z = 0.0;
        msgl.pose.pose.orientation = odom_quat;
        msgl.pose.covariance = odom_pose_covariance;

        msgl.child_frame_id = "base_footprint";
        msgl.twist.twist.linear.x = vx_;
        msgl.twist.twist.linear.y = vy_;
        msgl.twist.twist.angular.z = vth_;
        msgl.twist.covariance = odom_twist_covariance;

        pub_.publish(msgl);

    }
    /********************************************************
    函数功能：自定义deal，实现整合，并且发布TF变换和Odom
    入口参数：机器人线速度和角速度，调用上面三个函数
    出口参数：bool
    ********************************************************/
    bool robot::deal(double RobotV, double RobotYawRate)
    {
        // 向STM32发送对机器人的预期控制速度，以及预留信号控制位
        writeSpeed(RobotV, RobotYawRate, sensFlag_);
        // 从STM32读取机器人实际线速度，角速度和角度，以及预留信号控制位
        readSpeed(vx_, vth_, th_, receFlag_);
        // 里程计计算
        calcOdom();
        // 发布TF变换和Odom
        pubOdomAndTf();
    }
}

  robot_bringup.cpp文件内容如下：  

#include "robot_bringup/robot.h"

double RobotV_ = 0;
double RobotYawRate_ = 0;

// 速度控制消息的回调函数
void cmdCallback(const geometry_msgs::Twist& msg)
{
    RobotV_  = msg.linear.x * 1000;//mm/s
    RobotYawRate_ = msg.angular.z; //rad/s
}

int main(int argc, char** argv)
{
    //初始化ROS节点
    ros::init(argc, argv, "robot_bringup");
    ros::NodeHandle nh;

    //初始化robot
    robot::robot myrobot;
    if(!myrobot.init())
        ROS_ERROR("myrobot initialized failed.");
    ROS_INFO("myrobot initialized successful.");

    //订阅cmd_vel
    ros::Subscriber sub = nh.subscribe("cmd_vel", 50, cmdCallback);

    //循环运行
    ros::Rate loop_rate(50);
    while (ros::ok()) 
    {
        ros::spinOnce();
        // 机器人控制
        myrobot.deal(RobotV_, RobotYawRate_);
        loop_rate.sleep();
    }

    return 0;
}

  （4）修改CMakeLists.txt和package.xml

cd ~/catkin_test_ws/src/robot_bringup/
vim CMakeLists.txt
vim package.xml

  CMakeLists.txt内容如下：

cmake_minimum_required(VERSION 2.8.3)
project(robot_bringup)

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  sensor_msgs
  tf
)

catkin_package(
   INCLUDE_DIRS include
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(robot_bringup 
               src/robot_bringup.cpp
               src/robot.cpp
               src/mbot_linux_serial.cpp)

target_link_libraries(robot_bringup ${catkin_LIBRARIES})

  package.xml内容如下：

<?xml version="1.0"?>
<package format="2">
  <name>robot_bringup</name>
  <version>0.0.0</version>
  <description>The robot_bringup package</description>

  <maintainer email="kexue@todo.todo">kexue</maintainer>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>tf</build_depend>

  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>tf</build_export_depend>

  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>tf</exec_depend>

  <exec_depend>nav_msgs</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

  （5）编译

cd ~/catkin_test_ws
catkin_make
source devel/setup.bash
rospack profile

  （6）编写ROS机器人启动功能包launch文件

cd ~/catkin_test_ws/src/robot_bringup
mkdir launch
vim robot_bringup.launch

  robot_bringup.launch内容如下

<launch>

    <!-- 启动mbot -->
    <node pkg="robot_bringup" type="robot_bringup" name="robot_bringup" output="screen" />

    <!-- 加载机器人模型参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find robot_description)/xacro/robot_with_laser.xacro'" />
    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />  
    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="state_publisher">
        <param name="publish_frequency" type="double" value="50.0" />
    </node>

    <!-- 运行激光雷达驱动 -->
    <include file="$(find robot_bringup)/launch/rplidar.launch" />

</launch>

3.6.6 ROS小车ROS系统下启动测试

  在启动测试之前需要注意三个问题。   第一个问题，这里启动了激光雷达，根据每个人各自的情况，请自行安装各自型号雷达的驱动功能包，自行更改各自启动雷达的launch文件。   如你使用的rpliar A1 A2可以使用以下命令安装驱动。

sudo apt-get install ros-kinetic-rplidar-ros

第二个问题，关于串口设备号和串口权限的问题，如果大家没有做串口绑定，这里就需要根据大家设备的插入顺序，手动的更改程序中关于串口通信的串口设备的名字和激光雷达串口设备的名字，并且要更改串口设备的权限。   主要修改的地方是mbot_linux_serial.cpp文件中的串口设备名称，以及自己的激光雷达启动launch中的串口设备名称。

boost::asio::serial_port sp(iosev, "/dev/ttyUSB1");

  通常更改串口设备权限的命令。

sudo chmod 777 /dev/ttyUSB0      #一般情况下是这两个，具体情况，看自己端口情况
sudo chmod 777 /dev/ttyUSB1

  第三个问题，之前讲解里程计理论的时候，注意到一个参数“轮间距”，不知道大家还记得不？这个参数也是要修改的，大家根据自己小车的具体情况进行更改，同样是在mbot_linux_serial.cpp文件中。

const double ROBOT_LENGTH = 157.00;  //轮间距mm
const double ROBOT_RADIUS = 78.50;   //两轮之间的一半长度mm

  确保线路连接正确、通电完毕。   打开一个新的终端，在终端中输入下面的命令，启动launch文件

roslaunch robot_bringup robot_bringup.launch

  此时终端持续不断的打印出调试数据，就证明机器人启动成功，恭喜大家，万里长征的第一步终于完成了。

3.6.7 总结

写到这里，这篇文章也算结束了。关于代码细节上问题，就有劳大家自行阅读。代码说长不长，说短不短，所以大家耐心阅读就会有不一样的收获。一遍不行，就两遍呗。   在公众号：小白学移动机器人，发送：机器人启动功能包，即可获得机器人启动功能包。发送：ROS小车底层，即可获得底层驱动代码。   如果你感觉，我的文章比较适合你，关注我，点个赞，给你不一样的惊喜。