init
Dependencies: mbed ros_lib_kinetic
main.cpp
- Committer:
- surpace0924
- Date:
- 2019-10-29
- Revision:
- 0:b1907fbc1696
File content as of revision 0:b1907fbc1696:
#include "mbed.h" #include <ros.h> #include <std_msgs/Float32MultiArray.h> #include <std_msgs/Int32.h> float TH = 0; double a[9]; bool onLine[7] = {false}; int binarized_data = 0; void messageCb(const std_msgs::Int32 &msg) { binarized_data = msg.data; } Ticker flipper; ros::NodeHandle nh; std_msgs::Float32MultiArray send_data; ros::Publisher pub_to_ros("line_sensor/y", &send_data); ros::Subscriber<std_msgs::Int32> sub("line_sensor/binarized/y", &messageCb); void flip() { for(int i = 0; i < 9; i++) { send_data.data[i] = a[i]; } // ROSへデータを送信 pub_to_ros.publish(&send_data); } AnalogIn sensor[] = { AnalogIn(PA_7), AnalogIn(PA_6), AnalogIn(PA_5), AnalogIn(PA_4), AnalogIn(PB_0), AnalogIn(PB_1), AnalogIn(PA_3), AnalogIn(PA_1), AnalogIn(PA_0)}; DigitalOut leds[] = { DigitalOut(PA_9), DigitalOut(PA_10), DigitalOut(PA_8), DigitalOut(PB_7), DigitalOut(PB_6), DigitalOut(PF_0), DigitalOut(PF_1)}; int main() { nh.initNode(); nh.advertise(pub_to_ros); nh.subscribe(sub); // 送信パケットの初期化 send_data.data_length = 9; send_data.data = (float *)malloc(sizeof(float) * send_data.data_length); for (int i = 0; i < send_data.data_length; i++) send_data.data[i] = 0.0; // 送信関数を一定周期で呼び出す flipper.attach(&flip, 0.02); while (1) { for (int i = 0; i < 9; i++) { a[i] = sensor[i]; } for (int i = 0; i < 7; i++) { leds[i] = (int)((binarized_data >> i) & 1); } nh.spinOnce(); } }