Ryota Kotani / Mbed 2 deprecated ROS_R2_YONRIN

ROSでR2の足回りを動かせれるようにしたやつです。 使えるのは今のところ、小谷（私）しかいませんが一応。

Dependencies:   mbed URF MD_MDDS30_oit_ ros_lib_melodic

main.cpp@2:87caac24d995, 2022-05-04 (annotated)

Committer:
yakifrog
Date:
Wed May 04 15:52:12 2022 +0000
Revision:
2:87caac24d995
Parent:
1:f6a9a3a7455d 
ROSROSOROOAAAA;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akihiron 0:450a7208c682 1 #include "mbed.h"
akihiron 0:450a7208c682 2 #include "MDDS30_oit.h"
akihiron 0:450a7208c682 3 #include "URF.h"
yakifrog 1:f6a9a3a7455d 4 #include <ros.h>
yakifrog 1:f6a9a3a7455d 5 #include <geometry_msgs/Twist.h>
yakifrog 1:f6a9a3a7455d 6 #include <std_msgs/Float32.h>
yakifrog 1:f6a9a3a7455d 7 #include <sensor_msgs/Joy.h>
yakifrog 1:f6a9a3a7455d 8 #include <sensor_msgs/LaserScan.h>
yakifrog 1:f6a9a3a7455d 9 #include <std_msgs/String.h>
akihiron 0:450a7208c682 10
yakifrog 1:f6a9a3a7455d 11 ros::NodeHandle nh;
akihiron 0:450a7208c682 12
yakifrog 1:f6a9a3a7455d 13 URF usdRight(D6); // 右側に取り付ける超音波センサー
yakifrog 1:f6a9a3a7455d 14 URF usdLeft(D8); // 左側に取り付ける超音波センサー
yakifrog 1:f6a9a3a7455d 15
yakifrog 1:f6a9a3a7455d 16 Ticker update; // よくわからないけど、「24回/秒」処理されるってことかしら。
akihiron 0:450a7208c682 17
akihiron 0:450a7208c682 18 DigitalOut led(LED1);
akihiron 0:450a7208c682 19 DigitalOut led2(PC_4);
yakifrog 1:f6a9a3a7455d 20 DigitalIn btn(USER_BUTTON); // USER_BUTTONってどこだろ
akihiron 0:450a7208c682 21 MDDS30oit MD1(PC_10, PC_11); //left:モーター番号1（右後輪）、right:モーター番号2（左後輪）
akihiron 0:450a7208c682 22 MDDS30oit MD2(PA_0, PA_1); //left:モーター番号3（右前輪）、right:モーター番号4（左前輪）
akihiron 0:450a7208c682 23
yakifrog 1:f6a9a3a7455d 24
yakifrog 1:f6a9a3a7455d 25 // --- 初期設定 --- //
yakifrog 1:f6a9a3a7455d 26 double speed_ang = 0.0;
yakifrog 1:f6a9a3a7455d 27 double speed_lin = 0.0;
yakifrog 1:f6a9a3a7455d 28
yakifrog 1:f6a9a3a7455d 29 double w_r = 0.0;
yakifrog 1:f6a9a3a7455d 30 double w_l = 0.0;
yakifrog 1:f6a9a3a7455d 31
yakifrog 1:f6a9a3a7455d 32 float duty[4]={0.0f};
yakifrog 1:f6a9a3a7455d 33 // --------------- //
yakifrog 1:f6a9a3a7455d 34
yakifrog 1:f6a9a3a7455d 35
yakifrog 1:f6a9a3a7455d 36 // --- ROS --- //
yakifrog 1:f6a9a3a7455d 37 std_msgs::String str_msg;
yakifrog 1:f6a9a3a7455d 38 ros::Publisher chatter("chatter", &str_msg); // マイコンからROSにデータを飛ばすことができるかを試す実験
yakifrog 1:f6a9a3a7455d 39 char hello[13] = "hello world!";
yakifrog 1:f6a9a3a7455d 40 // ----------- //
yakifrog 1:f6a9a3a7455d 41
yakifrog 1:f6a9a3a7455d 42
yakifrog 1:f6a9a3a7455d 43 // 超音波センサー
akihiron 0:450a7208c682 44 void inter(void)
akihiron 0:450a7208c682 45 {
akihiron 0:450a7208c682 46 usdRight.send();
akihiron 0:450a7208c682 47 usdLeft.send();
akihiron 0:450a7208c682 48 }
akihiron 0:450a7208c682 49
akihiron 0:450a7208c682 50
yakifrog 1:f6a9a3a7455d 51 // 四輪オムニのPWM値出し
yakifrog 1:f6a9a3a7455d 52 void Yonrin(float x,float y){
akihiron 0:450a7208c682 53 duty[0] =-x/sqrtf(2)+y/sqrtf(2);
akihiron 0:450a7208c682 54 duty[1] =-x/sqrtf(2)-y/sqrtf(2);
akihiron 0:450a7208c682 55 duty[2] = x/sqrtf(2)-y/sqrtf(2);
akihiron 0:450a7208c682 56 duty[3] = x/sqrtf(2)+y/sqrtf(2);
akihiron 0:450a7208c682 57
yakifrog 1:f6a9a3a7455d 58 MD1.left(duty[0]);
yakifrog 1:f6a9a3a7455d 59 MD1.right(duty[1]);
yakifrog 1:f6a9a3a7455d 60 MD2.left(duty[2]);
yakifrog 1:f6a9a3a7455d 61 MD2.right(duty[3]);
yakifrog 1:f6a9a3a7455d 62 wait(0.01);
akihiron 0:450a7208c682 63 }
akihiron 0:450a7208c682 64
yakifrog 1:f6a9a3a7455d 65
yakifrog 1:f6a9a3a7455d 66 // SLAMで経路計画した結果、送られてくる移動方向の入力を受け取って機体を制御する関数
yakifrog 1:f6a9a3a7455d 67 void slamControl(const geometry_msgs::Twist& msg){
yakifrog 1:f6a9a3a7455d 68 // cmd_velを取得して制御
yakifrog 1:f6a9a3a7455d 69
yakifrog 1:f6a9a3a7455d 70 speed_ang = 1 - msg.angular.z; // 左右 (出力が１になると、一気に加速してしまうので１よりも小さくなってほしいために調整）
yakifrog 1:f6a9a3a7455d 71 speed_lin = 1 - msg.linear.x; // 上下
yakifrog 1:f6a9a3a7455d 72 w_r = -0.25f * speed_ang; // これも手動で調整した結果
yakifrog 1:f6a9a3a7455d 73 w_l = 0.25f * speed_lin;
yakifrog 1:f6a9a3a7455d 74
yakifrog 1:f6a9a3a7455d 75 Yonrin(w_l, w_r); // 上下，左右
akihiron 0:450a7208c682 76 }
akihiron 0:450a7208c682 77
yakifrog 1:f6a9a3a7455d 78
yakifrog 1:f6a9a3a7455d 79 // PS4などのコントローラで機体を制御する関数
yakifrog 1:f6a9a3a7455d 80 void joyControl(const sensor_msgs::Joy& joy){
yakifrog 1:f6a9a3a7455d 81 // axes[0]:Lスティック（左右）, axes[1]:Lスティック（上下）
yakifrog 1:f6a9a3a7455d 82 Yonrin(joy.axes[1] * 0.23f, joy.axes[0] * (-0.23f));
yakifrog 1:f6a9a3a7455d 83 }
yakifrog 1:f6a9a3a7455d 84
yakifrog 1:f6a9a3a7455d 85
yakifrog 1:f6a9a3a7455d 86 // LiDARで障害物を検知して機体を制御する関数
yakifrog 1:f6a9a3a7455d 87 void lidarControl(const sensor_msgs::LaserScan& lidar){
yakifrog 1:f6a9a3a7455d 88 if (lidar.range_min >= 0.1f && lidar.range_max <= 0.5f){ // 0.1m以上〜0.5m以下の場所に障害物がある場合、停止。
yakifrog 1:f6a9a3a7455d 89 while(1){
yakifrog 1:f6a9a3a7455d 90 Yonrin(0, 0);
yakifrog 1:f6a9a3a7455d 91 if (lidar.range_max > 0.5f){ // 障害物が0.5mよりも遠くにならないと、抜け出せないループ。
yakifrog 1:f6a9a3a7455d 92 break;
yakifrog 1:f6a9a3a7455d 93 }
yakifrog 1:f6a9a3a7455d 94 }
akihiron 0:450a7208c682 95 }
akihiron 0:450a7208c682 96 }
akihiron 0:450a7208c682 97
yakifrog 1:f6a9a3a7455d 98
yakifrog 1:f6a9a3a7455d 99 // --- ROS --- //
yakifrog 1:f6a9a3a7455d 100 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &slamControl); // SLAM データを取得したら関数が呼び出される仕組み
yakifrog 1:f6a9a3a7455d 101 ros::Subscriber<sensor_msgs::Joy> sub2("joy", &joyControl); // PS4 同上
yakifrog 1:f6a9a3a7455d 102 //ros::Subscriber<sensor_msgs::LaserScan> sub3("scan", &lidarCb); // LiDARの制御は、Bufferサイズの関係でバグって今は使えない（単に送る情報量が多すぎる）
yakifrog 1:f6a9a3a7455d 103 // ----------- //
yakifrog 1:f6a9a3a7455d 104
akihiron 0:450a7208c682 105
yakifrog 1:f6a9a3a7455d 106 int main(){
yakifrog 1:f6a9a3a7455d 107 //setup();
yakifrog 1:f6a9a3a7455d 108 nh.initNode(); // ノードを初期化
yakifrog 1:f6a9a3a7455d 109 nh.getHardware()->setBaud(57600);
yakifrog 1:f6a9a3a7455d 110 nh.advertise(chatter); // マイコンからROSにデータ送信するために必要な認証？のようなもの。(道を作っている）
yakifrog 1:f6a9a3a7455d 111 nh.subscribe(sub); // ROSからデータ取得
yakifrog 1:f6a9a3a7455d 112 nh.subscribe(sub2); // ROSからデータ取得
yakifrog 1:f6a9a3a7455d 113 //nh.subscribe(sub3);
akihiron 0:450a7208c682 114
yakifrog 1:f6a9a3a7455d 115 str_msg.data = hello;
yakifrog 1:f6a9a3a7455d 116 chatter.publish( &str_msg ); // ここで実際にROSにデータを送信している
yakifrog 1:f6a9a3a7455d 117
yakifrog 1:f6a9a3a7455d 118
yakifrog 1:f6a9a3a7455d 119 //void loop();
yakifrog 1:f6a9a3a7455d 120 while (1){
akihiron 0:450a7208c682 121 int dataRight = usdRight.read(mm);
akihiron 0:450a7208c682 122 int dataLeft = usdLeft.read(mm);
yakifrog 1:f6a9a3a7455d 123 nh.spinOnce(); // ROSはノードごとで処理をしているため、その処理を繰り返すためのオマジナイ
yakifrog 1:f6a9a3a7455d 124 wait_ms(10);
akihiron 0:450a7208c682 125 }
akihiron 0:450a7208c682 126 }