ROS入门

ROS入门

创建工作空间与功能包

src：代码空间

build：编译空间

devel：开发空间

install：安装空间

创建工作空间

$ mkdir catkin_ws        #创建工作空间，catkin_ws为工作空间的自定义命名，可以随意改动
$ cd catkin_ws/
$ mkdir src              #创建代码空间
$ cd src/
$ catkin_init_workspace  #初始化工作空间，在src中产生.txt文件，说明创建成功

编译工作空间

$ cd ..                 #回到根目录
$ catkin_make           #catkin工具提供的编译器指令，编译src里面所有功能包的源码，结果都放在devel和install里，在根目录产生build和devel
$ catkin_make install   #产生install文件夹

创建功能包

$ cd src/                                              #一定要在src里创建功能包
$ catkin_create_pkg test_pkg roscpp rospy std_msgs     #创建功能包，test_pkg为文件名，roscpp、rospy、std_msgs为依赖包，可以根据需要添加其他,必须要有.txt和.xml这两个文件
$ cd ..                                                #回到根目录
$ catkin_make                                          #编译工作空间，此时会编译test_pkg这个功能包

设置环境变量

$ source devel/setup.bash    #设置了环境变量才能运行功能包
$ echo $ROS_PACKAGE_PATH     #检查环境变量

发布者Publisher的编程实现

· 初始化ROS节点

· 创建节点句柄

· 向ROS Master注册节点信息，包括发布的话题名和话题中的消息类型，即创建发布者

· 创建消息数据

· 按照一定频率循环发布消息

//创建cpp文件cd 
$ learning_topic/src/touch 
$ velocity_publisher.cpp

代码：

//代码
/**
 * 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

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

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 设置循环的频率
    ros::Rate loop_rate(10);

    int count = 0;
    while (ros::ok())
    {
        // 初始化geometry_msgs::Twist类型的消息
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5;
        vel_msg.angular.z = 0.2;

        // 发布消息
        turtle_vel_pub.publish(vel_msg);
        ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", 
                vel_msg.linear.x, vel_msg.angular.z);

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}

配置CMakeLists.txt

//在install上方添加
add_executable(velocity_publisher src/velocity_publisher.cpp)
target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

编译并运行

$ cd ~/catkin_ws
$ catkin_mak
$ source devel/setup.bash   //打开.bashrc，并在文件的最后添加这段话source /home/tfb/catkin_ws/devel/setup.bash就不用写source devel/setup.bash了
$ roscore
//新开一个终端，开启turtlesim
$ rosrun turtlesim turtlesim_node
//新开一个终端，运行发布者cpp程序
$ rosrun learning_topic velocity_publisher
//编译cpp生成的文件在home/catkin_ws/devel/lib/learning_topic下面

订阅者Subscriber的编程实现

· 初始化ROS节点

· 订阅需要的话题

· 循环等待话题消息，接收到消息后进入回调函数

· 在回调函数中完成消息处理

//创建cpp文件
$ cd learning_topic/src/
$ touch velocity_publisher.cpp

代码：

//代码
/**
 * 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose
 */
 
#include <ros/ros.h>
#include "turtlesim/Pose.h"

// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

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

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

配置CMakeLists.txt

add_executable(pose_subscriber src/pose_subscriber.cpp)
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})

编译并运行

$ cd ~/catkin_ws
$ catkin_make
$ roscore
//新开一个终端，开启turtlesim
$ rosrun turtlesim turtlesim_node
//新开一个终端，运行发布者cpp程序
$ rosrun learning_topic pose_subscriber

话题消息的定义与使用

· rostopic list

列出当前系统中所有活跃着的话题

· rostopic echo 主题名称

显示指定话题中发送的消息包内容

· rostopic hz 主题名称

统计指定话题中消息包发送频率

· rqt gragh

显示各个话题关系

定义msg文件

在package.xml中添加功能包依赖

<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>

在CMakeList.txt添加编译选项

find_package(...... message_generation)

add_message_files(FILES Person.msg)
generate_messages(DEPENDENCIES std_msgs)

catkin_package(...... message_runtime)

编译生成语言相关文件

发布者代码

#include <ros/ros.h>
#include "learning_topic/Person.h"
int main(int argc, char **argv)
{
        //节点初始化
        ros::init(argc, argv, "person_publisher");

        //创建节点句柄
        ros::NodeHandle n;

        //创建一个Publisher，发布名为/person_info的topic，消息类型为learning_topic::Person,队列长度为10
        ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);

        //设置循环的频率
        ros::Rate loop_rate(1);

        int count = 0;
        while(ros::ok())
        {
                //初始化learning_topic::Person类型的消息
                learning_topic::Person person_msg;
                person_msg.name = "wkk";
                person_msg.age = 20;
                person_msg.sex = learning_topic::Person::male;

                //发布消息
                person_info_pub.publish(person_msg);

                ROS_INFO("Publish Person Info: name:%s age:%d sex:%d", person_msg.name.c_str(), person_msg.age, person_msg.sex);


                //按照循环频率延时
                loop_rate.sleep();

        }
        return 0;
}

订阅者代码

#include <ros/ros.h>
#include "learning_topic/Person.h"

void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
        ROS_INFO("Subscribe Person Info: name:%s age:%d sex:%d", msg->name.c_str(), msg->age, msg->sex);
}

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

        //创建节点句柄
        ros::NodeHandle n;

        //创建一个Subscriber，订阅名为/person_info的topic，注册回调函数personCallback
        ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);

        //循环等待回调函数
        ros::spin();

        return 0;

}

配置CMakeList.txt

add_executable(person_publisher src/person_publisher.cpp)
target_link_libraries(person_publisher ${catkin_LIBRARIES})
add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_subscriber src/person_subscriber.cpp)
target_link_libraries(person_subscriber ${catkin_LIBRARIES})
add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

使用launch文件启动节点

1、使用launch文件，可以通过 roslaunch 指令一次启动多个节点

2、在launch文件中，为节点添加 output = “screen” 属性，可以让节点信息输出在终端中。（ROS_WARN不受该属性控制）

3、在launch文件中，为节点添加 ==launch-prefix = “gnone-terminal -e”==属性，可以让节点单独运行在一个独立终端中

XML语法

<标记名称 属性名1= “属性值1” …>内容</标记名称>

<大纸盒>
 <小纸盒 颜色 = “黄色”> 
  <又一个小纸盒 颜色 = “黄色”>
  </又一个小纸盒>
 </小纸盒>
</大纸盒>


或（因为没有内容）
<大纸盒>
 <小纸盒 颜色 = “黄色”/> 
  <又一个小纸盒 颜色 = “黄色”/>
</大纸盒>

编写运行launch文件

随便放在一个软件文件夹里即可

编写

<launch>
	<node pkg = "ssr_pkg", type = "yao_node", name = "yao_node"/>
	<node pkg = "ssr_pkg", type = "chao_node", name = "chao_node" launch-prefix - "gnome-terminal -e"(使用一个新的终端程序去运行这个节点)/>
	<node pkg = "atr_pkg", type = "ma_node", name = "ma_node" output = “screen”/>
</launch>

运行

$ roslaunch atr_pkg kai_hei.launch 

ROS机器人的运动控制

方向规范

矢量运动

右手呈枪型手势，指向机器人，食指为x轴（正前方），中指为y轴（正左方），拇指为z轴（正上方）

旋转运动

右手呈点赞手势：拇指指向任意（xyz）一轴，四指方向就是旋转正方向

速度规范

速度消息包

geometry_msgs/Twist Message : Twist就是表示速度

linear：线性的，即代表矢量运动的速度

angular：有角度的，即代表旋转运动的速度

矢量速度单位

米/秒

旋转速度单位

弧度/秒（Π = 3.14… = 180°）

运动控制实现

启动仿真环境

$ git clone https://https://github.com/6-robot/wpr_simulation.git  #下载开源的仿真程序进行学习，在~/catkin_ws/src/终端下载
$ ./install_for_noetic.sh                                          #在wpr_simulation/scripts终端下载依赖包
$ roslaunch wpr_simulation wpb_simple.launch 					   #启动仿真机器人

速度实现

1、构建一个新的软件包，包名叫做vel_pkg

2、在软件包中新建一个节点，节点名叫做vel_node

3、在节点中，向ROS大管家NodeHandle申请发布话题==/cmd_vel==，并拿到发布对象vel_pub

4、构建一个geometry_msgs/Twist类型的消息包vel_msg，用来承载要发送的速度值

5、开启一个while循环，不停的使用vel_pub对象发送速度消息包vel_msg

#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);


    ros::Rate loop_rate(30);


    while(ros::ok())
    {

        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.5;
        vel_pub.publish(vel_msg);
        ros::spinOnce();
        loop_rate.sleep();

    }

    return 0;
}

$ roslaunch wpr_simulation wpb_simple.launch 
$ rosrun vel_pkg vel_node

激光雷达的工作原理

作用

探测周围障碍物的分布状况

分类

种类繁多，但在ROS中呈现的数据格式是一样的，只是在数据完整度和精度上会有所差异，按照多种类型分类如下：

测量维度

· 单线雷达

· 多线雷达

测量原理

· 三角测距雷达

· TOF雷达

工作方式

· 机械旋转雷达

· 固态雷达

TOF雷达

通常安装在机器人底盘隔层的中央位置，受到地盘结构的保护，机械结构有：固定底盘和可旋转的头部结构

头部结构

具有红外激光发射器和接收器，工作时发出红外激光并反射回来接收，用以探测周围的障碍物，以点的形式呈现出来，旋转一周即可完成周围环境探测

使用RViz观测传感器数据

方便我们对机器人的状态进行实时观测的辅助工具

数据可视化

1、传感器采集到的数据

2、机器人运算处理的中间结果和将要执行的目标指示

3、去往某个地点的路径规划

rviz布局

使用

1、把Fixed Frame参数修改成base_footprint

2、把机器人模型添加进来（Add按钮）

3、添加激光雷达显示条目（laserscan），并将参数设置成/scan

激光雷达消息包格式

获取雷达的测距数值

1、构建一个新的软件包，包名叫做lidar_pkg

2、在软件包中新建一个节点，节点名叫做lidar_node

3、在节点中，向ROS大管家NodeHandle申请订阅话题/scan，并设置回调函数为LidarCallback()

4、构建回调函数LidarCallback()，用来接收和处理雷达数据

5、调用ROS_INFO()显示雷达检测到的前方障碍物距离

代码

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>


void LidarCallback(const sensor_msgs::LaserScan msg)
{

    float fMidDist = msg.ranges[180];

    ROS_INFO("前方测距 range[180] = %f 米", fMidDist);

}

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);

    ros::spin();

    return 0;
}

激光雷达避障

1、让大管家NodeHandle发布速度控制话题/cmd_vel

2、构建速度控制消息包vel_cmd

3、根据激光雷达的测距数值，实时调整机器人运动速度，避开障碍物

代码

只需要将上方的测距数值进行处理，并赋予速度值即可

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

ros::Publisher vel_pub;

void LidarCallback(const sensor_msgs::LaserScan msg)
{

    float fMidDist = msg.ranges[180];

    ROS_INFO("前方测距 range[180] = %f 米", fMidDist);

    geometry_msgs::Twist vel_cmd;
    if(fMidDist < 1.5)
    {
        vel_cmd.angular.z = 0.3;
    }
    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();

    return 0;
}

IMU消息包的数据格式

IMU是安装在机器人内部的一种传感器模块，用于测量机器人的空间姿态

获取IMU数据的C++节点

1、构建一个新的软件包，包名叫做imu_pkg

2、在软件包中新建一个节点，节点包叫做imu_node

3、在节点中，向ROS大管家NodeHandle申请订阅话题/imu/data，并设置回调函数为IMUCallback()

4、构建回调函数IMUCallback()，用来接收和处理IMU数据

5、使用TF工具将四元数转换成欧拉角

6、调用ROS_INFO()，显示转换后的欧拉角数值

代码

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>

void IMUCallback(sensor_msgs::Imu msg)
{

    if(msg.orientation_covariance[0] < 0)       #判断四元数orientation的协方差矩阵的第一个数为-1，就四元数不存在了
        return;
    tf::Quaternion quaternion(                  #通过TF工具将消息包里的四元数转换为tf的四元数对象

        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w

    );

    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);   #将quaternion转换成一个tf的3x3矩阵对象，再调用矩阵对象的getRPY函数将其转换成欧拉角
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;

    ROS_INFO("滚转 = %.0f  俯仰 = %.0f  朝向 = %.0f", roll, pitch, yaw);

}

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL, "");    #显示中文
    
    ros::init(argc, argv, "imu_node");

    ros::NodeHandle n;

    ros::Subscriber imu_sub = n.subscribe ("/imu/data", 10, IMUCallback);

    ros::spin();    #让程序一直运作不要停止

    return 0;
}

IMU航向锁定的节点

1、让大管家NodeHandle发布速度控制话题/cmd_vel

2、设定一个目标朝向角，当姿态信息中的朝向角和目标朝向角不一致时，控制机器人转向目标朝向角

代码

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>


ros::Publisher vel_pub;

void IMUCallback(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);

    double target_yaw = 90;                     #目标朝向值
    double diff_angle = target_yaw - yaw;       #朝向值与目标朝向值差值
    geometry_msgs::Twist vel_cmd;               
    vel_cmd.angular.z = diff_angle * 0.01;      #给机器人一个旋转的速度直至旋转到目标值
    vel_pub.publish(vel_cmd);                   #发布速度的值

}

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

    ros::NodeHandle n;

    ros::Subscriber imu_sub = n.subscribe ("/imu/data", 10, IMUCallback);

    vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    ros::spin();

    return 0;
}

标准消息包std_msgs

常用消息包common_msgs

几何包geometry_msgs

传感器消息包sensor_msgs

自定义消息类型

1、创建新软件包，依赖项message_generation、message_runtime

2、软件包添加msg目录，新建自定义消息文件，以.msg结尾

3、在CMakeList.txt中，将新建的.msg文件加入add_message_files()

4、在CMakeList.txt中，去掉generation_mseeage()注释符号，将依赖性的其他消息包名称添加进去

5、在CMakeList.txt中，将message_runtimt加入catkin_package()的CATKIN_DEPENDS

6、在packagexml中，将message_generation、message_runtime加入<build_depend>和<exec_depend>

7、编译软件包，生成新的自定义消息类型

数据类型 变量名
string data
int name    .....
============================
数据类型
①bool、byte、char
②int8、int16、int32、int64、uint8、uint16、uint32、uint64
③float32、float64 
④string
⑤time、duration
⑥可变长度数组array[]
⑦固定长度数组array[n]
⑧其他软件包定义的消息类型

新消息类型在C++节点的应用

1、在节点代码中，先include新消息类型的头文件

2、在发布或订阅话题的时候，将话题中的消息类型设置为新的消息类型。

3、按照新的消息结构，对消息包进行赋值发送或读取解析

4、在CMakeList.txt文件的find_package0)中，添加新消息包名称作为依赖项

5、在节点的编译规则中，添加一条add dependencies0)，将新消息软件包名称_generate_messages_cpp作为依赖项

6、在package.xml中，将新消息包添加到<build_depend>和<exec_depend>中去

7、运行 catkin_make 重新编译

ROS中的栅格地图格式

这种地图会将画面转化为方格，默认为白色，当激光雷达扫射到障碍物时，会将方格变为黑色，栅格的尺寸越小，黑色的区域划分就越精细，也就越接近障碍物的轮廓，但此时地图数据量会越大，计算量也就越大，所以要给栅格设置一个适当的尺寸

栅格尺寸：

体现了地图的精细程度，常常被用来表示栅格地图的分辨率，ros中默认分辨率为0.05米

OccupancyGrid数据类型：将栅格的白色赋予0，黑色赋予100，然后根据行和列组成一个数组

发布地图代码

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

int main(int argc, char *argv[])
{

	ros::init(argc, argv, "map_pub_node");
	
	ros::NodeHandle n;
	
	ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10);
	
	ros::Rate r(1);
	
	while(ros::ok())
	{
	
		nav_msgs::OccuopancyGrid msg;
		
		msg.header.frame_id = "map";
		
		msg.header.stamp = ros::Time::now()
		
		msg.info.origin.position x = 0;
		msg.info.origin.position y = 0;    #这个是地图相对与世界原点的位置
		
		msg.info.resolution = 1.0;
		
		msg.info.width = 4;
		msg.info.length = 2;
		
		msg.data.resize(4*2);
		msg.data[0] = 100;
		msg.data[1] = 100;
		msg.data[2] = 0 ;
		msg.data[3] = -1;
		
		pub.publish(msg);
		
		r.sleep();
	}
	
	return 0;

}

在地图上显示栅格

$ rosrun map_pkg map_pub_node
$ rviz

①确定世界坐标系的原点位置，添加一个坐标轴标识（Axes）

②添加地图显示（map），并将地图消息的话题名称设置为实验节点发布的/map

发布地图代码

#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>

int main(int argc, char *argv[])
{

	ros::init(argc, argv, "map_pub_node");
	
	ros::NodeHandle n;
	
	ros::Publisher pub = n.advertise<nav_msgs::OccupancyGrid>("/map", 10);
	
	ros::Rate r(1);
	
	while(ros::ok())
	{
	
		nav_msgs::OccuopancyGrid msg;
		
		msg.header.frame_id = "map";
		
		msg.header.stamp = ros::Time::now()
		
		msg.info.origin.position x = 0;
		msg.info.origin.position y = 0;    #这个是地图相对与世界原点的位置
		
		msg.info.resolution = 1.0;
		
		msg.info.width = 4;
		msg.info.length = 2;
		
		msg.data.resize(4*2);
		msg.data[0] = 100;
		msg.data[1] = 100;
		msg.data[2] = 0 ;
		msg.data[3] = -1;
		
		pub.publish(msg);
		
		r.sleep();
	}
	
	return 0;

}

在地图上显示栅格

$ rosrun map_pkg map_pub_node
$ rviz

①确定世界坐标系的原点位置，添加一个坐标轴标识（Axes）

②添加地图显示（map），并将地图消息的话题名称设置为实验节点发布的/map