ROS講座51 オムニホイールのシミュレーション3（コントローラーを作る）

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要

前回作ったシミュレーションでは各々のモーターの軸の回転速度を指定して動かしました。これをtwist入力で動かすためのコントローラーをつくります。また高度な移動機能を持つためにはodomの取得が必須です。この2つの機能を持つc++ノードを製作します。

# 変換数式
twist(並進X・Y、回転Z）から3つの軸の回転速度に変換する数式を紹介します。

##値の定義

* 変数（入力側）

```math
V_x: 車体速度（前方+）(m/s)\\
V_y: 車体速度（左側+）(m/s)\\
\Omega: 車体角速度（反時計回り+）(rad/s)\\
```

* 変数（出力側）

```math
\omega_0: 0番オムニホイールのドライブ回転数(rad/s) \\
\omega_0: 1番オムニホイールのドライブ回転数(rad/s) \\
\omega_0: 2番オムニホイールのドライブ回転数(rad/s) \\
(軸側=外側から見て右回りが正) \\
```

* 定数

```math
r: タイヤの半径(m) \\
l: 車体中心とタイヤの距離(m) \\
\theta_0: 0番オムニホイールの位置(rad) \\
\theta_1: 1番オムニホイールの位置(rad) \\
\theta_2: 2番オムニホイールの位置(rad) \\
```

## 数式

* ロボットが並進するときのことを考えると

```math:並進の場合
r\omega_i=

\begin{pmatrix}
cos(\theta_i-\pi/2) & sin(\theta_i-\pi/2)
\end{pmatrix}

\begin{pmatrix}
V_x \\
V_y \\
\end{pmatrix}
```

* ロボットがその場で回転するときを考えると

```math:回転の場合
-r\omega_i=l\Omega
```

* この2つを合わせると

```math:並進と回転
\begin{pmatrix}
\omega_0 \\
\omega_1 \\
\omega_2 \\
\end{pmatrix}
=
\frac{1}{r}
\begin{pmatrix}
cos(\theta_0 - \pi/2) & sin(\theta_0 - \pi/2) \\
cos(\theta_1 - \pi/2) & sin(\theta_1 - \pi/2) \\
cos(\theta_2 - \pi/2) & sin(\theta_2 - \pi/2)
\end{pmatrix}

\begin{pmatrix}
V_x \\
V_y \\
\end{pmatrix}
-\frac{l}{r}
\begin{pmatrix}
1 \\
1 \\
1 \\
\end{pmatrix}
\Omega
\\
　\\
=
\frac{1}{r}
\begin{pmatrix}
cos(\theta_0 - \pi/2) & sin(\theta_0 - \pi/2) & -l\\
cos(\theta_1 - \pi/2) & sin(\theta_1 - \pi/2) & -l\\
cos(\theta_2 - \pi/2) & sin(\theta_2 - \pi/2) & -l
\end{pmatrix}

\begin{pmatrix}
V_x \\
V_y \\
\Omega
\end{pmatrix}
```

* （参考）逆にオドメトリの計算は

```math:オドメトリ
\begin{pmatrix}
V_x \\
V_y \\
\omega_0
\end{pmatrix}
=
r
\begin{pmatrix}
cos(\theta_0 - \pi/2) & sin(\theta_0 - \pi/2) & -l\\
cos(\theta_1 - \pi/2) & sin(\theta_1 - \pi/2) & -l\\
cos(\theta_2 - \pi/2) & sin(\theta_2 - \pi/2) & -l
\end{pmatrix}
^{-1}
\begin{pmatrix}
\omega_0 \\
\omega_1 \\
\omega_2 \\
\end{pmatrix}
```

ソースコード中の`calculation()`が上記の数式を解いているところです。

# ソースコード

## twist変換

```c++:sim2_lecture/src/sim2_omni_commander.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
#include <string>
#include <math.h>

geometry_msgs::Twist twist_last;
bool twist_enable;
void twist_stamped_callback(const geometry_msgs::Twist& twist_msg)
{
  twist_last = twist_msg;
  twist_enable = true;
}

float wheel_base = 0.100;
float wheel_radius = 0.20;
float wheel[3] = { M_PI / 3, M_PI, 5 * M_PI / 3 };
float wheel_normal[3];

void calculation(float* out, float* in)
{
  for (int i = 0; i < 3; i++)
  {
    out[i] = (cos(wheel_normal[i]) * in[0] + sin(wheel_normal[i]) * in[1]) / wheel_radius;
    out[i] += -in[2] * wheel_base / wheel_radius;
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "omni_driver");
  ros::NodeHandle n;
  ros::NodeHandle pn("~");
  // rosparam
  pn.getParam("wheel_base", wheel_base);
  pn.getParam("wheel_radius", wheel_radius);
  pn.getParam("wheel0", wheel[0]);
  pn.getParam("wheel1", wheel[1]);
  pn.getParam("wheel2", wheel[2]);

  // publish
  ros::Publisher wheel0_pub = n.advertise<std_msgs::Float64>("wheel0", 10);
  ros::Publisher wheel1_pub = n.advertise<std_msgs::Float64>("wheel1", 10);
  ros::Publisher wheel2_pub = n.advertise<std_msgs::Float64>("wheel2", 10);
  // Subscribe
  ros::Subscriber joy_sub = n.subscribe("cmd_vel", 10, twist_stamped_callback);

  for (int i = 0; i < 3; i++)
    wheel_normal[i] = wheel[i] - M_PI / 2;

  ros::Rate loop_rate(20);
  while (ros::ok())
  {
    if (twist_enable)
    {
      float in[3] = { 0 };
      float out[3] = { 0 };
      in[0] = twist_last.linear.x;
      in[1] = twist_last.linear.y;
      in[2] = twist_last.angular.z;
      calculation(out, in);
      std_msgs::Float64 data[3];
      data[0].data = out[0];
      data[1].data = out[1];
      data[2].data = out[2];
      wheel0_pub.publish(data[0]);
      wheel1_pub.publish(data[1]);
      wheel2_pub.publish(data[2]);
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```

## odom変換

```c++:sim2_lecture/src/sim2_omni_odometry.cpp
#include <ros/ros.h>
#include <control_msgs/JointControllerState.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <string>
#include <math.h>

float wheel_speed[3] = { 0 };
void state0_callback(const control_msgs::JointControllerState& state_msg)
{
  wheel_speed[0] = state_msg.process_value;
}
void state1_callback(const control_msgs::JointControllerState& state_msg)
{
  wheel_speed[1] = state_msg.process_value;
}
void state2_callback(const control_msgs::JointControllerState& state_msg)
{
  wheel_speed[2] = state_msg.process_value;
}

float wheel_base = 0.100;
float wheel_radius = 0.20;
float publish_rate = 20;
std::string frame_id = "odom";
geometry_msgs::Twist wheel_invert(float* in)
{
  geometry_msgs::Twist out;
  float lv = 1.0 / wheel_radius;
  float av = wheel_base / wheel_radius;
  float r3 = sqrt(3);
  out.linear.x = +(1.0 / r3 / lv) * in[0] + 0.0 * in[1] - (1.0 / r3 / lv) * in[2];
  out.linear.y = -(1.0 / 3 / lv) * in[0] + (2.0 / 3 / lv) * in[1] - (1.0 / 3 / lv) * in[2];
  out.angular.z = -(1.0 / 3 / av) * in[0] - (1.0 / 3 / av) * in[1] - (1.0 / 3 / av) * in[2];
  return out;
}
geometry_msgs::Pose update_pose(geometry_msgs::Pose last_pose, geometry_msgs::Twist speed, float dt)
{
  geometry_msgs::Pose next_pose;

  tf::Quaternion quat_tmp;
  double roll, pitch, last_yaw = 0;
  // get yaw
  quaternionMsgToTF(last_pose.orientation, quat_tmp);
  tf::Matrix3x3(quat_tmp).getRPY(roll, pitch, last_yaw);
  // update
  next_pose.position.x = last_pose.position.x + (cos(last_yaw + speed.angular.z * dt / 2) * speed.linear.x -
                                                 sin(last_yaw + speed.angular.z * dt / 2) * speed.linear.y) *
                                                    dt;
  next_pose.position.y = last_pose.position.y + (sin(last_yaw + speed.angular.z * dt / 2) * speed.linear.x +
                                                 cos(last_yaw + speed.angular.z * dt / 2) * speed.linear.y) *
                                                    dt;
  float next_yaw = last_yaw + speed.angular.z * dt;
  // sey yaw
  quat_tmp = tf::createQuaternionFromRPY(0, 0, next_yaw);
  quaternionTFToMsg(quat_tmp, next_pose.orientation);
  return next_pose;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "s4_omni_odom");
  ros::NodeHandle n;
  ros::NodeHandle pn("~");
  // publish
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
  // Subscribe
  ros::Subscriber odometry0 = n.subscribe("wheel0/state", 10, state0_callback);
  ros::Subscriber odometry1 = n.subscribe("wheel1/state", 10, state1_callback);
  ros::Subscriber odometry2 = n.subscribe("wheel2/state", 10, state2_callback);

  pn.getParam("wheel_base", wheel_base);
  pn.getParam("wheel_radius", wheel_radius);
  pn.getParam("publish_rate", publish_rate);
  pn.getParam("frame_id", frame_id);

  float dt = 1.0 / publish_rate;
  ros::Rate loop_rate(publish_rate);
  geometry_msgs::Pose body_position;
  body_position.orientation.w = 1.0;
  while (ros::ok())
  {
    geometry_msgs::Twist body_speed;
    body_speed = wheel_invert(wheel_speed);

    body_position = update_pose(body_position, body_speed, dt);

    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = frame_id;
    odom_msg.twist.twist = body_speed;
    odom_msg.pose.pose = body_position;
    odom_pub.publish(odom_msg);

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
```

## CMakeList
```cmake:sim_lecture/CMakeLists.txtに追加
add_executable(sim2_omni_commander src/sim2_omni_commander.cpp)
add_executable(sim2_omni_odometry src/sim2_omni_odometry.cpp)
target_link_libraries(sim2_omni_commander
  ${catkin_LIBRARIES}
)
target_link_libraries(sim2_omni_odometry
  ${catkin_LIBRARIES}
)
```

# ビルド
```shell
cd ~/catkin_ws
catkin_make
```

# 実行
以下のコマンドで実行します。各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。


```shell:実行
roslaunch sim2_lecture odm_gazebo3.launch 
```

また別のターミナルを開いて以下を打つとロボットが動き出します。

```shell:cmd_velの送信
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 0.2
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 
```

![sim2_omni_controller.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/254442/58bd5d5e-36e6-dce7-0b9f-4d23b9c7576d.gif)


# 参考
[オムニホイールの運動](https://blog.tokor.org/2015/05/14/%E3%82%AA%E3%83%A0%E3%83%8B%E3%83%9B%E3%82%A4%E3%83%BC%E3%83%AB%E3%82%92%E7%94%A8%E3%81%84%E3%81%9F%E5%85%A8%E6%96%B9%E5%90%91%E7%A7%BB%E5%8B%95%E8%BB%8A%E3%81%AE%E8%A3%BD%E4%BD%9C%E3%81%A8%E5%88%B6%E5%BE%A1/)


# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)