ROS：：用摇杆控制你的机器人（实现篇）

参考代码 https://github.com/AaronMR/Learning_ROS_for_Robotics_Programming

《Learning ROS for Robotics Programming》，我们这里简称《LRRP》

由于最新的ROS已经更新到了Hydro，看上去我们这份教材的示例程序还没能跟上新版本。不过改动不大，让我们来稍稍调整一下这个程序吧。

所有程序在ROS Hydro上测试。

获取参考代码

《LRRP》示例代码，我们作为参考。

git clone https://github.com/AaronMR/Learning_ROS_for_Robotics_Programming

乌龟机器人的实例代码，这里以此为基础进行修改。

git clone https://github.com/ros/ros_tutorials.git

然后找到以下三个文件

ros_tutorials/turtlesim/tutorials/teleop_turtle_key.cpp

ros_tutorials/turtlesim/src/turtlesim.cpp

Learning_ROS_for_Robotics_Programming/chapter4_tutorials/src/example1.cpp

分析代码

teleop_turtle_key.cpp是用键盘控制乌龟运动的测试程序。

程序很简单，我们用一个循环来监听按键事件， 然后向Topic turtle1/cmd_vel中发送控制乌龟的消息。

对比代码我们可以找到不少有用的部分。比如《LRRP》的topic该换了。

turtlesim.cpp是乌龟的主程序。

我们可以看到， 我们使用x作为前进后退的控制，z作为乌龟方向的控制

  lin_vel_ = vel->linear.x;
  ang_vel_ = vel->angular.z;

example1.cpp是《LRRP》的示例程序。

从中我们可以知道，要获取joystick的事件，只要监听/joy topic并且等待回调就可以了。

修改程序

1. 作为最快速度，我们可以简单拷贝teleop_turtle_key.cpp并改名为teleop_turtle_joystick.cpp放在相同目录下。

并且把TeleopTurtle重命名为TeleopTurtleJoyStick。

2. 这里我们不需要在keyloop, 我们可以把它删除

同时由于我们需要使用ros::spin，quit函数也不需要了，可以删除相关代码。

3. 修改的重点在于添加一个Subscriber来监听Topic joy，并且注册回调函数。

ros::Subscriber twist_sub_;
twist_sub_ = nh_.subscribe("joy", 10, &TeleopTurtleJoyStick::callBack, this);

另外在回调函数中把joystick事件转换成乌龟控制事件。从turtlesim.cpp我们可以知道需要如何去做数据转换。

void TeleopTurtleJoyStick::callBack(const sensor_msgs::Joy::ConstPtr& joy)
{
	geometry_msgs::Twist twist;
	twist.linear.x = joy->axes[1]; //speed
	twist.angular.z = joy->axes[0]; //angle


    twist_pub_.publish(twist);
}

4. 修改Makefile，把teleop_turtle_joystick.cpp加入编译

add_executable(turtle_teleop_joystick tutorials/teleop_turtle_joystick.cpp)
target_link_libraries(turtle_teleop_joystick ${catkin_LIBRARIES})
add_dependencies(turtle_teleop_joystick turtlesim_gencpp)

5. 运行

roscore

rosrun joy joy_node

rosrun turtlesim turtlesim_node

./turtle_teleop_joystick

6. 可能会用到的调试手段

 rostopic echo /joy

最后的程序大致如下所示

#include 
#include 
#include 
#include 
#include 
#include 

class TeleopTurtleJoyStick
{
public:
  TeleopTurtleJoyStick();
  void keyLoop();
  void callBack(const sensor_msgs::Joy::ConstPtr& joy);

private:
  ros::NodeHandle nh_;
  double linear_, angular_, l_scale_, a_scale_;
  ros::Publisher twist_pub_;
  ros::Subscriber twist_sub_;
};

TeleopTurtleJoyStick::TeleopTurtleJoyStick():
  linear_(0),
  angular_(0),
  l_scale_(2.0),
  a_scale_(2.0)
{
  nh_.param("scale_angular", a_scale_, a_scale_);
  nh_.param("scale_linear", l_scale_, l_scale_);

  twist_pub_ = nh_.advertise("turtle1/cmd_vel", 1);
  twist_sub_ = nh_.subscribe("joy", 10, &TeleopTurtleJoyStick::callBack, this);
}

void TeleopTurtleJoyStick::callBack(const sensor_msgs::Joy::ConstPtr& joy)
{
	geometry_msgs::Twist twist;
	twist.linear.x = joy->axes[1]; //speed
	twist.angular.z = joy->axes[0]; //angle

    twist_pub_.publish(twist);
}

int kfd = 0;
struct termios cooked, raw;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop_turtle_joystick");
  TeleopTurtleJoyStick teleop_turtle;

  ros::spin();
}