init

Dependencies:   mbed 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 <std_msgs/Int32.h>
00005 
00006 float TH = 0;
00007 double a[9];
00008 bool onLine[7] = {false};
00009 int binarized_data = 0;
00010 void messageCb(const std_msgs::Int32 &msg)
00011 {
00012     binarized_data = msg.data;
00013 }
00014 
00015 Ticker flipper;
00016 ros::NodeHandle nh;
00017 std_msgs::Float32MultiArray send_data;
00018 ros::Publisher pub_to_ros("line_sensor/y", &send_data);
00019 ros::Subscriber<std_msgs::Int32> sub("line_sensor/binarized/y", &messageCb);
00020 
00021 
00022 void flip() {
00023     for(int i = 0; i < 9; i++)
00024     {
00025         send_data.data[i] = a[i];
00026     }
00027 
00028     // ROSへデータを送信
00029     pub_to_ros.publish(&send_data);
00030 }
00031 
00032 AnalogIn sensor[] = {
00033     AnalogIn(PA_7),
00034     AnalogIn(PA_6),
00035     AnalogIn(PA_5),
00036     AnalogIn(PA_4),
00037     AnalogIn(PB_0),
00038     AnalogIn(PB_1),
00039     AnalogIn(PA_3),
00040     AnalogIn(PA_1),
00041     AnalogIn(PA_0)};
00042 
00043 DigitalOut leds[] = {
00044     DigitalOut(PA_9),
00045     DigitalOut(PA_10),
00046     DigitalOut(PA_8),
00047     DigitalOut(PB_7),
00048     DigitalOut(PB_6),
00049     DigitalOut(PF_0),
00050     DigitalOut(PF_1)};
00051 
00052 int main()
00053 {
00054     nh.initNode();
00055     nh.advertise(pub_to_ros);
00056     nh.subscribe(sub);
00057     
00058     // 送信パケットの初期化
00059     send_data.data_length = 9;
00060     send_data.data = (float *)malloc(sizeof(float) * send_data.data_length);
00061     for (int i = 0; i < send_data.data_length; i++)
00062         send_data.data[i] = 0.0;
00063 
00064     // 送信関数を一定周期で呼び出す
00065     flipper.attach(&flip, 0.02);
00066     
00067     while (1)
00068     {
00069         for (int i = 0; i < 9; i++)
00070         {
00071             a[i] = sensor[i];
00072         }
00073         for (int i = 0; i < 7; i++)
00074         {   
00075             leds[i] = (int)((binarized_data >> i) & 1);
00076         }
00077 
00078         nh.spinOnce();
00079     }
00080 }