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
次の3つのファイルを見つけます
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はキーボードで亀の動きを制御するテストプログラムです.
プログラムは簡単です.キーイベントを1サイクルで傍受し、Topic turtle 1/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>("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<:twist>("turtle1/cmd_vel", 1);
  twist_sub_ = nh_.subscribe<:joy>("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();
}