trans test
Dependencies: mbed ros_lib_kinetic
Diff: main.cpp
- Revision:
- 0:230d6c5f4ad7
diff -r 000000000000 -r 230d6c5f4ad7 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jun 19 11:18:25 2019 +0000 @@ -0,0 +1,67 @@ +#include "mbed.h" +#include <ros.h> +#include <std_msgs/Int32MultiArray.h> +#include <std_msgs/Int32.h> +#include <std_msgs/String.h> + +#define DATA_NUM 3 + +ros::NodeHandle nh; + +Timeout toff; +int recieve_data[3]; +bool cb_flg = false; + +Serial micon(D1,D0); + +std_msgs::Int32MultiArray send_data; +std_msgs::String dbg_msg; +ros::Publisher chatter("/Chatter", &send_data); +ros::Publisher debuger("/debuger", &dbg_msg); + +void messageCb(const std_msgs::Int32MultiArray& msg){ + int i; + for(i=0;i<DATA_NUM;i++){ + recieve_data[i] = msg.data[i]; + } + for(i=0;i<DATA_NUM;i++){ + if(i+1<DATA_NUM) send_data.data[i] = recieve_data[i+1]; + else send_data.data[i] = recieve_data[0]; + } + chatter.publish(&send_data); + cb_flg = true; +} + +ros::Subscriber<std_msgs::Int32MultiArray> sub("/data", &messageCb); + +int main(){ + micon.baud(115200); + micon.format(8,Serial::None,1); + send_data.data_length = DATA_NUM; + send_data.data = (int32_t *)malloc(sizeof(int32_t)*DATA_NUM); + int i,j; + for(i=0;i<DATA_NUM;i++){ + send_data.data[i] = 0; + } + nh.getHardware()->setBaud(115200); + nh.initNode(); + nh.subscribe(sub); + nh.advertise(debuger); + nh.advertise(chatter); + + while(1){ + dbg_msg.data = "success"; + debuger.publish(&dbg_msg); + if(cb_flg==true){ + for(j=0;j<DATA_NUM;j++){ + for(i=0;i<4;i++){ + micon.putc((char)(send_data.data[j]>>(i*8) & 0xff)); + wait_ms(1); + } + } + cb_flg = false; + } + nh.spinOnce(); + wait_ms(100); + } +} \ No newline at end of file