用ＲＯＳ制作我们的机器人小车（二）： 编码篇

参考文献：

［1］ 《Learning ROS for Robostic Programming》 chapter 5

前言：

在参考资料［1］里，作者展示了如何用代码来操纵小车机械臂的移动。

但笔者认为，对于初学者来说，移动小车要比操作机械比来得实用得多，而且重用性也更高。

因此在本文中，我们将展示如果通过代码来控制小车的移动。

实现：

首先是中学物理……一切运动都是相对的，所以要定义小车的移动，没有参考点就无从谈起。

所以参考点就成了定义问题的关键。

我们从代码开始。car_state_publisher.cpp

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    const double degree = M_PI/180;
    const double radius = 2;

    // robot state
    double angle= 0;
    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(4);
        joint_state.position.resize(4);
	joint_state.name[0] ="base_to_wheel1";
        joint_state.position[0] = 0;
	joint_state.name[1] ="base_to_wheel2";
        joint_state.position[1] = 0;
	joint_state.name[2] ="base_to_wheel3";
        joint_state.position[2] = 0;
	joint_state.name[3] ="base_to_wheel4";
        joint_state.position[3] = 0;

        // update transform
        // (moving in a circle with radius)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = radius * cos(angle);
        odom_trans.transform.translation.y = radius * sin(angle);
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);

        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);


	// Create new car angle
        angle += degree/4;

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }

    return 0;
}

编译代码得到可执行文件car_state_publisher后，依次执行以下命令。

用rviz打开你的小车模型

roslaunch urdf_tutorial xacrodisplay.launch model:=04_xacro.xacro

打开state_publisher

../car_state_publisher

咦？小车怎么没反应？这是因为rviz默认将base_link参照坐标，而base_link就是车身，小车相对车身当然不会移动。

按下图所示，将参考坐标设为odom。ＯＫ，你会看到小车绕着坐标原点绕圈啦。

分析

这段代码很简单，基本就做了两件事情。

第一：向joint_state发送joint的状态。但由于这里我们的小车只有四个轮子，而且我们也没有打算模拟轮子的滚动。

所以我们只是把、所有的joint位置置0。.

        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(4);
        joint_state.position.resize(4);
	joint_state.name[0] ="base_to_wheel1";
        joint_state.position[0] = 0;
	joint_state.name[1] ="base_to_wheel2";
        joint_state.position[1] = 0;
	joint_state.name[2] ="base_to_wheel3";
        joint_state.position[2] = 0;
	joint_state.name[3] ="base_to_wheel4";
        joint_state.position[3] = 0;

第二：建立了一个tf节点。这里我们进行的是简单的坐标变换。

我们不但控制tf围绕原点作圆周运动，并且通过控制rotation来保持小车方向始终为圆的切线。

因此只要选择odom作为参照坐标，我们的小车就开始做圆周运动啦。

OK，通过这种方法，你就可以很容易的控制你的小车。

        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = radius * cos(angle);
        odom_trans.transform.translation.y = radius * sin(angle);
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle);