ros手柄控制机器人小车(二)

接着上一篇的文章．上一篇实现了可以用摇杆控制海龟的运动控制，但按键部分不行，仔细看了下rostopic echo /joy后，应该是关于axis的设置问题

 比较容易实现，直接在demo.launch文件里面添加关于按键的参数设置：

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
   
    <node pkg="ros_handle_control" type="teleop_turtle" name="teleop" output="screen" />

<!-- 方向按键控制 -->
    <param name="axis_linear" value="7" type="int" />
    <param name="axis_angular" value="6" type="int" />
<!-- 摇杆控制 -->
    <param name="sticks_left" value="1" type="int" />
    <param name="sticks_right" value="3" type="int" />

    <node respawn="true" pkg="joy" type="joy_node" name="joystick" />

</launch>

然后在源文件中修改一下，就可以了：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>

class TeleopTurtle
{
public:
    TeleopTurtle();

private:
    // 处理手柄发送过来的信息
    void callback(const sensor_msgs::Joy::ConstPtr &joy);
    // 实例化ROS句柄
    ros::NodeHandle nh;
    // 定义订阅者对象，用来订阅手柄发送的数据
    ros::Subscriber sub;
    // 定义发布者对象，用来将手柄数据发布到乌龟控制话题上
    ros::Publisher pub;
    // 用来接收launch文件中设置的参数，绑定手柄摇杆、轴的映射
    int axis_linear, axis_angular;
    int sticks_left, sticks_right;
};

TeleopTurtle::TeleopTurtle()
{
    // 从参数服务器读取的参数
    // 按手柄摇杆分配
    nh.param<int>("sticks_left", sticks_left, 1);
    nh.param<int>("sticks_right", sticks_right, 3);
    // 按键分配
    nh.param<int>("axis_linear", axis_linear, 1);
    nh.param<int>("axis_angular", axis_angular, 2);
    // Axes:  0:     0  1:     0  2:　0　  3:     0  4:     0  5:　0　  6:     0  7:     0 
    // 对应：0: 左摇杆轴左右
    // 　　　1: 左摇杆轴前后
    // 　　　2: 默认全不开放
    // 　　　3: 右摇杆轴左右
    // 　　　4: 右摇杆轴前后
    // 　　　5: 默认全不开放
    //　　　 6: 方向按键左+右
    // 　　　7: 方向按键前+后  
    pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    sub = nh.subscribe<sensor_msgs::Joy>("joy", 10, &TeleopTurtle::callback, this);
}

void TeleopTurtle::callback(const sensor_msgs::Joy::ConstPtr &joy)
{
    geometry_msgs::Twist vel;

    if(joy->axes[axis_linear] || joy->axes[axis_angular])
    {
        vel.linear.x = joy->axes[axis_linear];
        vel.angular.z = joy->axes[axis_angular];
    }
    else if(joy->axes[sticks_left] ||  joy->axes[sticks_right])
    {
        vel.linear.x = joy->axes[sticks_left];
        vel.angular.z = joy->axes[sticks_right];
    }
    // // 将手柄摇杆轴拨动时值的输出赋值给乌龟的线速度和角速度
    // vel.linear.x = joy->axes[axis_linear];
    // vel.angular.z = joy->axes[axis_angular];
    // // 按键按动时也可以输出乌龟的线速度和角速度
    // vel.linear.x = joy->axes[sticks_left];
    // vel.angular.z = joy->axes[sticks_right];
    ROS_INFO("当前线速度为:%.3lf ; 角速度为:%.3lf", vel.linear.x, vel.angular.z);
    pub.publish(vel);
}

int main(int argc, char **argv)
{
    // 设置编码
    setlocale(LC_ALL, "");
    // 初始化ROS节点
    ros::init(argc, argv, "teleop_turtle");
    TeleopTurtle teleopTurtle;
    ros::spin();
    return 0;
}

代码中的注释写的很清楚了，主要还是对于axis[]不同按键的设置问题，最开始设置好了发现只能在callback函数里面最前面的可以运行，后面就不行了，可能是因为每次只能接受最前面的按键指令？改了if-else if判断后就好很多了．

但现在还存在一个问题，就是它每回按键只能运行一下，就是当前设置的运动周期，一直按着也只能运行一个周期，必须有按键的改变才能继续运行．这应该是没有设置loop循环的原因，重新设置一下，具体实现参考了这篇文章．

具体改变有两处：
１．添加了安全保护按钮，只有当L1按动的时候，控制按键才会启动

２．设置了按住不动的时候一直发送控制指令

具体代码实现如下：

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>

void callback(const sensor_msgs::Joy::ConstPtr &joy);

int axis_linear, axis_angular;
int sticks_left, sticks_right;
int ton;
double vel_x, vel_z;

int main(int argc, char **argv)
{
    // 设置编码
    setlocale(LC_ALL, "");

    ros::init(argc, argv, "teleop_turtle");
    ros::NodeHandle nh;

    ros::Subscriber sub;
    ros::Publisher pub;

    ros::Rate r(20);


    nh.param<int>("ton", ton, 4);
    // 按手柄摇杆分配
    nh.param<int>("sticks_left", sticks_left, 7);
    nh.param<int>("sticks_right", sticks_right, 6);
    // 按键分配
    nh.param<int>("axis_linear", axis_linear, 1);
    nh.param<int>("axis_angular", axis_angular, 2);

    sub = nh.subscribe<sensor_msgs::Joy>("joy", 10, callback);
    // 如果想要换成机器人控制就把"/turtle1/cmd_vel"更换成要求的
    pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    while(nh.ok())
    {
        geometry_msgs::Twist v;
        v.linear.x = vel_x;
        v.angular.z = vel_z;
        if(v.linear.x != 0 || v.angular.z != 0)
        {
            pub.publish(v);
        }
        else
        {
            v.linear.x = 0;
            v.angular.z = 0;    
            pub.publish(v);
        }

        ros::spinOnce();
        r.sleep();
    }
    return 0;
}

void callback(const sensor_msgs::Joy::ConstPtr &joy)
{
    if(joy->buttons[ton])
    {
        if(joy->axes[axis_linear] || joy->axes[axis_angular])
        {
            vel_x = joy->axes[axis_linear];
            vel_z = joy->axes[axis_angular];
        }
        else if(joy->axes[sticks_left] ||  joy->axes[sticks_right])
        {
            vel_x = joy->axes[sticks_left];
            vel_z = joy->axes[sticks_right];
        }
        ROS_INFO("当前线速度为:%.3lf ; 角速度为:%.3lf", vel_x, vel_z);
    }
    else
    {
        vel_x = 0;
        vel_z = 0;
    }
}

还有cmakelists.txt和launch里面修改一下．

最开始写的时候发现按一下后，就会一直发送速度指令，与设想不符合，然后就加了一个判断当没有按键的时候就会发送速度为零，并且以L4为安全按键，当不按L4的时候，就会停止．

基本实现了海龟的控制，之后在移植机器人还要更改发布的topic和设置开机启动，就是之后的事了．