ROS学习笔记二——速度、雷达、IMU

感谢b站up 机器人工匠阿杰
文章仅供个人学习使用

工具软件

Rviz：全称 The Robot VisualZation Tool
可视化状态观测

Gazebo：仿真，模拟发出传感器数据

运动控制

运动表示

矢量运动：右手坐标系，单位m/s
旋转运动：右手坐标系，单位rad/s
比如旋转速度3.14，表示一秒钟转半圈

消息包：
geometry_msgs 软件包的 twist 消息类型，包含两个数据成员：
liner：float x，float y，float z
angular：float x，float y，float z

机器人配套的程序源码通常会有个核心节点，向上会订阅速度话题，通常话题名称是/cmd_vel；向下能控制底层硬件。

例程

创建软件包、节点

cd ~/catkin_ws/src/
catkin_create_pkg vel_pkg roscpp geometry_msgs

在src目录下编写cpp节点：vel_node.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc,  char *argv[])
{
    ros::init(argc, argv, "vel_node");
    ros::NodeHandle n;
    ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    geometry_msgs::Twist vel_msg;

    vel_msg.linear.x = 0.1;
    vel_msg.linear.y = 0;
    vel_msg.linear.z = 0; 
    vel_msg.angular.x = 0;
    vel_msg.angular.y = 0;
    vel_msg.angular.z = 0; 

    ros::Rate rate(30);
    while(ros::ok())
    {
        vel_pub.publish(vel_msg);
        rate.sleep();
    }

    return 0;
}

cmakelist添加：

add_executable(vel_node src/vel_node.cpp)

target_link_libraries(vel_node
  ${catkin_LIBRARIES}
)

雷达

消息包

sensor_msgs 的 LaserScan

显示雷达话题：

rostopic echo /scan --noarr

雷达 话题 和 消息包名称 是约定俗成的

编写雷达避障节点

catkin_create_pkg lidar_pkg roscpp sensor_msgs

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>

// 速度消息发布对象（全局变量）
ros::Publisher vel_pub;
static int nCount = 0;

void LidarCallback(const sensor_msgs::LaserScan msg)
{
    int nNum = msg.ranges.size();
    
    int nMid = nNum/2;
    float fMidDist = msg.ranges[nMid];
    ROS_INFO("前方测距 ranges[%d] = %f 米", nMid, fMidDist); 

    if(nCount > 0)//检测后延时检测
    {
        nCount--;
        return;
    }

    geometry_msgs::Twist vel_cmd;
    if(fMidDist < 1.5f)
    {
        vel_cmd.angular.z = 0.3;
        nCount = 50;
    }
    else
    {
        vel_cmd.linear.x = 0.05;
    }
    vel_pub.publish(vel_cmd);
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"lidar_node");
    
    ros::NodeHandle n;
    ros::Subscriber lidar_sub = n.subscribe("/scan", 10, &LidarCallback);
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);

    ros::spin();
}

IMU

消息包

IMU是惯性测量单元
sensor_msgs 的 Imu 消息
该消息是6轴传感器，如果使用磁强计，需要使用单独的消息：MagneticField

按照ROS 官方标准，一个IMU 可能包含三个话题：

通常订阅的是第二个话题：imu/data， 直接获取imu融合好的姿态四元数

编写IMU航向锁定节点

catkin_create_pkg imu_pkg roscpp sensor_msgs

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"

// 速度消息发布对象（全局变量）
ros::Publisher vel_pub;

// IMU 回调函数
void IMUCallback(const sensor_msgs::Imu msg)
{
    // 检测消息包中四元数数据是否存在
    if(msg.orientation_covariance[0] < 0)
        return;
    // 四元数转成欧拉角
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    // 弧度换算成角度
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("滚转= %.0f 俯仰= %.0f 朝向= %.0f", roll, pitch, yaw);
    // 速度消息包
    geometry_msgs::Twist vel_cmd;
    // 目标朝向角
    double target_yaw = 90;
    // 计算速度
    double diff_angle = target_yaw - yaw;
    vel_cmd.angular.z = diff_angle * 0.01;
    vel_cmd.linear.x = 0.1;
    vel_pub.publish(vel_cmd);
}

int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc,argv, "imu_node"); 

    ros::NodeHandle n;
    // 订阅 IMU 的数据话题
    ros::Subscriber sub = n.subscribe("imu/data", 100, IMUCallback);
    // 发布速度控制话题
    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    ros::spin();

    return 0;
}