今天学到了，使用了ros::spin()就是让程序一直在subscriber里面死循环．
更新需求，要把按键设置成控制速度按钮，摇杆作为前进后退的控制．
首先需要测试手柄上所有的按键对应的buttons[]的哪一位：
sudo jstest /dev0/input/js0全部测试后，发现了以下对应信息：
Buttons[]:
A: 0
B: 1
X: 2
Y: 3
L1: 4
R1: 5
select:6
start: 7
mode: 8
//9,10没有分配控制的逻辑还是在按下L1下开始，只有按下L1其他按键按动才会启动，现在的设想是：
Ｙ和A分别控制线速度的增加和减少，Ｘ和Ｂ分别控制角速度的增加和减少;
每个设置４档：０，１，２，３
SELECT设置为最低速度, START设置为最高速度，其它按键暂时不设置．
设置比较简单，但最后的逻辑需要思考一下，先实现：
首先在launch里面更改：
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
<node pkg="ros_handle_control" type="teleop_turtle4" name="teleop4" output="screen" />
<node pkg="ros_handle_control" type="teleop_turtle5" name="teleop5" 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" />
<!-- 线速度系数设置 -->
<param name="linear_ratio" value="0.5" type="double" />
<!-- 角速度系数设置 -->
<param name="angular_ratio" value="0.5" type="double" />
<!-- 线速度换档 -->
<param name="linear_up" value="3" type=int />
<param name="linear_down" value="0" type=int />
<!-- 角速度换挡 -->
<param name="angular_up" value="1" type=int />
<param name="angular_down" value="2" type=int />
<!-- 设置最低速度和最高速度 -->
<param name="min_speed" value="6" type=int />
<param name="max_speed" value="7" type=int />
<param name="ton" value="4" type="int" />
<node respawn="true" pkg="joy" type="joy_node" name="joystick" />
</launch>然后现在teleop_turtle4.cpp里面先修改:
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
int axis_linear, axis_angular;
int sticks_left, sticks_right;
int ton;
double linear_ratio, angular_ratio;
int linear_up, linear_down, angular_up, angular_down;
int max_speed, min_speed;
double vel_x, vel_z;
bool L1_press = false;
void linear_speed_updown(const sensor_msgs::Joy::ConstPtr &joy)
{
if(joy->buttons[linear_up])
{
vel_x += linear_ratio;
}
else if(joy->buttons[linear_down])
{
vel_x -= linear_ratio;
}
}
void angular_speed_updown(const sensor_msgs::Joy::ConstPtr &joy)
{
if(joy->buttons[angular_up])
{
vel_z += angular_ratio;
}
else if(joy->buttons[angular_down])
{
vel_z -= angular_ratio;
}
}
void max_min_charge(double& v)
{
if(v > 0)
v > 4 ? v = 4 : v;
else if(v < 0)
v < -4 ? v = -4 : v;
}
void callback(const sensor_msgs::Joy::ConstPtr &joy)
{
ROS_INFO("6546546546546");
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];
linear_speed_updown(joy);
}
else if(joy->axes[sticks_left] || joy->axes[sticks_right])
{
vel_x = joy->axes[sticks_left];
vel_z = joy->axes[sticks_right];
angular_speed_updown(joy);
}
linear_speed_updown(joy);
angular_speed_updown(joy);
if(joy->buttons[min_speed])
{
vel_x = 0.5;
vel_z = 0.5;
}
else if(joy->buttons[max_speed])
{
vel_x = 4;
vel_z = 4;
}
max_min_charge(vel_x);
max_min_charge(vel_z);
ROS_INFO("当前线速度为:%.3lf ; 角速度为:%.3lf", vel_x, vel_z);
}
else
{
vel_x = 0;
vel_z = 0;
}
}
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "sub_joy_pub_msgs");
ros::NodeHandle nh;
ros::Rate r(20);
ros::Subscriber sub;
ros::Publisher pub;
nh.param<int>("linear_up", linear_up, 3);
nh.param<int>("linear_down", linear_down, 0);
nh.param<int>("angular_up", angular_up, 1);
nh.param<int>("angular_down", angular_down, 2);
nh.param<double>("linear_ratio", linear_ratio, 0.5);
nh.param<double>("angular_ratio", angular_ratio, 0.5);
nh.param<int>("max_speed", max_speed, 6);
nh.param<int>("min_speed", min_speed, 7);
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);
ros::spin();
return 0;
}第一版写的很繁琐，需要修改，相关的逻辑也很乱，怎么才能做到在按下L１的时候按方向键控制方向，并且在按动Ｙ和Ａ的时候进行速度的加减，之前尝试了一种方法没有完美实现，只能在按下Ｌ１后确定方向按键后松开按键，然后再对速度进行加减．但不太行，想了一下，
准备在设置多个bool来判断按键，然后在进一步处理，这样对于数据的处理更加合理．
想了很久，发现可以根据每一个按键设置对应的速度，最后在把所有的速度全部加起来，这样就可以完美解决按键匹配的功能．
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/Joy.h>
int axis_linear, axis_angular;
int sticks_left, sticks_right;
int ton;
double linear_ratio, angular_ratio;
int linear_up, linear_down, angular_up, angular_down;
int max_speed, min_speed;
double vel_x, vel_z;
double vx_scale = 0, vz_scale = 0;
double vx = 0, vz = 0;
bool min_flag = false, max_flag = false;
bool use_min_max_speed = false;
int cnt = 0;
ros::Subscriber sub;
ros::Publisher pub;
void linear_speed_updown(const sensor_msgs::Joy::ConstPtr &joy)
{
if(joy->buttons[linear_up])
{
min_flag = false;
max_flag = false;
vx_scale += linear_ratio;
}
else if(joy->buttons[linear_down])
{
min_flag = false;
max_flag = false;
vx_scale -= linear_ratio;
}
}
void angular_speed_updown(const sensor_msgs::Joy::ConstPtr &joy)
{
if(joy->buttons[angular_up])
{
min_flag = false;
max_flag = false;
vz_scale += angular_ratio;
}
else if(joy->buttons[angular_down])
{
min_flag = false;
max_flag = false;
vz_scale -= angular_ratio;
}
}
//这里可以加前进后退方向的判断
void max_min_speed(const sensor_msgs::Joy::ConstPtr &joy)
{
if(joy->buttons[min_speed])
{
min_flag = true;
vx = 0;
vz = 0;
vel_x = 0;
vel_z = 0;
vx_scale = 0;
vz_scale = 0;
}
else if(joy->buttons[max_speed])
{
max_flag = true;
vx = 0;
vz = 0;
vel_x = 0;
vel_z = 0;
vx_scale = 0;
vz_scale = 0;
}
}
void max_min_charge(double& v)
{
if(v > 0)
v > 1 ? v = 1 : v;
else if(v < 0)
v < -1 ? v = -1 : v;
}
void callback(const sensor_msgs::Joy::ConstPtr &joy)
{
if(joy->buttons[ton])
{
if(joy->axes[axis_linear]||joy->axes[axis_angular])
{
linear_speed_updown(joy);
angular_speed_updown(joy);
vel_x = joy->axes[axis_linear] * 0.1;
vel_z = joy->axes[axis_angular] * 0.1;
use_min_max_speed = true;
}
vx = vel_x + vx_scale;
vz = vel_z + vz_scale;
if(min_flag == true)
{
vx = 0.1;
}
if(max_flag == true)
{
vx = 1;
}
if((joy->buttons[min_speed] || joy->buttons[max_speed]) && use_min_max_speed)
{
if(joy->buttons[min_speed])
{
min_flag = true;
}
else if(joy->buttons[max_speed])
{
max_flag = true;
}
use_min_max_speed = false;
}
max_min_charge(vx);
max_min_charge(vz);
}
else
{
vx = 0;
vz = 0;
vel_x = 0;
vel_z = 0;
vx_scale = 0;
vz_scale = 0;
cnt = 0;
}
ROS_INFO("当前线速度为:%.3lf ; 角速度为:%.3lf", vx, vz);
}
int main(int argc, char** argv)
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "sub_joy_pub_msgs");
ros::NodeHandle nh;
ros::Rate r(10);
nh.param<int>("linear_up", linear_up, 3);
nh.param<int>("linear_down", linear_down, 0);
nh.param<int>("angular_up", angular_up, 1);
nh.param<int>("angular_down", angular_down, 2);
nh.param<double>("linear_ratio", linear_ratio, 0.5);
nh.param<double>("angular_ratio", angular_ratio, 0.5);
nh.param<int>("max_speed", max_speed, 6);
nh.param<int>("min_speed", min_speed, 7);
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);
pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
while(ros::ok())
{
geometry_msgs::Twist v;
v.linear.x = vx;
v.angular.z = vz;
pub.publish(v);
ros::spinOnce();
}
return 0;
}但在设置最低速度和最高速度的时候出了，问题，必须要一直按着selec和start按键，才能设置成相应的速度，按一下就松开就会刷新走，最后请教了师兄，把判断的flag的放置进速度调整里面就行了．还是实际开发能力太弱了，得加强！
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
