SLAM从入门到精通（用c++实现机器人运动控制）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        之前的一篇文章，我们知道了可以通过wpr_simulation包仿真出机器人和现场环境。如果需要控制机器人，这个时候就需要rqt_robot_steering这个包来实现。可是大家有没有想过，为什么用rqt_robot_steering就可以实现对车的控制，如果是我们自己用c++编程，是不是也可以实现相同的控制呢？

1、rqt_bot_steering控制的原因

        不知道大家对这样的一幅图，究竟还有没有印象。rqt_robot_steering能对小车进行控制，本质原因还是因为它给/cmd_vel发送了消息。简而言之，如果我们能编写对应的消息，那么其实也可以实现对小车的控制。

2、编写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 r(30);
 
	while(ros::ok())
	{
		vel_pub.publish(vel_msg);
		r.sleep();
	}
 
	return 0;
}

        代码的编写部分主要集中在vel_msg的发布。这里面有两个知识点，一个是/cmd_vel的发布；一个是vel_msg的填写。其中linear下面的x、y、z指的是三个方向的线速度，而angular下面的x、y、z则指的是围绕三个轴的角速度。线速度的单位一般是m/s，而角速度一般单位是弧度/s。

3、更新CMakeLists.txt文件

add_executable(vel_node src/vel_node.cpp)
target_link_libraries(vel_node ${catkin_LIBRARIES})
add_dependencies(vel_node beginner_tutorials_generate_messages_cpp)

4、准备编译

        编译的方法其实很简单，和之前ros的编译方法是一样的。即在catkin_ws输入catkin_make即可。如果没有问题，就可以生成vel_node程序。

5、验证vel_node是否ok

        要验证vel_node程序是否真的发挥了作用，主要流程就是两个步骤。第一，用roslaunch启动wpr_simulation，

roslaunch wpr_simulation wpb_simple.launch

        第二，用rosrun启动vel_node，回头看gazebo里面的小车，如果小车式向前进行了移动，那么说明我们编写的vel_node程序是成功的。反之，如果没有看到这样的现象，那么就要回头看下，究竟我们在编写代码的过程中引入了什么错误。

rosrun beginner_tutorials vel_node

注1：

        不管是仿真，还是真实的机器人，线速度和角速度是机器人底盘控制的重要目标，建议这部分好好掌握。对于平面小车来说，一般linear的z为0，angular的x和y为0。

注2：

        常见的控制目标如下所示，

rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 1, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'
1米/秒往前走
 
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 6.28}}'
原地打转
 
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: -1, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'
1米/秒往后走
 
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.3, y: 0, z: 0}, angular: {x: 0, y: 0, z: 1.5}}'
圆形轨迹