環境
この記事は以下の環境で動いています。
| 項目 | 値 |
|---|---|
| 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"
