ROS 罗技手柄控制机器人（仿真和实体机器人）

最新推荐文章于 2024-07-26 16:43:47 发布

Will_Ye

最新推荐文章于 2024-07-26 16:43:47 发布

阅读量3.6k

点赞数 3

分类专栏： ROS learning 文章标签： ROS

本文链接：https://blog.csdn.net/Will_Ye/article/details/117001748

版权

ROS learning 专栏收录该内容

42 篇文章 18 订阅

订阅专栏

20210615更新

测试的时候发现一个问题，之前代码的逻辑是每次按一下摇柄就发一个命令，但是如果一直按着摇柄往一个方向，也是发送一次命令，摇柄转到其他方向，信号发生变化才再次发送一个速度命令，这点其实在仿真里面没有问题。
但是，在实际控制机器人时，就会出现，比如要机器人往前，把摇杆推前，并保持，这时发送一个速度指令可能让机器人走一个loop的时间，然后就停下，这个问题也跟实际机器人的底层控制有关，如果这个机器人是接收到一个命令就一直执行，就不会有这个问题，但是实体机器人一般不会这样做控制，也太危险了。理想情况应该是，当我们控制摇杆往不同方向时，应该一直在发送速度指令，让机器人一直执行不同的指令。
综上所述，改了一下代码，如下：

#include<ros/ros.h>
#include<string>
#include<iostream>
#include<geometry_msgs/Pose.h>
#include<nav_msgs/Odometry.h>
#include<sensor_msgs/Joy.h>
#include<geometry_msgs/Twist.h>
#include<time.h>

double vlinear,vangular;
int axis_ang,axis_lin_x,axis_lin_y,ton;
double vx, vy, vtheta_z;
/*
class Teleop
{
public:
    Teleop();
 
private:
    void callback(const sensor_msgs::Joy::ConstPtr& Joy);
    ros::NodeHandle n;
    ros::Subscriber sub ;
    ros::Publisher pub ;
    double vlinear,vangular;
    int axis_ang,axis_lin_x,axis_lin_y,ton;
};
 
Teleop::Teleop()
{
    n.param<int>("axis_linear_x",axis_lin_x,7);
    n.param<int>("axis_linear_y",axis_lin_y,6);
    n.param<int>("axis_angular",axis_ang,3);
    n.param<double>("vel_linear",vlinear,0.15);
    n.param<double>("vel_angular",vangular,0.25);
    n.param<int>("button",ton,4);
    pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1);
    sub = n.subscribe<sensor_msgs::Joy>("joy",10,&Teleop::callback,this);
}
*/
void callback(const sensor_msgs::Joy::ConstPtr& Joy)
{
    geometry_msgs::Twist v;

   if(Joy->buttons[ton])
     {
     vx =(Joy->axes[axis_lin_x])*vlinear;
     vy =(Joy->axes[axis_lin_y])*vlinear;
     vtheta_z = (Joy->axes[axis_ang])*vangular;
     ROS_INFO("linear x y:%.3lf %.3lf  angular:%.3lf",vx,vy,vtheta_z);
    //  pub.publish(v);
     }
}
 
int main(int argc,char** argv)
{
    ros::init(argc, argv, "joy");

    ros::NodeHandle n;
    ros::Subscriber sub ;
    ros::Publisher pub ;

    ros::Rate r(20);

    n.param<int>("axis_linear_x",axis_lin_x,7);
    n.param<int>("axis_linear_y",axis_lin_y,6);
    n.param<int>("axis_angular",axis_ang,3);
    n.param<double>("vel_linear",vlinear,0.15);
    n.param<double>("vel_angular",vangular,0.25);
    n.param<int>("button",ton,4);
    sub = n.subscribe<sensor_msgs::Joy>("joy",10,callback);
    pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1);

    while(n.ok()){

        geometry_msgs::Twist v;

        // if(Joy->buttons[ton])
        //     {
        v.linear.x =vx;
        v.linear.y =vy;
        v.angular.z = vtheta_z;
        // ROS_INFO("linear x y:%.3lf %.3lf  angular:%.3lf",v.linear.x,v.linear.y,v.angular.z);
        pub.publish(v);
            // }
        ros::spinOnce();
        r.sleep();

    }
    return 0;
}

这个就可以满足控制真实机器人时的需求。

机器人在实际使用中，经常需要通过外设来控制它完成一些简单的任务。

可以使用键盘，也可以使用罗技的手柄，手柄比较方便。键盘的代码就不放了，太多了。记录一下手柄控制方法。

要控制的对象是一个全向轮小车，所以会有一个y方向的横移，以及绕z轴自转。不管是holonomic还是non-holonomic的小车，就差一个y方向。

Mehtod：

安装手柄驱动：
sudo apt-get install ros-melodic-joy
sudo apt-get install joystick
根据实际ROS版本，选择安装包
查看手柄串口号
ls -l /dev/input/js0
测试手柄信号
sudo jstest /dev/input/js0
运行ROS节点
rosrun joy joy_node

查看手柄发出的信号
rostopic echo joy
按几下手柄，看看输出的信号对不对

axes和buttons中的数值都在-1到1之间，然后搞清楚每个axes和buttons对应手柄上的按钮就可以开始写代码了，先看下面几张图：
写一个订阅手柄信息和发布/cmd_vel指令的node
先上代码：

#include<ros/ros.h>
#include<string>
#include<iostream>
#include<geometry_msgs/Pose.h>
#include<nav_msgs/Odometry.h>
#include<sensor_msgs/Joy.h>
#include<geometry_msgs/Twist.h>
#include<time.h>

class Teleop
{
public:
    Teleop();
 
private:
    /* data */
    void callback(const sensor_msgs::Joy::ConstPtr& Joy);
    ros::NodeHandle n;
    ros::Subscriber sub ;
    ros::Publisher pub ;
    double vlinear,vangular;
    int axis_ang,axis_lin_x,axis_lin_y,ton;
};
 
Teleop::Teleop()
{
    n.param<int>("axis_linear_x",axis_lin_x,7);
    n.param<int>("axis_linear_y",axis_lin_y,6);
    n.param<int>("axis_angular",axis_ang,3);
    n.param<double>("vel_linear",vlinear,0.15);
    n.param<double>("vel_angular",vangular,0.25);
    n.param<int>("button",ton,4);
    pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",1);
    sub = n.subscribe<sensor_msgs::Joy>("joy",10,&Teleop::callback,this);
}
 
void Teleop::callback(const sensor_msgs::Joy::ConstPtr& Joy)
{
    geometry_msgs::Twist v;

   if(Joy->buttons[ton])
     {
     v.linear.x =(Joy->axes[axis_lin_x])*vlinear;
     v.linear.y =(Joy->axes[axis_lin_y])*vlinear;
     v.angular.z = (Joy->axes[axis_ang])*vangular;
     ROS_INFO("linear x y:%.3lf %.3lf  angular:%.3lf",v.linear.x,v.linear.y,v.angular.z);
     pub.publish(v);
     }
}
 
int main(int argc,char** argv)
{
    ros::init(argc, argv, "joy");
    Teleop telelog;
    ros::spin();
    return 0;
}

代码中设定的初始速度比较低，成功测试后可以适当调高vlinear和vangular。

需要用到多少个按钮，就根据实际情况定就好了，这里写的控制逻辑是，每次发送指令时都是按住左上角的LB键，然后在按遥感A和B来发送速度指令，这也是为了安全，防止使用时不小心碰到遥感使机器人撞到其他东西。

完成上述部分后，就可以写一个launch文件和一个shell文件，以后就能一个指令启动，不用一次次打开多个文件。
先写一个名叫handle_control.launch文件：

<?xml version = "1.0"?>
<launch>

<node name="base_node_serial" pkg="gazebo_mobile_manipulator" type="base_node_serial" />
<node name="handle_control" pkg="gazebo_mobile_manipulator" type="handle_control" />

</launch>

其中的pkg名字，根据自己的package修改

然后建一个名为handle_control.sh的shell文件：

#!/bin/bash
# This is our first script.rosnode kill --all;
echo 'Handle control Start!'

gnome-terminal --tab --title="joy" --command="bash -c 'rosrun joy joy_node; $SHELL'"

gnome-terminal --tab --title="handle control" --command="bash -c 'source catkin_ws/devel/setup.bash; roslaunch gazebo_mobile_manipulator handle_control.launch; $SHELL'"

之后使用时，只需要在terminal中输入bash handle_control.sh即可