ロボットを駆動する

いくつかの種類のアクチュエータが存在する
* [differential_drive](#differential_drive)：gazebo側が用意する差動二輪駆動のためのプラグイン機能
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/3942e4c6-a755-3e69-b7c7-ac4fae244281.png)
* [gazebo_ros_control](#gazebo_ros_control)：四輪ロボットなど，二輪ではないロボットを駆動させるのに使うrosパッケージ．
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/8634616c-37b7-4a7a-f14c-4a385adeca31.png)


# differential_drive
作成した車輪のurdfにgazebo pluginを追記する．

```xml:wheel/wheel.urdf.xacro
<gazebo>
    <!-- Gazeboプラグインを読み込む -->
	<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">

          <!-- 左右の車輪のJointを指定することで，このjointにアクチュエータが追加され制御が可能になる -->
	      <leftJoint>left_wheel_joint</leftJoint>
	      <rightJoint>right_wheel_joint</rightJoint>
          
          <!-- ロボットの中心位置になるlinkを指定．rvizなどで表示されるロボット位置がこれになる．-->
	      <robotBaseFrame>base_footprint</robotBaseFrame>

      	  <!-- 車輪間距離と車輪の大きさを入力 -->
	      <wheelSeparation>0.5</wheelSeparation>
	      <wheelDiameter>0.15</wheelDiameter>

          <!-- jointの位置情報をROSトピックでpublishさせる -->
	      <publishWheelJointState>true</publishWheelJointState>
	      <legacyMode>false</legacyMode>
	</plugin>
</gazebo>
```

これでGazeboを起動すると駆動可能の状態で表示される．
```
$ roslaunch robot_description gazebo_display.launch
```
### 速度情報の入力
Gazeboを起動した状態で，別のターミナルを開き，速度情報を入力する．
今回使用したプラグイン（diff_drive_controller）では，cmd_velという名前のトピック経由で速度を指示できる．

1. 速度情報を何も流さない状態で，cmd_velトピックに流れている情報を見てみる
    ```
    （gazeboを起動した状態で....）
    $ rostopic info cmd_vel
    ```
    ![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/e778a81c-73cf-ac0f-51da-054cfba70bc0.png)
    ☝のように表示される．
    cmd_velトピックでは，*geometry_msgs/Twist*という型でメッセージが流されると分かる
    
2. cmd_velトピックに速度情報を直接流す
    ```
    $ rostopic pub -l /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 2.0}}'
    ```
3. キーボードから操作する．
    teleop_twist_keyboardパッケージのteleop_twist_keyboard.pyノードは/cmd_velトピックにキーボードから入力された命令をpublishするので，それを流用する
    
    ```cpp
    // 一度Gazeboを閉じる

    // キーボード操作用のパッケージをインストール
    $ sudo apt install ros-melodic-teleop-twist-keyboard
    
    // 2つのターミナルを開いてgazebo起動&以下のコマンドでgazebo上のロボットをキーボードで制御
    $ rosrun teleop_twist_keyboard teleop_twist_keyboard.py
    ```

:::note info
この時入力されるのはjointの速度制御！
直進するためには *angular velocity* を入力する必要がある！
:::

# gazebo_ros_control
`position`，`velocity`， `effort`の3種類を入力することでjointを制御するrosパッケージ．
このパッケージでjointを駆動させるには，urdfのほかに`gazeboタグ`と`transmissionタグ`，`yamlファイル`の記述が必要となる．

```xml:wheel.gazebo.xacro
<?xml version="1.0"?>
<!-- gazebo内で車輪が受ける摩擦などのパラメータ設定 -->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
    <xacro:macro name="wheel_gazebo" params="prefix">
        <gazebo reference="${prefix}_wheel_link">
            <mu1 value="1.0"/>
            <mu2 value="1.0"/>
            <kp value="10000000.0" />
            <kd value="1.0" />
            <fdir1 value="1 0 0"/>
        </gazebo>
    </xacro:macro>
</robot>
```

```xml:wheel.transmission.xacro
<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
    <xacro:macro name="wheel_transmission" params="prefix">
        <transmission name="${prefix}_wheel_trans" type="SimpleTransmission">
			<type>transmission_interface/SimpleTransmission</type>
            <!-- 駆動したいjointを設定 -->
			<joint name="${prefix}_wheel_joint">
            <!-- 制御入力の種類を設定 -->
				<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
			</joint>
            <actuator name="${prefix}_wheel_motor">
				<mechanicalReduction>1</mechanicalReduction>
			</actuator>			
		</transmission>
    </xacro:macro>
</robot>
```

```yaml:control.yaml
  # publish states
husky_joint_publisher:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50

husky_velocity_controller:
  type: "diff_drive_controller/DiffDriveController"
  left_wheel: ['front_left_wheel_joint', 'rear_left_wheel_joint']
  right_wheel: ['front_right_wheel_joint', 'rear_right_wheel_joint']
  publish_rate: 50
  pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
  twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
  cmd_vel_timeout: 0.25
  velocity_rolling_window_size: 2

  # Base frame_id
  base_frame_id: chassis_link

  # Odometry fused with IMU is published by robot_localization, so
  # no need to publish a TF based on encoders alone.
  enable_odom_tf: false

  # Wheel separation and radius multipliers
  wheel_separation_multiplier: 1.875 # default: 1.0
  wheel_radius_multiplier    : 1.0 # default: 1.0

  # Velocity and acceleration limits
  # Whenever a min_* is unspecified, default to -max_*
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 1.0   # m/s
      has_acceleration_limits: true
      max_acceleration       : 3.0   # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 2.0   # rad/s
      has_acceleration_limits: true
      max_acceleration       : 6.0   # rad/s^2
```

さらに，launchファイルにも追記が必要．（yamlファイルのパスは随時変えること）

:::note warn
このとき，yamlファイルに書いたコントローラの名前（☝の場合，`husky_joint_publisher`と`husky_velocity_controller`）を`args`として．そろえて入力する必要があるので注意！
:::

```xml:gazebo_display.launch
<launch> 
    <!-- gazebo turn on -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
    <!-- define parameter of robot model(xacro to urdf) -->    
    <param name="robot_description" command="$(find xacro)/xacro $(find robot_description)/urdf/robot/robot.urdf.xacro"/>
    <!-- show the model on gazebo -->
    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model myrobot"/>

    <!-- 以下追記 -->
	<!-- for driver & drive controller -->
	<rosparam command="load" file="$(find hauser_viz)/launch/config/yaml/control.yaml"/>
  	<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
		args="husky_joint_publisher husky_velocity_controller" />
</launch>
```

### 速度情報の入力
Gazeboを起動した状態で，別のターミナルを開き，速度情報を入力する．
速度情報の流れるトピック名は，yamlファイルのコントローラ名によって変わり，今回は`/husky_velocity_controller/cmd_vel`に流れる．
    
**cmd_velトピックに速度情報を直接流す**
```
$ rostopic pub -l /husky_velocity_controller/cmd_vel geometry_msgs/Twist '{linear: {x: 1.0}, angular: {z: 0.0}}'
```
この時は車両が速度1.0 m/sで直進する．

:::note info
gazebo_ros_controllerを使った場合は，直進させるのに*linear*に速度を入力する．（コマンド内容もより実機の制御に近づく）
:::