実機で動くように設定する - TukamotoRyuzo/rostest GitHub Wiki

前の章でNavigation Stackはgazebo上では動くようになりました。
しかし、実機で動かすには何をすればいいのかわかりませんでした。
結論としては、実機で動かすにはros_controlの動作概念と設定方法を理解する必要があります。

gazebo上でのNavigation Stackはros_controlという仕組みを利用して動いています。
このros_controlというものが非常にややこしいのです。
使い方自体はそこまで難しいものではないのですが、動作概念を理解するのにとにかく時間がかかりました（２ヶ月くらい苦しみました）。
先に言っておくと、ros_controlはNavigation Stackにとって 必須ではありません 。
ただ、Navigtation Stackの設定が楽になるので使うほうがいいです。

ros_controlがどういう仕組みなのかを表す図をwikiから引っ張ってきます。

図6-6 ros_controlの概念図

なぜ難しいかというと，最初に示したNavigation Stackの図（図6－7）と，ros_controlとの繋がりがわからなかったからです。

図6－7 Navigation Stack 日本語版（再掲）

ros_controlを用いると図6-7は図6-8のように修正されます。

図6-8 ros_controlを使った場合のNavigation Stack

つまりros_controlは、Navigation Stackにおいて以下の3つの働きを担ってくれます。

• 走行指令を受け取って，モータを回転する命令を出す
• オドメトリを受け取って，トピックとしてpublishする
• ロボットの移動に応じてtf（ロボットの各部品の位置）を計算する

特にtfの計算をしてくれることは助かります。自分で行うのはとても面倒だからです。urdfファイルを記述することでtfの計算を行ってくれます。 またros_controlを適切に設定すれば，走行指令以外にも，腕の操作，首の操作もできるので絶対やるべきです。

ros_controlを解説する記事はネット上にいくつかありますので，参考に載せておきます。[1][2]は難しいです。[3]の記事はやさしく書かれていますのでお勧めです。一度読んで見てください。ros_controlは便利なので，理解する価値は大いにあります。

[1]Controller と HardwareInterface との間の処理の仕組み（1. ロボットモデルの定義と登録）

[2]ros_controls/ros_controllers の制御の仕組み (position/effort/velocity_controllers の基礎)

[3]Gazebo + ROS で自分だけのロボットをつくる 5. GazeboとROSの連携

記事を読む前に覚えておいてもらいたいのは，Navigation Stackのことは「一回忘れる」ということです。 今回ros_controlを利用するのはNavigation Stackを利用するのに都合が良いからであって、本来ros_controlはNavigation Stackのためだけに作られたものではないからです。 Navigation Stackはros_controlの概念図（図6－6）で言うと ROS Interfaceのe.g. msgの部分に当たるのですが，この図の大部分に関係しませんので，「どこでNavigation Stackが出てくるんだ？」と思いながら読むと混乱します。だから一回忘れてください。

しかし，参考URL[1][2][3]を読んでも，実機で動かすための設定をどうすればいいかが分からないと思います。

結論としては，RobotHWというものを作る必要があります。 （ros_control概念図のhardware_interface::RobotHWという部分）

gazeboではRobotHWの代わりにRobotHWSimというものが動いていて，走行指令を受け取る部分とオドメトリの計算はRobotHWSimの中で行われています。実機で動かす場合はRobotHWSimに代わるものを動かす必要があります。

まとめると、実機で動かすために必要なことは次の3点です。

• RobotHWノードを作る
• gazeboの代わりにRobotHWノードを起動する
• gazeboにロボットをスポーンする代わりに実機を起動する(センサ類の立ち上げ、マイコンの立ち上げ等）

私はRobotHWをmy_robo_controlの中に作りました。headerは次のようなものです。このうち，PositionJointInterfaceは首を回転させるために用意したものなので無視してください。

my_robo_control/include/my_robo_hw.h
#include <ros/ros.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <map>
#include <string>
#include <vector>

class MyRobo : public hardware_interface::RobotHW
{
public:
    MyRobo();
    ros::Time getTime() const { return ros::Time::now(); }
    ros::Duration getPeriod() const { return ros::Duration(0.01); }

    void read(ros::Time time, ros::Duration period);
    void write(ros::Time time, ros::Duration period);
    int open() const;
    void stop() const;
private:
    hardware_interface::JointStateInterface    jnt_state_interface_;
    hardware_interface::VelocityJointInterface jnt_vel_interface_;
    hardware_interface::PositionJointInterface jnt_pos_interface_;
    double cmd_[4];
    double pos_[4];
    double vel_[4];
    double eff_[4];
};

大事なのはreadメソッドとwriteメソッドです。

• readメソッドで走行指令をモータドライバに送る
• writeメソッドでオドメトリを計算する ように記述します。

※「readとwriteでやる処理が逆じゃないか？」と思うかもしれませんが，ロボットから見れば移動指令は読み込むべきものであり，オドメトリはユーザに書いて教えてあげるものなので，間違いではありません。

参考にしたソースもそうしてありました。 https://github.com/open-rdc/icart/blob/indigo-devel/icart_mini_driver/src/icart_mini_driver_node.cpp

でも，これは最初に示したros_controlの図とは異なりますので混乱を招いたらごめんなさい。

cppファイルからreadとwriteを一部抜粋します。全文はソースを見てください。

my_robo_control/src/my_robo_control.cpp

void MyRobo::read(ros::Time time, ros::Duration period)
{
    //if (cmd_[0] || cmd_[1])
    //	ROS_INFO_STREAM("Commands for joints: " << cmd_[0] << ", " << -cmd_[1]);
    	
    int ret = YP_wheel_vel(cmd_[0], -cmd_[1]);

…// 以降は首回転処理    
}

void MyRobo::write(ros::Time time, ros::Duration period)
{
    double yp_vel[2];
    yp_vel[0] = 0;
    yp_vel[1] = 0;
    YP_get_wheel_vel(&yp_vel[0], &yp_vel[1]);
    yp_vel[1] = -yp_vel[1];
    
    //if (yp_vel[0] || yp_vel[1])
    //    ROS_INFO_STREAM("YPSpur vel: " << yp_vel[0] << ", " << -yp_vel[1]);
for (unsigned int i = 0; i < 2; ++i)
{
    pos_[i] += yp_vel[i] * getPeriod().toSec();
        vel_[i] = yp_vel[i];
    }
…
}

readメソッドでYP-Spurの走行命令を，writeメソッドでYP-Spurの読み込み命令を実行しています。 ypspur_rosノードとやり取りするのではありません。YP-Spurの命令を直接呼び出してください。

ちなみにYP-Spurを使うのではなく自作のモータドライバや別のモータドライバを使う場合は，そのモータドライバに書き込み、読み取りを行うようにソースを変えてください。とにかくreadメソッドでマイコンに書き込んで，writeメソッドでマイコンから読み出すのです。そうすればモータは動きます。このやり方は，走行系以外でも同じです。

これらをmain関数で次のように呼んでいます。

my_robo_control/src/my_robo_control.cpp

while (ros::ok())
{
    ros::Time now = myrobo.getTime();
    ros::Duration dt = myrobo.getPeriod();

    if (YP_get_error_state() == 0)
    {
        myrobo.read(now, dt);
 myrobo.write(now, dt);
   }
else
{
    ROS_WARN("Disconnected T-frog driver");
 myrobo.stop();
            
 while (myrobo.open() < 0)
 {
      ROS_WARN("try to connect T-frog driver");
      ros::Duration(1).sleep();
 }
            
 ROS_INFO("connected T-frog driver");
}

    cm.update(now, dt);
    rate.sleep();
}

readメソッドとwriteメソッドを一定周期で呼んでいることがわかります。 このソースをcatkin_makeしてノードを作ります。

gazeboの代わりにRobotHWをlaunchします。
rostestではmy_robo_controlという名前でノードを作り，次のようにして起動しています。

my_robo_control/launch/control_real.launch

<launch>
  <include file="$(find my_robo_control)/launch/control_base.launch"/>
  <node name="my_robo_control" pkg="my_robo_control" type="my_robo_control" output="screen"/>
</launch>

control_base.launchは次のようにしています。

my_robo_control/launch/control_real.launch

<launch>
  <arg name="model" default="$(find my_robo_description)/my_robo.urdf"/>
  
  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)"/>
   
  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find my_robo_control)/config/controller.yaml" command="load"/>
 
  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager"
        type="spawner" output="screen"
        args="joint_state_controller
              diff_drive_controller
              head_position_controller
              neck_position_controller"/>
              
  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
        type="robot_state_publisher" respawn="false" output="screen"/>
</launch>

control_base.launchはチュートリアルそのままで，このlaunchファイルに加えてRobotHWを記述したノードmy_robo_controlを動かしています。

実機の起動に必要なノードのlaunchファイルを一部抜粋します。

robot_launcher/launch/b205_demo.launch

<launch>
  <!-- setup robot -->
  <include file="$(find robot_launcher)/launch/setup_robot.launch"/>
  <include file="$(find my_robo_control)/launch/control_real.launch"/>
  <include file="$(find my_robo_2dnav)/launch/move_base_B205.launch"/>
（省略）
</launch>

gazeboの起動に必要だった「シーンとロボットのスポーン」がなくなり，代わりにロボットのセンサ立ち上げ処理がsetup_robot.launchで行われます。 そしてcontrol_real.launchでRobotHWを起動します。 最後にmove_baseの立ち上げをmove_base_B205.launchで行っています。 これで実機でNavigation Stackを使うことができます。