LoginSignup
5
1

More than 1 year has passed since last update.

@noby_aoao

OSOYOO メカナム４輪 ROS化

Last updated at Posted at 2021-09-17

４輪ロボットのROS化

4輪ロボットをROS(cmd_vel)で操作したい。

通信は以下を採用。
ubuntu→(ssh)→raspi→(serial)→arduino→(gpio)→モータ

前準備

ubuntuPCにArduino開発環境を用意

以下の参考記事の通り
ros_wiki

４輪ロボットのスペック確認

モーターのデータシート
https://osoyoo.com/picture/mecanum_wheel_robotic/GM25-370-motor%20specification.pdf
・ギア比1/75（今回は未使用）

コーディング

システム構成

基本的なロボットのシステム構成は以下のよう．
image.png
しかし今回はめんどくさかったので簡単のためにエンコーダ未使用です．
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に以下のコードを書き込み

wheel_cont.ino
#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通信のためのパッケージをインストール

raspi
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
raspiターミナル1
export ROS_IP=192.168.201.xxx ##raspiのIP
roscore
raspiターミナル2
export ROS_IP=192.168.201.xxx ##raspiのIP
rosrun rosserial_python serial_node.py /dev/ttyACM0
pc
#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:
1. shareボタンを押す
2. xボタンを押しながらジョイスティック操作

うまくいくとこうなるはず
a.gif

launchファイル

pc_launch.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

5
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
5
1