differential_drive

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

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という名前のトピック経由で速度を指示できる．

速度情報を何も流さない状態で，cmd_velトピックに流れている情報を見てみる
```
（gazeboを起動した状態で....）
$ rostopic info cmd_vel
```
☝のように表示される．
cmd_velトピックでは，geometry_msgs/Twistという型でメッセージが流されると分かる

cmd_velトピックに速度情報を直接流す

$ rostopic pub -l /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0}, angular: {z: 2.0}}'

キーボードから操作する．
teleop_twist_keyboardパッケージのteleop_twist_keyboard.pyノードは/cmd_velトピックにキーボードから入力された命令をpublishするので，それを流用する

// 一度Gazeboを閉じる

// キーボード操作用のパッケージをインストール
$ sudo apt install ros-melodic-teleop-twist-keyboard

// 2つのターミナルを開いてgazebo起動&以下のコマンドでgazebo上のロボットをキーボードで制御
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

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

gazebo_ros_control

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

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>

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>

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ファイルのパスは随時変えること）

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

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で直進する．

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

ロボットを駆動する

differential_drive

速度情報の入力

gazebo_ros_control

速度情報の入力