環境

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

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

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

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

変換数式

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

##値の定義

変数（入力側）

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

変数（出力側）

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

定数

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

数式

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

並進の場合

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}

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

回転の場合

-r\omega_i=l\Omega

この2つを合わせると

並進と回転

\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}

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

オドメトリ

\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変換

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変換

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

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}
)

ビルド

cd ~/catkin_ws
catkin_make

実行

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

実行

roslaunch sim2_lecture odm_gazebo3.launch

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

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"

参考

オムニホイールの運動

目次ページへのリンク

ROS講座の目次へのリンク