#4輪ロボットのROS化
4輪ロボットをROS(cmd_vel)で操作したい。
通信は以下を採用。
ubuntu→(ssh)→raspi→(serial)→arduino→(gpio)→モータ
##前準備
###ubuntuPCにArduino開発環境を用意
以下の参考記事の通り
ros_wiki
###4輪ロボットのスペック確認
モーターのデータシート
https://osoyoo.com/picture/mecanum_wheel_robotic/GM25-370-motor%20specification.pdf
・ギア比1/75(今回は未使用)
コーディング
システム構成
基本的なロボットのシステム構成は以下のよう.
しかし今回はめんどくさかったので簡単のためにエンコーダ未使用です.
FBによる細かい速度の調整もめんどくさいので諦めました.
今回使用したコードはgithubに置いてあります.
https://github.com/nobunoby/Osoyoo4WheelROS
逆運動学
速度成分から各タイヤの回転速度への変換を以下の記事,論文を参考にコードを記述.
http://robotsforroboticists.com/drive-kinematics/
http://www2.lib.yamagata-u.ac.jp/you-campus/sangitan/kiyou-sangitan/11/p81-84.pdf
実装
Arduinoに以下のコードを書き込み
#include <ros.h>
#include <geometry_msgs/Twist.h>
#define speedPinR 9 // RIGHT WHEEL PWM pin D45 connect front MODEL-X ENA
#define RightMotorDirPin1 22 //Front Right Motor direction pin 1 to Front MODEL-X IN1 (K1)
#define RightMotorDirPin2 24 //Front Right Motor direction pin 2 to Front MODEL-X IN2 (K1)
#define LeftMotorDirPin1 26 //Left front Motor direction pin 1 to Front MODEL-X IN3 ( K3)
#define LeftMotorDirPin2 28 //Left front Motor direction pin 2 to Front MODEL-X IN4 ( K3)
#define speedPinL 10 // Left WHEEL PWM pin D7 connect front MODEL-X ENB
#define speedPinRB 11 // RIGHT WHEEL PWM pin connect Back MODEL-X ENA
#define RightMotorDirPin1B 5 //Rear Right Motor direction pin 1 to Back MODEL-X IN1 ( K1)
#define RightMotorDirPin2B 6 //Rear Right Motor direction pin 2 to Back MODEL-X IN2 ( K1)
#define LeftMotorDirPin1B 7 //Rear left Motor direction pin 1 to Back MODEL-X IN3 K3
#define LeftMotorDirPin2B 8 //Rear left Motor direction pin 2 to Back MODEL-X IN4 k3
#define speedPinLB 12 // LEFT WHEEL PWM pin D8 connect Rear MODEL-X ENB
#define WHEEL_RADIUS 0.04 // diameter 80mm
#define WHEEL_SEPARATION_WIDTH 0.107
#define WHEEL_SEPARATION_LENGTH 0.0465
ros::NodeHandle nh;
void callback(const geometry_msgs::Twist& vel)
{
float lin_x, lin_y, ang_z;
lin_x = vel.linear.x;
lin_y = vel.linear.y;
ang_z = vel.angular.z;
int fl = (lin_x - lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z) / WHEEL_RADIUS / 10 * 250;
int fr = (1/WHEEL_RADIUS) * (lin_x + lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
int rl = (1/WHEEL_RADIUS) * (lin_x + lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
int rr = (1/WHEEL_RADIUS) * (lin_x - lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
//nh.loginfo("receieved cmd_vel");
FL(fl);
FR(fr);
RL(rl);
RR(rr);
}
ros::Subscriber<geometry_msgs::Twist> sub("/ugv/cmd_vel", &callback);
void setup()
{
init_GPIO();
nh.initNode();
nh.subscribe(sub);
}
void loop()
{
nh.spinOnce();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void wheel_vel(float lin_x, float lin_y, float ang_z)
{
int fl = (lin_x - lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z) / WHEEL_RADIUS / 10 * 250;
int fr = (1/WHEEL_RADIUS) * (lin_x + lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
int rl = (1/WHEEL_RADIUS) * (lin_x + lin_y - (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
int rr = (1/WHEEL_RADIUS) * (lin_x - lin_y + (WHEEL_SEPARATION_WIDTH + WHEEL_SEPARATION_LENGTH)*ang_z)/10*250;
FL(fl);
FR(fr);
RL(rl);
RR(rr);
}
void FL(int fl)
{
int speed;
if (fl >= 0) // rotate fw
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,HIGH);
speed = fl;
}
else if (fl < 0) // rotate bw
{
digitalWrite(LeftMotorDirPin1,HIGH);
digitalWrite(LeftMotorDirPin2,LOW);
speed = -1 * fl;
}
analogWrite(speedPinL,speed);
}
void FR(int fr)
{
int speed;
if (fr >= 0) // rotate fw
{
digitalWrite(RightMotorDirPin1,LOW);
digitalWrite(RightMotorDirPin2,HIGH);
speed = fr;
}
else if (fr < 0) // rotate bw
{
digitalWrite(RightMotorDirPin1,HIGH);
digitalWrite(RightMotorDirPin2,LOW);
speed = -1 * fr;
}
analogWrite(speedPinR,speed);
}
void RL(int rl)
{
int speed;
if (rl >= 0) // rotate fw
{
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,HIGH);
speed = rl;
}
else if (rl < 0) // rotate bw
{
digitalWrite(LeftMotorDirPin1B,HIGH);
digitalWrite(LeftMotorDirPin2B,LOW);
speed = -1 * rl;
}
analogWrite(speedPinLB,speed);
}
void RR(int rr)
{
int speed;
if (rr >= 0) // rotate fw
{
digitalWrite(RightMotorDirPin1B,LOW);
digitalWrite(RightMotorDirPin2B,HIGH);
speed = rr;
}
else if (rr < 0) // rotate bw
{
digitalWrite(RightMotorDirPin1B,HIGH);
digitalWrite(RightMotorDirPin2B,LOW);
speed = -1 * rr;
}
analogWrite(speedPinRB,speed);
}
void brake()
{
digitalWrite(LeftMotorDirPin1,LOW);
digitalWrite(LeftMotorDirPin2,LOW);
digitalWrite(RightMotorDirPin1,LOW);
digitalWrite(RightMotorDirPin2,LOW);
digitalWrite(LeftMotorDirPin1B,LOW);
digitalWrite(LeftMotorDirPin2B,LOW);
digitalWrite(RightMotorDirPin1B,LOW);
digitalWrite(RightMotorDirPin2B,LOW);
}
void stop_Stop() //Stop
{
analogWrite(speedPinLB,0);
analogWrite(speedPinRB,0);
analogWrite(speedPinL,0);
analogWrite(speedPinR,0);
}
//Pins initialize
void init_GPIO()
{
pinMode(RightMotorDirPin1, OUTPUT);
pinMode(RightMotorDirPin2, OUTPUT);
pinMode(speedPinL, OUTPUT);
pinMode(LeftMotorDirPin1, OUTPUT);
pinMode(LeftMotorDirPin2, OUTPUT);
pinMode(speedPinR, OUTPUT);
pinMode(RightMotorDirPin1B, OUTPUT);
pinMode(RightMotorDirPin2B, OUTPUT);
pinMode(speedPinLB, OUTPUT);
pinMode(LeftMotorDirPin1B, OUTPUT);
pinMode(LeftMotorDirPin2B, OUTPUT);
pinMode(speedPinRB, OUTPUT);
stop_Stop();
}
実行
raspi↔arduino通信のためのパッケージをインストール
sudo apt-get install ros-melodic-rosserial -y
sudo apt-get install ros-melodic-rosserial-arduino -y
arduino接続してるシリアルポートを$ dmesg | grep tty で確認
ssh ubuntu@192.168.201.xxx
export ROS_IP=192.168.201.xxx ##raspiのIP
roscore
export ROS_IP=192.168.201.xxx ##raspiのIP
rosrun rosserial_python serial_node.py /dev/ttyACM0
#pcのip
export ROS_IP=192.168.201.xxx
#raspiのip
export ROS_MASTER_URI=http://192.168.201.xxx:11311
rosrun joy joy_node &
rosrun teleop_twist_joy teleop_node cmd_vel:=ugv/cmd_vel
arduinoとraspiの通信の参考
http://wiki.ros.org/rosserial_arduino/Tutorials/Blink
ds3:
- shareボタンを押す
- xボタンを押しながらジョイスティック操作
###launchファイル
<?xml version="1.0"?>
<launch>
<node name="joy" pkg="joy" type="joy_node" output="log">
</node>
<node name="joy_twist" pkg="teleop_twist_joy" type="teleop_node" output="log">
<remap from="/cmd_vel" to="/ugv/cmd_vel
" />
</node>
<node name="arduino" pkg="rosserial_python" type="serial_node.py" args="/dev/ttyACM0" />
</launch>
本当はpcとraspiのlaunchファイルまとめたいけど,ちゃんとssh接続の設定してないのが仇となった...
qiita記事1
参考:http://wiki.ros.org/roslaunch/Tutorials/Roslaunch%20tips%20for%20larger%20projects
###トラブルシュート
・pcのipはあってるか
・ps3のshareボタンは押したか
・Arduinoの電源接続してるか
どちらでもない
→ros wiki tutorialを参考に接続からやり直す
##シミュレーション(執筆途中)
URDFモデルはここから取得して利用
###参考
一応使わなかったが記載
エンコーダのサンプルコード
https://osoyoo.com/2019/11/08/how-to-test-motor-encoder-with-arduino/
https://github.com/ros-controls/ros_controllers/tree/melodic-devel/four_wheel_steering_controller
https://qiita.com/MoriKen/items/78b0ad8c1eae257646dd