使用游戏手柄控制ROS机器人速度

使用游戏手柄控制ROS机器人速度

买了一个带usb接收的游戏手柄，想要用来遥控机器人，写一个节点来实现：
该节点可以把手柄摇杆的信号转换成cmd_vel消息发布到系统中。
首先用turtle_sim中的“小王八”来测试，然后在机器人上使用。
需要注意的是：cmd_vel需要持续发布

#include "geometry_msgs/Twist.h"
#include "sensor_msgs/Joy.h"
#include "ros/ros.h"
#include "iostream"

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

int num_axe_lin, num_axe_ang;//选择手柄的数据
double factor_axe_lin, factor_axe_ang;//控制系数
int Button_Select;//对应手柄select键
std::string pub_name;//发布的消息名称
geometry_msgs::Twist speed;

void joy_callBack(const sensor_msgs::Joy::ConstPtr &joy)
{
    double linear_speed = joy->axes[num_axe_lin]*factor_axe_lin;
    double angular_speed = joy->axes[num_axe_ang]*factor_axe_ang;
    Button_Select = joy->buttons[10];

    speed.linear.x = linear_speed;
    speed.angular.z = angular_speed;

    ROS_INFO("joystick linear:%f, angular:%f\n",speed.linear.x, speed.angular.z);
    if(Button_Select == 1)
        ROS_INFO("Button_Select\n");
    
}

int main(int argc, char **argv){

    ros::init(argc,argv,"Joystick");

    ros::NodeHandle nh;

    //配置默认参数，可从launch文件中配置自定义参数
    std::string default_pub_name = "/turtle1/cmd_vel";
    nh.param("num_axe_lin",num_axe_lin,1);
    nh.param("num_axe_ang",num_axe_ang,2);
    nh.param("factor_axe_lin",factor_axe_lin,1.0);
    nh.param("factor_axe_ang",factor_axe_ang,1.0);
    nh.param("pub_name",pub_name,default_pub_name);

    sub = nh.subscribe<sensor_msgs::Joy>("/joy",10,&joy_callBack);
    pub = nh.advertise<geometry_msgs::Twist >(pub_name,10);

    ros::Rate r(10);//发布频率

    while(nh.ok()){
        ros::spinOnce();
        pub.publish(speed);
        r.sleep();
    }

    return 0;

}

小王八测试用的launch文件：

<launch>
    <node name="turtlesim" pkg="turtlesim" type="turtlesim_node" />
    <node name="Joystick" pkg="joystick" type="joystick_node" output="screen"/>

        <param name="num_axe_lin" value="1" type="int"/>
        <param name="num_axe_ang" value="2" type="int"/>
        <param name="factor_axe_lin" value="1.0" type="double"/>
        <param name="factor_axe_ang" value="1.0" type="double"/>
        <param name="pub_name" value="turtle1/cmd_vel" type="string"/>
    
</launch>

测试结果：通过