init
Dependencies: mbed ros_lib_kinetic
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 }
Generated on Sun Aug 28 2022 16:54:23 by
1.7.2