由于安装 ROS 确实很 easy,官方文档已经是保姆级的教程了,这里不在赘述。本文的实验环境是 Ubuntu 20.04 + ROS Noetic Ninjemys。
运行第一个 ROS 包
sudo apt install ros-noetic-rqt-robot-steering
sudo apt install ros-noetic-turtlesim
roscore
rosrun rqt_robot_steering rqt_robot_steering
rosrun turtlesim turtlesim_node
从 Github 下载软件包编译
mkdir catkin_ws
cd catkin_ws
mkdir src
cd src
git clone https://github.com/6-robot/wpr_simulation.git
cd wpr_simulation/scripts
./install_for_noetic.sh
然后返回 ~/catkin_ws
目录下,
catkin_make
cd devel
source ~/catkin_ws/devel/setup.zsh
roslaunch wpr_simulation wpb_simple.launch
rosrun rqt_robot_steering rqt_robot_steering
将 source ~/catkin_ws/devel/setup.zsh
添加到 ~/.zshrc
中。
VSCode 配置开发环境
在 VSCode 中打开 ~/catkin_ws/src
文件夹,安装 ROS 和 CMake Tools 插件
按住 Ctrl + Shift + B
,选择 catkin_make: build
选项,就可以编译代码了。
此外,点击 catkin_make: build
旁边的设置齿轮图标,进入 task.json
,修改 group
字段后内容如下:
{
"version": "2.0.0",
"tasks": [
{
"type": "catkin_make",
"args": [
"--directory",
"/home/lrl52/catkin_ws",
"-DCMAKE_BUILD_TYPE=RelWithDebInfo"
],
"problemMatcher": [
"$catkin-gcc"
],
"group": {
"kind": "build",
"isDefault": true
},
"label": "catkin_make: build"
}
]
}
此后直接按住 Ctrl + Shift + B
快捷键就可以编译了。
关闭 CMake 插件的自动配置,
创建第一个 Node 结点
catkin_create_pkg ssr_pkg rospy roscpp std_msgs
创建后得到如下文件夹和文件,
创建 src/chao_node.cpp
,编写如下代码:
#include <ros/ros.h>
int main(int argc, char *argv[]) {
printf("hello, ros world!\n");
return 0;
}
在 CMakeLists.txt 里添加 add_executable(chao_node src/chao_node.cpp)
。
快捷键编译,然后
roscore
rosrun ssr_pkg chao_node
可以看到终端输出了 hello, ros world!
。
完善上述代码,
#include <ros/ros.h>
int main(int argc, char *argv[]) {
ros::init(argc, argv, "chao_node");
printf("Hello, ros world!\n");
while (ros::ok()) {
printf("ROS is running\n");
}
return 0;
}
在 CMakeLists.txt 里添加 target_link_libraries(chao_node ${catkin_LIBRARIES})
。完整 CMakeLists.txt 如下:
cmake_minimum_required(VERSION 3.0.2)
project(ssr_pkg)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES ssr_pkg
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(chao_node src/chao_node.cpp)
target_link_libraries(chao_node
${catkin_LIBRARIES}
)
再次编译运行,得到如下结果
Topic 与 Message
Publisher 实现
在 chao_node.cpp
中编写如下代码,
#include <ros/ros.h>
#include <std_msgs/String.h>
int main(int argc, char *argv[]) {
ros::init(argc, argv, "chao_node");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::String>("topic_name", 25); // ("话题名称", 缓冲区长度)
ros::Rate loop_rate(25); // 每秒 25 次
printf("Hello, ros world!\n");
while (ros::ok()) {
std_msgs::String msg;
msg.data = "message from chao_node";
pub.publish(msg); // 发布话题
// printf("ROS is running\n");
loop_rate.sleep();
}
return 0;
}
编译,然后
rosrun
rosrun ssr_pkg chao_node
使用 rostopic list
可以查看当前 ROS 有哪些已经发布的 topic,
使用 rostopic echo /topic_name
查看对于 topic 里的 message。rostopic hz /topic_name
可以查看 topic 的发送频率。
Subscriber 实现
catkin_create_pkg atr_pkg rospy roscpp std_msgs
创建一个新的 package。编写如下代码:
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
void chao_callback(const std_msgs::String &msg) {
// std::cout << msg.data << "\n";
ROS_INFO(msg.data.c_str());
ROS_WARN(msg.data.c_str());
}
int main(int argc, char *argv[]) {
ros::init(argc, argv, "ma_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("topic_name", 10, chao_callback);
while (ros::ok()) {
ros::spinOnce();
}
return 0;
}
补全 CMakelists.txt 的内容(同上),接着
roscore
src rosrun ssr_pkg chao_node
src rosrun atr_pkg ma_node
可以看到效果如下图
时间戳是从 1970 年 1 月 1 日开始的 UNIX 时间戳,即当前距离 1970 年 1 月 1 日的秒数。
使用 rqt_graph
可以图形化显示 topic 通讯关系,
另外注意,话题并不属于发布者或者订阅者,而是由 ROS 系统创建管理的,只要向 NodeHandle 提出了话题订阅需求或者话题发布需求,话题就会自动创建。
添加一个新的 Publisher 节点
在 ssr_pkg/src
下新建 yao_node.cpp
,编写如下代码:
#include <ros/ros.h>
#include <std_msgs/String.h>
int main(int argc, char *argv[]) {
ros::init(argc, argv, "yao_node");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::String>("yao_topic", 25); // ("话题名称", 缓冲区长度)
ros::Rate loop_rate(25); // 每秒 25 次
printf("Hello, ros world!\n");
while (ros::ok()) {
std_msgs::String msg;
msg.data = "message from yao_node";
pub.publish(msg); // 发布话题
// printf("ROS is running\n");
loop_rate.sleep();
}
return 0;
}
更改 atr_pkg/src/ma_node.cpp
中的代码为如下:
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <iostream>
void chao_callback(const std_msgs::String &msg) {
ROS_INFO(msg.data.c_str());
}
void yao_callback(const std_msgs::String &msg) {
ROS_WARN(msg.data.c_str());
}
int main(int argc, char *argv[]) {
ros::init(argc, argv, "ma_node");
ros::NodeHandle nh;
ros::Subscriber sub1 = nh.subscribe("chao_topic", 10, chao_callback);
ros::Subscriber sub2 = nh.subscribe("yao_topic", 10, yao_callback);
while (ros::ok()) {
ros::spinOnce();
}
return 0;
}
同时在 ssr_pkg/CMakeLists.txt
中添加
add_executable(yao_node src/yao_node.cpp)
target_link_libraries(yao_node
${catkin_LIBRARIES}
)
运行效果如下:
Python 实现 Publisher 和 Subscriber
以 chao_node
为例,
catkin_create_pkg ssr_pypkg rospy roscpp std_msgs
cd ..
catkin_make # py 本身不需要编译,这里只是为了让 ROS 能够找到这个包
然后新建 scripts
文件夹,添加 py 文件,
编写如下 chao_node.py
代码:
#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
if __name__ == "__main__":
rospy.init_node("chao_node")
rospy.logwarn("Hello, ros world from python!")
pub = rospy.Publisher("chao_room", String, queue_size=10)
rate = rospy.Rate(15)
while not rospy.is_shutdown():
rospy.loginfo("hahaha")
msg = String()
msg.data = "message from chao_node"
pub.publish(msg)
rate.sleep()
yao_node.py
也是同理,这里略去。再创建 atr_pypkg
包,
catkin_create_pkg atr_pypkg rospy roscpp std_msgs
cd ..
catkin_make
编写如下 ma_node.py
代码:
#!/usr/bin/env python3
#coding=utf-8
import rospy
from std_msgs.msg import String
def chao_callback(msg : String) -> None:
rospy.loginfo(msg.data)
def yao_callback(msg : String) -> None:
rospy.logwarn(msg.data)
if __name__ == "__main__":
rospy.init_node("ma_node")
sub1 = rospy.Subscriber("chao_room", String, chao_callback, queue_size=10)
sub2 = rospy.Subscriber("yao_room", String, yao_callback, queue_size=10)
rospy.spin()
使用 launch 文件启动多个 ROS 节点
递归创建 atr_pkg/launch/start.launch
,编写如下启动 XML 文件,
<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>
相关解释如下:
效果如下图
机器人运动控制
运动控制使用的是 geometry_msgs/Twist Message
速度消息包,可发送六轴数据
运动控制的话题名称一般是 /cmd_vel
,
实现思路如下:
catkin_create_pkg vel_pkg rospy roscpp geometry_msgs
cd ..
catkin_make
C++ 版本
在 vel_pkg/src
中创建 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(50);
while (ros::ok()) {
vel_pub.publish(vel_msg);
rate.sleep();
}
return 0;
}
在 CMakeLists.txt 添加如下规则:
add_executable(vel_node src/vel_node.cpp)
target_link_libraries(vel_node
${catkin_LIBRARIES}
)
然后
roscore
roslaunch wpr_simulation wpb_simple.launch
rosrun vel_pkg vel_node
Python 版本
在 vel_pkg/scripts
中创建 vel_node.py
,编写如下代码:
#!/usr/bin/env python3
# coding=utf-8
import rospy
from geometry_msgs.msg import Twist
if __name__ == "__main__":
rospy.init_node("vel_node")
vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
vel_msg = Twist()
vel_msg.linear.x = -0.1
rate = rospy.Rate(50)
while not rospy.is_shutdown():
vel_pub.publish(vel_msg)
rate.sleep()
运行方式同上,效果同上。
有趣的是,我发现这个 topic 名称填写 /cmd_vel
或cmd_vel
都可以。
激光雷达与使用 RViz 观测传感器数据
- Gazebo 是模拟真是机器人发出传感器数据的工具。
-
RViz 是接收传感器数据并负责进行显示的工具。
简单操作流程图
然后同样的方式添加 LazerScan
,把左侧的 LazerScan 的 Topic 设置为 /scan
,同时可以调整 Size,
保存配置文件,
之后再重新打开即可。
获取激光雷达数据
[sensor_msgs/LaserScan Message](sensor_msgs/LaserScan Message)
C++ 版本
查看 LazerScan
消息包:
rostopic echo /scan --noarr
catkin_create_pkg lidar_pkg roscpp rospy sensor_msgs
在 src/lidar_node.cpp
中编写如下代码:
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
void LidarCallback(const sensor_msgs::LaserScan msg) {
auto dis = msg.ranges[179];
ROS_INFO("前方测距 range[179] = %f 米", dis);
}
int main(int argc, char *argv[]) {
setlocale(LC_ALL, "");
ros::init(argc, argv, "lidar_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, LidarCallback);
ros::spin();
return 0;
}
修改 CMakeLists.txt 文件,添加如下规则。(从这之后不再赘述了)
add_executable(lidar_node src/lidar_node.cpp)
target_link_libraries(lidar_node
${catkin_LIBRARIES}
)
效果如下:
Python 版本
#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor_msgs.msg import LaserScan
def LidarCallback(msg : LaserScan):
dis = msg.ranges[180]
rospy.loginfo("前方测距 ranges[180] = {} 米".format(dis))
if __name__ == "__main__":
rospy.init_node("lidar_node")
sub = rospy.Subscriber("/scan", LaserScan, LidarCallback, queue_size=10)
rospy.spin()
激光雷达避障
C++ 版本
在 lidar_node.cpp
中完善代码:
#include <ros/ros.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) {
auto dis = msg.ranges[179];
ROS_INFO("前方测距 range[179] = %f 米", dis);
if (nCount > 0) {
nCount--;
return;
}
geometry_msgs::Twist vel_msg;
if (dis < 1.5f) {
vel_msg.angular.z = 0.5;
nCount = 50;
} else {
vel_msg.linear.x = 0.05;
}
vel_pub.publish(vel_msg);
}
int main(int argc, char *argv[]) {
setlocale(LC_ALL, "");
ros::init(argc, argv, "lidar_node");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, LidarCallback);
vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
ros::spin();
return 0;
}
但我有两个问题还不太能理解:
- 为什么 LaserScan 消息包的
scan_time
和time_increment
字段是 0 - 在创建包的时候并没有引入
geometry_msgs
依赖,似乎没有什么影响
UP 主的补充回答:
- 仿真环境是这样的,只有真机跑起来才有时间
创建包的时候填写的依赖项,会被写在 CMakeLists.txt 和 package.xml 里。这个依赖项有点意思,我发现只要有 roscpp 就可以使用 ROS 内置的消息类型。猜想可能是 roscppConfig.cmake 的设置有关
Python 版本
在 lidar_node.py
中完善代码:
#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
nCount = 0
def LidarCallback(msg : LaserScan):
global nCount, vel_pub
dis = msg.ranges[180]
rospy.loginfo("前方测距 ranges[180] = {:.4f} 米".format(dis))
vel_msg = Twist()
if nCount > 0:
nCount -= 1
rospy.logwarn("持续转向 nCount = {}".format(nCount))
return
if dis < 1.5:
vel_msg.angular.z = 0.3
nCount = 50
else:
vel_msg.linear.x = 0.05
vel_pub.publish(vel_msg)
if __name__ == "__main__":
rospy.init_node("lidar_node")
sub = rospy.Subscriber("/scan", LaserScan, LidarCallback, queue_size=10)
vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
rospy.spin()
获取 IMU 数据
[sensor_msgs/Imu Message](sensor_msgs/Imu Message)
catkin_create_pkg imu_pkg roscpp rospy sensor_msgs
C++ 版本
在 imu_pkg/src
中编写 imu_node.cpp
,
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>
void IMUCallback(const sensor_msgs::Imu msg) {
if (msg.orientation_covariance[0] < 0.0) return;
tf::Quaternion q(
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
ROS_INFO("roll = %f, pitch = %f, yaw = %f", roll * 180 / M_PI, pitch * 180 / M_PI, yaw * 180 / M_PI);
}
int main(int argc, char *argv[]) {
setlocale(LC_ALL, "");
ros::init(argc, argv, "imu_data");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("imu/data", 100, IMUCallback);
ros::spin();
return 0;
}
效果如下:
Python 版本
#!/usr/bin/env python3
#coding=utf-8
import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math
def IMUCallback(msg : Imu):
if msg.orientation_covariance[0] < 0:
return
q = [msg.orientation.x, msg.orientation.y,
msg.orientation.z, msg.orientation.w]
roll, pitch, yaw = euler_from_quaternion(q)
rospy.loginfo("roll = {:.2f}, pitch = {:.2f}, yaw = {:.2f}".format(
roll * 180 / math.pi,
pitch * 180 / math.pi,
yaw * 180 / math.pi
))
if __name__ == "__main__":
rospy.init_node("imu_node")
imu_sub = rospy.Subscriber("/imu/data", Imu, IMUCallback, queue_size=10)
rospy.spin()
IMU 实现航向锁定
C++ 版本
继续在 imu_pkg/src
中完善 imu_node.cpp
,
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>
#include <geometry_msgs/Twist.h>
ros::Publisher vel_pub;
void IMUCallback(const sensor_msgs::Imu msg) {
if (msg.orientation_covariance[0] < 0.0) return;
tf::Quaternion q(
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
ROS_INFO("roll = %f, pitch = %f, yaw = %f", roll * 180 / M_PI, pitch * 180 / M_PI, yaw * 180 / M_PI);
geometry_msgs::Twist vel_cmd;
double target_yaw = 90;
double diff_yaw = target_yaw - yaw * 180 / M_PI;
vel_cmd.angular.z = diff_yaw * 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_data");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("imu/data", 100, IMUCallback);
vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
ros::spin();
return 0;
}
Python 版本
#!/usr/bin/env python3
# coding=utf-8
import rospy
from sensor_msgs.msg import Imu
from tf.transformations import euler_from_quaternion
import math
from geometry_msgs.msg import Twist
# IMU 回调函数
def imu_callback(msg):
if msg.orientation_covariance[0] < 0:
return
# 四元数转成欧拉角
quaternion = [
msg.orientation.x,
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
]
(roll,pitch,yaw) = euler_from_quaternion(quaternion)
# 弧度换算成角度
roll = roll*180/math.pi
pitch = pitch*180/math.pi
yaw = yaw*180/math.pi
rospy.loginfo("滚转= %.0f 俯仰= %.0f 朝向= %.0f", roll, pitch, yaw)
# 速度消息包
vel_cmd = Twist()
# 目标朝向角
target_yaw = 90
# 计算速度
diff_angle = target_yaw - yaw
vel_cmd.angular.z = diff_angle * 0.01
vel_cmd.linear.x = 0.1
global vel_pub
vel_pub.publish(vel_cmd)
# 主函数
if __name__ == "__main__":
rospy.init_node("imu_behavior")
# 订阅 IMU 的数据话题
imu_sub = rospy.Subscriber("/imu/data",Imu,imu_callback,queue_size=10)
# 发布机器人运动控制话题
vel_pub = rospy.Publisher("cmd_vel",Twist,queue_size=10)
rospy.spin()
ROS 中的消息包
标准消息包
常用消息包
几何消息包:可以看到里面一些消息包带了 Stamped
后缀关键词,这些消息包都是多了一个 Header,多了一个时间和空间坐标系 ID,将空间量和时间量进行了绑定,在一些预测和滤波算法里会用得着。
传感器消息包:
自定义消息包
创建自定义消息包
生成自定义消息包的步骤:
catkin_create_pkg qq_msgs roscpp rospy std_msgs message_generation message_runtime
创建 msg
文件夹和 Carry.msg
:
string grade
int64 star
string data
然后修改 CMakeLists.txt,重新编译。终端中输入:
rosmsg show qq_msgs/Carry
可以看到新的自定义消息包诞生了。
C++ 版本
修改 chao_node.cpp
,ma_node.cpp
以及对应的 CMakeLists.txt 和 package.xml 文件,最后运行效果如下:
Python 版本
Python 不需要在编译规则中添加 add_dependencies()
。
💥未完待更~
ROS 中的栅格地图
下面使用在 ROS 中发布自定义地图,
C++ 版本
catkin_create_pkg map_pkg rospy roscpp nav_msgs
在 map_pkg/src/map_pub_node.cpp
编写如下代码:
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
int main(int argc, char *argv[]) {
ros::init(argc, argv, "map_pub_node");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 10);
ros::Rate rate(10);
while (ros::ok()) {
nav_msgs::OccupancyGrid msg;
msg.header.frame_id = "map";
msg.header.stamp = ros::Time::now();
msg.info.origin.position.x = 2;
msg.info.origin.position.y = 3;
msg.info.resolution = 1.0;
msg.info.width = 4;
msg.info.height = 2;
msg.data.resize(msg.info.width * msg.info.height);
msg.data[0] = 100;
msg.data[1] = 100;
msg.data[2] = 0;
msg.data[3] = -1;
pub.publish(msg);
rate.sleep();
}
}
然后修改 CMakeLists.txt 添加编译规则,运行
rosrun map_pkg map_pub_node
rviz
在 RViz 中点击左下角的 Add,添加 Axes 和 Map,
Map 话题名称设置为 /map
,
Python 版本
#!/usr/bin/env python3
#coding=utf-8
import rospy
from nav_msgs.msg import OccupancyGrid
if __name__ == "__main__":
rospy.init_node("map_pub_node")
pub = rospy.Publisher("/map", OccupancyGrid, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = OccupancyGrid()
msg.header.frame_id = "map"
msg.header.stamp = rospy.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.height = 2
msg.data = [0] * msg.info.width * msg.info.height
msg.data[0] = 100
msg.data[1] = 100
msg.data[2] = 0
msg.data[3] = -1
pub.publish(msg)
rate.sleep()
Hector_Mapping 初体验
roslaunch wpr_simulation wpb_stage_slam.launch
rosrun hector_mapping hector_mapping
rosrun rviz rviz
rosrun rqt_robot_steering rqt_robot_steering
通过 lanuch 文件启动
<launch>
<!-- 载入 机器人 和 SLAM 的仿真场景 -->
<include file="$(find wpr_simulation)/launch/wpb_stage_slam.launch"/>
<!-- Hector SLAM -->
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping"></node>
<!-- Rviz 显示 -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/slam.rviz"/>
<!-- 运动控制 -->
<node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering"/>
</launch>
可以给 hector_mapping 添加参数,增快建图速度。具体见 wpr_simulation/launch/wpb_hector_comparison.launch
文件。
roslaunch wpr_simulation wpb_hector_comparison.launch
初识 ROS 中的 TF 系统
roslaunch slam_pkg hector.launch
rostopic list
rostopic type /tf
rostopic echo /tf
rosrun rqt_tf_tree rqt_tf_tree
tf 的消息类型为 tf2_msgs/TFMessage
,可以看到 tf 的消息如下:
---
transforms:
-
header:
seq: 0
stamp:
secs: 772
nsecs: 262000000
frame_id: "odom"
child_frame_id: "base_footprint"
transform:
translation:
x: 2.5370301864651945
y: 1.5186889059358941
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.95114390602314
w: 0.30874790693224397
---
rqt_tf_tree
可以显示 TF 树,可以看到空间坐标系的父子关系。
初识里程计
这一节内容比较复杂,具体见 B站-什么是里程计。有讲 hector 建图和 gmapping 建图算法的区别。
使用 Gmapping 进行 SLAM 建图
roslaunch wpr_simulation wpb_stage_robocup.launch
rostopic echo /scan --noarr
rosrun gmapping slam_gmapping
rosrun rviz rviz
rosrun wpr_simulation keyboard_vel_ctrl
由于撞到了东西,使参照物发生了移动,可以看到建图与原地图有初入。
然呃因为 gazebo 在强制关闭后再启动报错,我尝试寻找解决办法,
killall gzserver
killall gzclient
这下启动依旧报错,甚至重启 ubuntu 都没法解决问题了😰。暂时没有找到原因和解决办法。
gmapping lanch 文件如下(还没测试过):
<launch>
<include file="$(find wpr_simulation)/launch/wpb_stage_robocup.launch"/>
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find slam_pkg)/rviz/gmapping.rviz"/>
<node pkg="wpr_simulation" type="keyboard_ver_ctrl" name="keyboard_ver_ctrl"/>
</launch>
Gmapping 建图的参数设置
地图保存与加载
地图的保存:
rosrun map_server map_saver -f mymap
保存后会得到 mymap.pgm
和 mymap.yaml
两个文件。其中 mymap.pgm
是图像文件,可以直接打开看,后者 mymap.yaml
是地图配置文件,内容如下:
image: mymap.pgm
resolution: 0.025000 #地图分辨率
origin: [-12.812499, -12.812499, 0.000000] #地图左下角的坐标和地图偏转角度
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
地图还原:
rosrun map_server map_server mymap.yaml
VSCode 配置 ROS 调试环境
配置 X11-Forwarding
编辑 /etc/ssh/sshd_config
,确认 X11Forwarding
已经打开。
#AllowAgentForwarding yes
#AllowTcpForwarding yes
#GatewayPorts no
X11Forwarding yes
#X11DisplayOffset 10
#X11UseLocalhost yes
#PermitTTY yes
安装 VScode 关于 X11 的拓展,
在 Remote X11 的拓展中,选择设置,删除以下环境变量(笔者在这里被这个扩展卡了一会):
LIBGL_ALWAYS_INDIRECT=1
然后关闭 VScode,重新打开一个新窗口,输入 SSH 连接命令,-X
选项表示启用 X11 转发。
ssh -X lrl52@192.168.217.130
保存配置,打开 ssh config,可以看到 ForwardX11
被设置为了 yes
,
此时在 VSCode 的终端输入 echo $DISPLAY
,可以发现 DISPLAY 环境变量已经被设置,
> echo $DISPLAY
localhost:10.0
回到虚拟机,输入同样的命令,
> echo $DISPLAY
:0
我希望在 VSCode 运行 GUI 程序时,GUI 程序能够在虚拟机正常运行显示,这样便于在 VScode 中启动节点进行调试。因此需要把 VSCode 终端中的 DISPLAY
的值修改为 :0
。
export DISPLAY=:0
然后在 VScode 终端中输入 rviz 测试,可以发现启动成功。
为了避免每次打开都手动修改环境变量,编辑 ~/.zshrc
添加如下脚本:
# Check if VSCODE_INJECTION exists and equals to 1
if [[ $VSCODE_INJECTION = 1 ]]; then
export DISPLAY=:0
fi
这样在 VScode 中打开的终端就会自动设置该环境变量啦,其它的终端则不受影响。
配置调试
创建 launch.json
文件,添加 ROS: Launch
和 ROS: Attach
模板配置。修改 launch 部分的启动路径:
{
// 使用 IntelliSense 了解相关属性。
// 悬停以查看现有属性的描述。
// 欲了解更多信息,请访问: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "ROS: Attach",
"type": "ros",
"request": "attach"
},
{
"name": "ROS: Launch",
"type": "ros",
"request": "launch",
"target": "/home/lrl52/shenlan_ws/src/grid_path_searcher/launch/demo.launch"
}
]
}
调试之前,删除之前的 build 和 devel 文件夹,重新构建项目,生成调试 symbol。执行:
catkin_make -DCMAKE_BUILD_TYPE=Debug -j
可以在 VSCode 中后台启动 ROS。确保调试时 ROS master 进行在运行中。
尝试 ROS: Launch
调试,单步调试完美启动:
但目前笔者尚不清楚 RViz 为什么不会在后台启动运行,即使去掉所有断点也不行。
这时候还可以尝试另一种要求更低,更通用的调试办法,那就是 GDB Attach
。
选择 ROS: Attach
,选择 demo_node
进程:
此时可能会提示
笔者发现不管你选择 y
or N
都没法进行调试,在这里又被卡了一会,原因是 Linux 默认系统内核参数 kernel.yama.ptrace
设置为1(True),此时普通用户进程是不能对其他进程进行 attach 操作的。更改办法见非root权限进行gdb attach调试。执行命令:
sudo sysctl kernel/yama/ptrace_scope=0
此时再次尝试 Attach 就可以成功调试了。Oh wowowowhahaha!🎉
(另外如果调试的时候,调试当前行上下乱跳,记着检查一下各个包的 CMakeLists.txt
,一定要把所有 O2, O3 优化都关掉)
References
GitHub: vscode-ros-debug-support
What are CMAKE_BUILD_TYPE: Debug, Release, RelWithDebInfo and MinSizeRel?