turtlebot2笔记（更新中）

1. 运行环境

• Ubuntu 16.04
• ROS Kinetic

2. 启动turtlebot2

roslaunch turtlebot_bringup minimal.launch --screen

参数--screen可以将启动过程的详细信息输出到终端，可以不加。

3. turtlebot2的节点和话题

启动turtlebot2后查询话题

$ rostopic list

可以看到

/cmd_vel_mux/active
/cmd_vel_mux/input/navi
/cmd_vel_mux/input/safety_controller
/cmd_vel_mux/input/switch
/cmd_vel_mux/input/teleop
/cmd_vel_mux/parameter_descriptions
/cmd_vel_mux/parameter_updates
/diagnostics
/diagnostics_agg
/diagnostics_toplevel_state
/joint_states
/laptop_charge
/mobile_base/commands/controller_info
/mobile_base/commands/digital_output
/mobile_base/commands/external_power
/mobile_base/commands/led1
/mobile_base/commands/led2
/mobile_base/commands/motor_power
/mobile_base/commands/reset_odometry
/mobile_base/commands/sound
/mobile_base/commands/velocity
/mobile_base/controller_info
/mobile_base/debug/raw_control_command
/mobile_base/debug/raw_data_command
/mobile_base/debug/raw_data_stream
/mobile_base/events/bumper
/mobile_base/events/button
/mobile_base/events/cliff
/mobile_base/events/digital_input
/mobile_base/events/power_system
/mobile_base/events/robot_state
/mobile_base/events/wheel_drop
/mobile_base/sensors/bumper_pointcloud
/mobile_base/sensors/core
/mobile_base/sensors/dock_ir
/mobile_base/sensors/imu_data
/mobile_base/sensors/imu_data_raw
/mobile_base/version_info
/mobile_base_nodelet_manager/bond
/odom

博文turtlebot的mobile_base节点解析对这些节点有具体的说明。

运行以下命令：

$ rosrun rqt_graph rqt_graph

在rqt_graph界面进行以下操作：

1. 选择显示所有活跃的节点和话题：Nodes/Topics(active)
2. Namespaces改为0
3. 取消勾选Dead sinks和Leaf topics
4. 刷新页面

如下所示：

可以看到：

可知turtlebot2（或者说底盘Kobuki）的产生的主要节点是/mobile_base_nodelet_manager。

本文仅关心以下话题：

话题名	message类型	说明	类型
/mobile_base/commands/velocity	geometry_msgs::Twist	设置预期速度，控制机器人移动	订阅
/odom	nav_msgs::Odometry	里程计信息	发布
/mobile_base/sensors/imu_data	sensor_msgs::Imu	IMU信息	发布

4. turtlebot2运动控制

4.1 键盘控制运动

$ roslaunch turtlebot_teleop keyboard_teleop.launch

4.2 代码控制运动

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char** argv){
  ros::init(argc, argv, "move_turtle_goforward");
  ros::NodeHandle node;
  ros::Publisher cmdVelPub = node.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity", 1);	
  ros::Rate rate(10);
  geometry_msgs::Twist speed; 
  while (ros::ok()){
    speed.linear.x = 0.1; 
    speed.angular.z = 0.1; 
    cmdVelPub.publish(speed);
    rate.sleep();
  }
  return 0;
}

geometry_msgs::Twist的结构如下，由三维线速度和三维角速度构成。

Vector3  linear
Vector3  angular

turtlebot2的坐标系为x轴指向前方，y轴指向左方，z轴指向上方，坐标系原点位于机器人中心在地面的投影，因此z坐标始终固定为0。

地面机器人只能前进后退，不能上升下降，也不能左右平移，因此只需要设置线速度的x分量；只能绕z轴旋转，不能俯仰和翻滚，因此只需要设置角速度的z分量，也就是偏航角yaw。

线速度和角速度的单位分别为m/s和rad/s。

5. turtlebot2数据读取

5.1 里程计数据

turtlebot2的里程信息通过/odom话题发布，消息类型为nav_msgs::Odometry，对其结构展开如下：

- nav_msgs::Odometry
	-- Header header
		--- uint32 seq
		--- time stamp
		--- string frame_id
	-- string child_frame_id
	-- geometry_msgs/PoseWithCovariance pose
		--- geometry_msgs/Pose pose
			---- geometry_msgs/Point position
				----- float64 x
				----- float64 y
				----- float64 z
			---- geometry_msgs/Quaternion orientation
				----- float64 x
				----- float64 y
				----- float64 z
				----- float64 w
		--- float64[36] covariance
	-- geometry_msgs/TwistWithCovariance twist
		--- geometry_msgs/Twist twist
			---- geometry_msgs/Vector3 linear
				----- float64 x
				----- float64 y
				----- float64 z
			---- geometry_msgs/Vector3 angular
				----- float64 x
				----- float64 y
				----- float64 z
		--- float64[36] covariance

所以里程计数据主要包括

• 时间戳
• 位姿（位置和姿态）
• 速度（线速度和角速度）
• 协方差矩阵（位姿协方差和速度协方差）

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>
void odomCallBack(const nav_msgs::Odometry::ConstPtr& msg){
	/* 时间戳 */
	double timestamp = msg->header.seq;
	/* 平移 */
	int pos_x = msg->pose.pose.position.x;
	int pos_y = msg->pose.pose.position.y;
	int pos_z = msg->pose.pose.position.z;
	/* 旋转（四元数形式） */
	int rot_x = msg->pose.pose.orientation.x;
	int rot_y = msg->pose.pose.orientation.y;
	int rot_z = msg->pose.pose.orientation.z;
	int rot_w = msg->pose.pose.orientation.w;
	/* 速度 */
	float linear = msg->twist.twist.linear.x;
	float angular = msg->twist.twist.angular.z;
	ROS_INFO("Seq: [%d]", timestamp);
	ROS_INFO("Position-> x: [%f], y: [%f], z: [%f]", pos_x, pos_x, pos_z);
	ROS_INFO("Orientation-> x: [%f], y: [%f], z: [%f], w: [%f]", rot_x, rot_y, rot_z, rot_w);
	ROS_INFO("Vel-> Linear: [%f], Angular: [%f]",linear, angular);
}
int main(int argc, char** argv){
	ros::init(argc, argv, "read_odom_data");
	ros::NodeHandle node;
	ros::Subscriber sub = node.subscribe("/odom", 1000, odomCallBack);
	ros::spin();
	return 0;
}

5.2 ＩＭＵ数据

turtlebot2的IMU信息通过/mobile_base/sensors/imu_data话题发布，消息类型为sensor_msgs/Imu，对其结构展开如下：

- sensor_msgs/Imu
	-- std_msgs/Header header
		--- uint32 seq
		--- time stamp
		--- string frame_id
	-- geometry_msgs/Quaternion orientation
		--- float64 x
		--- float64 y
		--- float64 z
		--- float64 w
	-- float64[9] orientation_covariance
	-- geometry_msgs/Vector3 angular_velocity
		--- float64 x
		--- float64 y
		--- float64 z
	-- float64[9] angular_velocity_covariance
	-- geometry_msgs/Vector3 linear_acceleration
		--- float64 x
		--- float64 y
		--- float64 z
	-- float64[9] linear_acceleration_covariance

6. turtlebot2仿真

本节参考：在gazebo中运行turtlebot机器人模拟gmapping的slam过程。

实现目标：使用gazebo提供turtlebot2机器人以及周围环境的仿真模型，通过键盘控制机器人移动，使用gmapping建立栅格地图，在rviz中显示地图。

6.1 仿真环境准备

（1）安装gazebo相关组件

$ sudo apt-get install ros-kinetic-gazebo-ros-pkgs ros-kinetic-gazebo-ros-control

（2）安装turtlebot相关包：

$ sudo apt-get install ros-kinetic-turtlebot-*

6.2 开始仿真

（1）设置环境模型的路径

$ export TURTLEBOT_GAZEBO_WORLD_FILE=/opt/ros/kinetic/share/turtlebot_gazebo/worlds/playground.world

如果不执行这一步，在启动turtlebot_gazebo时会提示以下错误信息：

Invalid tag: environment variable ‘TURTLEBOT_GAZEBO_WORLD_FILE’ is not set.
Arg xml is The traceback for the exception was written to the log file

(2) 启动Gazebo并加载机器人和环境模型

$ roslaunch turtlebot_gazebo turtlebot_world.launch

（3）启动键盘遥控

$ roslaunch turtlebot_teleop keyboard_teleop.launch

（4）使用gmapping进行建图

$ roslaunch turtlebot_gazebo gmapping_demo.launch

（5）启动rviz展示建图过程

$ roslaunch turtlebot_rviz_launchers view_navigation.launch

7. 其他记录

数据发布频率

从turtlebot2的硬件参数可以看出/odom和imu_data的数据发布频率应该是50Hz：
通过rostopic命令也可以确定

$ rostopic hz /odom
$ rostopic hz /mobile_base/sensors/imu_data

补充：kobuki获取传感器数据是通过时间的方式进行数据实时获取的，可参考：kobuki_driver:Sigslots 。

数据延迟

所谓数据延迟，是指数据产生和到达之间延迟

$ rostopic delay /odom
$ rostopic delay /mobile_base/sensors/imu_data

轮子里程计和IMU的数据延迟区别很大，/odom的延迟高达18ms，而imu_data的延迟只有1ms，相差18倍。

为什么有这种现象？

从数据结构看，/odom用了37个float64，imu_data用了85个float64，也就差了2.3倍，所以不完全是因为数据量过大引起。通过查看带宽也可以验证：

$ rostopic bw /odom
$ rostopic bw /mobile_base/sensors/imu_data

/odom和imu_data的带宽分别大约是36KB/s和16KB/s，2.25倍。

协方差

位姿协方差：
$$\left(\begin{array}{cccccc}0.1& 0& 0& 0& 0& 0\\ 0& 0.1& 0& 0& 0& 0\\ 0& 0& 10^{10}& 0& 0& 0\\ 0& 0& 0& 10^{10}& 0& 0\\ 0& 0& 0& 0& 10^{10}& 0\\ 0& 0& 0& 0& 0& 0.5\end{array}\right)$$

速度协方差：
$$\left(\begin{array}{cccccc}0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right)$$

8. 参考

[1] turtlebot的mobile_base节点解析
[2] 简单的代码让turtlebot动起来 （使用kobuki底座）直行与旋转
[3] 简单的代码让turtlebot动起来II（使用kobuki底座）走三角形
[4] geometry_msgs/Twist Message
[5] nav_msgs/Odometry Message
[6] A C++ code to listen odometry messages from a Pioneer P3At robot using ROS.
[7] 在gazebo中运行turtlebot机器人模拟gmapping的slam过程
[8] kobuki doc