ROS学习总结七：ArbotiX+rviz功能仿真

要实现这个仿真，首先需要安装一个ArbotiX功能包：
在src文件夹下新建终端：

$ git clone https://github.com/vanadiumlabs/arbotix_ros.git

注：这是kinetic版本的方式。不同版本不一定相同。
然后再回到上层文件夹：

$ cd ..
$ catkin_make

下面是正式的仿真：
由于这里用到的几个包来自于shenlan的源码，本着对知识产权的尊重，就不粘贴了，有需要的可以自己去找一下这个的资源。
这里主要用到了两个文件，首先是在ROS中启动一个小车的仿真：

$ roslaunch mbot_description arbotix_mbot_with_camera_xacro.launch 

这个launch文件跟之前的差不多，具体代码如下：

<launch>
	<arg name="model" default="$(find xacro)/xacro --inorder '$(find mbot_description)/urdf/xacro/mbot_with_camera.xacro'" />
	<arg name="gui" default="false" />

	<param name="robot_description" command="$(arg model)" />

    <!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="$(arg gui)"/>

      <!-- 启动 arbotix_python 节点-->
	<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
	         <!-- 加载控制器参数-->
        <rosparam file="$(find mbot_description)/config/fake_mbot_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

    <!-- 运行rviz可视化界面 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mbot_description)/config/mbot_arbotix.rviz" required="true" />

</launch>

可以看出它其实跟之前的launch文件是差不多的，只是多了一个启动 arbotix_python 节点的命令，这个文件在之前下的启动 arbotix_ros中可以找到，如果这里节点启动失败可以找到该文件，右键属性查看可执行权限，python文件需要添加可执行权限才能运行。
另外这里还有一个 <rosparam file="$(find mbot_description)/config/fake_mbot_arbotix.yaml" 文件，如果是自己写的包的话这个文件需要自己写一些，具体内容如下：

controllers: {
   base_controller: {
       type: diff_controller, 
       base_frame_id: base_footprint, 
       base_width: 0.26, 
       ticks_meter: 4100, 
       Kp: 12, 
       Kd: 12, 
       Ki: 0, 
       Ko: 50, 
       accel_limit: 1.0 
    }
}

这是一个参数配置文件，定义了一个controller。包括了控制器类型、控制的车体坐标系、轮子的间距、控制频率、PID以及加速度等参数
如果没有问题的话运行上面的launch文件应该能显示出一个类似于前面的简单的四轮机器人，还带了一个摄像头的。这时候我们想让机器人动起来，则需要再启动一个键盘控制节点：

 $ roslaunch mbot_teleop mbot_teleop.launch

这个文件内容如下：

<launch>
    <node name="mbot_teleop" pkg="mobt_teleop" type="mbot_teleop.py" output="screen">
        <param name="scale_liner" value="0.1" type="double"/>
        <param name="scale_angular" value="0.4" type="double"/>
    </node>
</launch>

它其实只是一个调用的文件，主要起作用的是其中的一个mbot_teleop.py文件。下面是两个参数赋值，但是实际上我查看了这个python文件后发现里面并没有这个参数的调用？而我把这两行赋值去掉后整个程序也确实是没有受到任何的影响的。

例如这里的两张图片，上面是源码跑的，下面是把两行赋值去掉以后跑的，其实不影响控制效果。
这里在开启上一个launch文件的时候出现了一个问题：

ImportError: No module named arbotix_msgs.msg

说是找不到这个文件，然后我打开arbotix文件夹下确实是有这个文件夹的，里面包含了两个文件。而且这个包是GitHub上直接下载的应该不会有问题，当时搞了挺久，后来在古月居的专栏里找到了跟我遇到一样问题的小伙伴，参考链接如下：
https://www.guyuehome.com/237
这里有人提到了使用rosdep install命令，但是我没试出来，我采用的是后面那个人提到的

$ source ～/catkin_ws/devel/setup.bash

这里面的catkin_ws为自己的包名称，不一定是这个。
这样运行成功后，我们打开rqt_graph看一下：

从这里大致可以看出来，键盘控制节点发布的消息是给到arbotix的，然后arbotix经过运算再发布出来给到robot_state_publisher。而我们之前使用的xacro所调用的其实都是robot_state_publisher的消息，所以从这里可以看出来一点，那就是我任意更换机器人模型本身对于消息的发布是没有影响的。那么回到最上面的launch文件，我们可以把launch文件修改为：

<launch>
	<arg name="model" default="$(find xacro)/xacro --inorder '$(find exper4241)/xacro/exper_xacro.xacro'" />
	<arg name="gui" default="false" />

	<param name="robot_description" command="$(arg model)" />

    <!-- 设置GUI参数，显示关节控制插件 -->
	<param name="use_gui" value="$(arg gui)"/>

      <!-- 启动 arbotix_python 节点-->
	<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
	         <!-- 加载控制器参数-->
        <rosparam file="$(find mbot_description)/config/fake_mbot_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

    <!-- 运行rviz可视化界面 -->
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find exper4241)/config/mbot_arbotix.rviz" required="true" />

</launch>

其他的不做改变，同样的启动这个launch文件：

$ roslaunch mbot_description arbotix_mbot_with_camera_xacro.launch

这时候你可以得到一辆之前自己写的小车：

这个就是之前上一章内容里面最后完成的小车模型。然后我再去启动一个键盘节点：

$ roslaunch mbot_teleop mbot_teleop.launch

这时候我同样是可以让车子动起来：

第二点，既然小车可以使用自己的代码编写，那么键盘节点是否可以自己写呢，当然是可以的，接下来我们可以试一下。首先我们需要知道键盘节点发布出来的是什么样的内容，而根据shenlan的源代码其teleop的最后面发布的其实是一个twist内容的cmd_vel数据，也就是这个数据被abortix订阅了，本来想看一下具体被arbotix中的那个函数使用了，奈何函数太多找了半天没找到，当然这不重要。我们只需要知道输入是什么就可以了。既然这里给定一个cmd_vel数据就可以，那么接下来我们可以尝试以及写一个。这里我参考的是古月居的一篇博客：
https://blog.csdn.net/hcx25909/article/details/9004617
首先建立一个文件夹：

$ roscreate-pkg exper4251 rospy geometry_msgs std_msgs roscpp rosmake

然后我们进入这个文件夹下的src文件夹，在src文件目录下新建一个名为teleop.py的文件，并复制下列代码：

#!/usr/bin/env python
import roslib; roslib.load_manifest('smartcar_teleop')
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import String
 
class Teleop:
    def __init__(self):
        pub = rospy.Publisher('cmd_vel', Twist)
        rospy.init_node('smartcar_teleop')
        rate = rospy.Rate(rospy.get_param('~hz', 1))
        self.cmd = None
	
        cmd = Twist()
        cmd.linear.x = 0.2
        cmd.linear.y = 0
        cmd.linear.z = 0
        cmd.angular.z = 0
        cmd.angular.z = 0
        cmd.angular.z = 0.5
 
        self.cmd = cmd
        while not rospy.is_shutdown():
            str = "hello world %s" % rospy.get_time()
            rospy.loginfo(str)
            pub.publish(self.cmd)
            rate.sleep()
 
if __name__ == '__main__':Teleop()
————————————————

这样我们得到一个简单的cmd_vel消息发布文本。去执行一下：

rosrun exper4251 teleop.py

可以得到以下结果：

可以看到小车在不停的旋转运动，这说明我们的消息已经是成功的被arbotix接收并使用的。
后面还有另外一个复杂一点的使用键盘控制的代码，我们也可以试一下。
首先我们在src文件夹下新建一个keyboard.cpp文件，复制代码：

#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
 
#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
 
#define KEYCODE_W 0x77
#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64
 
#define KEYCODE_A_CAP 0x41
#define KEYCODE_D_CAP 0x44
#define KEYCODE_S_CAP 0x53
#define KEYCODE_W_CAP 0x57
 
class SmartCarKeyboardTeleopNode
{
    private:
        double walk_vel_;
        double run_vel_;
        double yaw_rate_;
        double yaw_rate_run_;
        
        geometry_msgs::Twist cmdvel_;
        ros::NodeHandle n_;
        ros::Publisher pub_;
 
    public:
        SmartCarKeyboardTeleopNode()
        {
            pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
            
            ros::NodeHandle n_private("~");
            n_private.param("walk_vel", walk_vel_, 0.5);
            n_private.param("run_vel", run_vel_, 1.0);
            n_private.param("yaw_rate", yaw_rate_, 1.0);
            n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
        }
        
        ~SmartCarKeyboardTeleopNode() { }
        void keyboardLoop();
        
        void stopRobot()
        {
            cmdvel_.linear.x = 0.0;
            cmdvel_.angular.z = 0.0;
            pub_.publish(cmdvel_);
        }
};
 
SmartCarKeyboardTeleopNode* tbk;
int kfd = 0;
struct termios cooked, raw;
bool done;
 
int main(int argc, char** argv)
{
    ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
    SmartCarKeyboardTeleopNode tbk;
    
    boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));
    
    ros::spin();
    
    t.interrupt();
    t.join();
    tbk.stopRobot();
    tcsetattr(kfd, TCSANOW, &cooked);
    
    return(0);
}
 
void SmartCarKeyboardTeleopNode::keyboardLoop()
{
    char c;
    double max_tv = walk_vel_;
    double max_rv = yaw_rate_;
    bool dirty = false;
    int speed = 0;
    int turn = 0;
    
    // get the console in raw mode
    tcgetattr(kfd, &cooked);
    memcpy(&raw, &cooked, sizeof(struct termios));
    raw.c_lflag &=~ (ICANON | ECHO);
    raw.c_cc[VEOL] = 1;
    raw.c_cc[VEOF] = 2;
    tcsetattr(kfd, TCSANOW, &raw);
    
    puts("Reading from keyboard");
    puts("Use WASD keys to control the robot");
    puts("Press Shift to move faster");
    
    struct pollfd ufd;
    ufd.fd = kfd;
    ufd.events = POLLIN;
    
    for(;;)
    {
        boost::this_thread::interruption_point();
        
        // get the next event from the keyboard
        int num;
        
        if ((num = poll(&ufd, 1, 250)) < 0)
        {
            perror("poll():");
            return;
        }
        else if(num > 0)
        {
            if(read(kfd, &c, 1) < 0)
            {
                perror("read():");
                return;
            }
        }
        else
        {
            if (dirty == true)
            {
                stopRobot();
                dirty = false;
            }
            
            continue;
        }
        
        switch(c)
        {
            case KEYCODE_W:
                max_tv = walk_vel_;
                speed = 1;
                turn = 0;
                dirty = true;
                break;
            case KEYCODE_S:
                max_tv = walk_vel_;
                speed = -1;
                turn = 0;
                dirty = true;
                break;
            case KEYCODE_A:
                max_rv = yaw_rate_;
                speed = 0;
                turn = 1;
                dirty = true;
                break;
            case KEYCODE_D:
                max_rv = yaw_rate_;
                speed = 0;
                turn = -1;
                dirty = true;
                break;
                
            case KEYCODE_W_CAP:
                max_tv = run_vel_;
                speed = 1;
                turn = 0;
                dirty = true;
                break;
            case KEYCODE_S_CAP:
                max_tv = run_vel_;
                speed = -1;
                turn = 0;
                dirty = true;
                break;
            case KEYCODE_A_CAP:
                max_rv = yaw_rate_run_;
                speed = 0;
                turn = 1;
                dirty = true;
                break;
            case KEYCODE_D_CAP:
                max_rv = yaw_rate_run_;
                speed = 0;
                turn = -1;
                dirty = true;
                break;              
            default:
                max_tv = walk_vel_;
                max_rv = yaw_rate_;
                speed = 0;
                turn = 0;
                dirty = false;
        }
        cmdvel_.linear.x = speed * max_tv;
        cmdvel_.angular.z = turn * max_rv;
        pub_.publish(cmdvel_);
    }
}

同时我们需要修改exper4251文件夹下的cmakelist文件，在该文件的下面添加下列代码：

include_directories(include ${catkin_INCLUDE_DIRS})  

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

这里的写法跟原博客有些不一样，因为我开始照着他那个写出现了一些问题。后来我就改成了这个写法，这个是跟总结一种写talker以及listener的cmakelist文件一样的方式。
然后在exper文件夹下新建终端编译刚才的.cpp文件：

$ cd ~/exper
$ catkin_make

这里应该不会有太大的问题。然后我们同样可以执行一下这个文件：

$ rosrun exper4251 keyboard

然后使用w、A、S、D等键就可以控制小车的移动了。