ros

Dependencies:   mbed QEI ros_lib_kinetic

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include <ros.h>
00003 #include <std_msgs/Float32MultiArray.h>
00004 #include "StdControlLibrary.h"
00005 #include <math.h>
00006 
00007 #define getMPS(rpm, wheelDiameter) (rpm * wheelDiameter * PI / 60)
00008 const double WHEELDIAMETER = 0.060;
00009 
00010 float last_velo[3] = {0};  // 前ループのロボットの速度
00011 float robot_odom[3] = {0}; // ロボットのオドメトリ
00012 float pose[3] = {0.0};     // ROSから返ってきた自己位置
00013 
00014 void getGyro(float return_gyro[3]);
00015 void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3]);
00016 void calOdom(float robot_velo[3], float return_odom[3]);
00017 void messageCb(const std_msgs::Float32MultiArray &msg);
00018 void sendToRos();
00019 
00020 Ticker flipper;
00021 
00022 MPU6050 mpu(p9, p10);
00023 QEI qei_x(p5, p6, NC, 159, QEI::X4_ENCODING);
00024 QEI qei_y(p8, p7, NC, 159, QEI::X4_ENCODING);
00025 
00026 DigitalOut leds[4] = {
00027     DigitalOut(LED1),
00028     DigitalOut(LED2),
00029     DigitalOut(LED3),
00030     DigitalOut(LED4)};
00031 
00032 // ROSへ送信するデータ
00033 // [0]リセット信号
00034 // [1]オドメトリx
00035 // [2]オドメトリy
00036 // [3]オドメトリ角度
00037 // [4]ラインセンサ1
00038 // [5]ラインセンサ2
00039 //
00040 // ROSから送られてくるデータ
00041 // [0]リセット信号の応答
00042 // [1]推定自己位置x
00043 // [2]推定自己位置y
00044 // [3]推定自己位置rad
00045 ros::NodeHandle nh;
00046 std_msgs::Float32MultiArray send_data;
00047 ros::Publisher pub_to_ros("mbed_to_ros", &send_data);
00048 ros::Subscriber<std_msgs::Float32MultiArray> sub("ros_to_mbed", &messageCb);
00049 
00050 int main()
00051 {
00052     nh.initNode();
00053     nh.advertise(pub_to_ros);
00054     nh.subscribe(sub);
00055 
00056     // 送信パケットの初期化
00057     send_data.data_length = 6;
00058     send_data.data = (float *)malloc(sizeof(float) * send_data.data_length);
00059     for (int i = 0; i < send_data.data_length; i++)
00060         send_data.data[i] = 0.0;
00061 
00062     // 送信関数を一定周期で呼び出す
00063     flipper.attach(&sendToRos, 0.025);
00064 
00065     while (1)
00066     {
00067         // ジャイロ取得
00068         float gyro[3] = {0.0};
00069         getGyro(gyro);
00070 
00071         // 速度を出す
00072         float robot_velo[3] = {0};
00073         getVelo(robot_odom[2], gyro, robot_velo);
00074 
00075         // オドメトリを計算する
00076         calOdom(robot_velo, robot_odom);
00077 
00078         // ループが動いてるか確認
00079         leds[0] = !leds[0];
00080 
00081         // Y座標が1m以上なら点灯
00082         leds[3] = (pose[1] > 1) ? 1 : 0;
00083 
00084         nh.spinOnce();
00085     }
00086 }
00087 
00088 // ロボットの速度の取得
00089 void getVelo(float angle /*[rad]*/, float gyro[3], float return_velo[3])
00090 {
00091     // 速度配列へ代入
00092     float buf_x = getMPS(qei_x.getRPM(), WHEELDIAMETER) / 0.64;
00093     float buf_y = getMPS(qei_y.getRPM(), WHEELDIAMETER) / 0.64;
00094     return_velo[0] = buf_x * cos(angle) - buf_y * sin(angle);
00095     return_velo[1] = buf_x * sin(angle) + buf_y * cos(angle);
00096     return_velo[2] = gyro[2] * 3.1415926 / 180;
00097 }
00098 
00099 // オドメトリの計算
00100 void calOdom(float robot_velo[3], float return_odom[3])
00101 {
00102     // 速度を積分
00103     static Timer deltaTimer;
00104     deltaTimer.start();
00105     long double dt = deltaTimer.read();
00106     for (int i = 0; i < 3; i++)
00107     {
00108         return_odom[i] += ((robot_velo[i] + last_velo[i]) * (dt / 2.0));
00109         last_velo[i] = robot_velo[i];
00110     }
00111 
00112     deltaTimer.reset();
00113 }
00114 
00115 // ros_to_mbed コールバック関数
00116 void messageCb(const std_msgs::Float32MultiArray &msg)
00117 {
00118     leds[2] = !leds[2];
00119 
00120     pose[0] = msg.data[1];
00121     pose[1] = msg.data[2];
00122     pose[2] = msg.data[3];
00123 }
00124 
00125 // 送信用関数
00126 void sendToRos()
00127 {
00128     send_data.data[0] = 0;
00129     send_data.data[1] = robot_odom[0];
00130     send_data.data[2] = robot_odom[1];
00131     send_data.data[3] = robot_odom[2];
00132     send_data.data[4] = 0b00011000;
00133     send_data.data[5] = 0b11000000;
00134 
00135     // ROSへデータを送信
00136     pub_to_ros.publish(&send_data);
00137 
00138     leds[1] = !leds[1];
00139 }
00140 
00141 void getGyro(float return_gyro[3])
00142 {
00143     double tmp_gyro[3];
00144     if (mpu.read())
00145     {
00146         mpu.getGyro(MPU6050_CORRECT, tmp_gyro);
00147     }
00148 
00149     // 値が小さい時は静止とみなし,大きい場合はそのまま代入
00150     for (int i = 0; i < 3; i++)
00151         return_gyro[i] = (abs(tmp_gyro[i]) < 0.8) ? 0 : (float)tmp_gyro[i];
00152 }