okapia johnstoni / Mbed 2 deprecated ros_mbed_transfer

trans test

Dependencies:   mbed ros_lib_kinetic

main.cpp@0:230d6c5f4ad7, 2019-06-19 (annotated)

Committer:
okapi
Date:
Wed Jun 19 11:18:25 2019 +0000
Revision:
0:230d6c5f4ad7
se

Who changed what in which revision?

UserRevisionLine numberNew contents of line
okapi 0:230d6c5f4ad7 1 #include "mbed.h"
okapi 0:230d6c5f4ad7 2 #include <ros.h>
okapi 0:230d6c5f4ad7 3 #include <std_msgs/Int32MultiArray.h>
okapi 0:230d6c5f4ad7 4 #include <std_msgs/Int32.h>
okapi 0:230d6c5f4ad7 5 #include <std_msgs/String.h>
okapi 0:230d6c5f4ad7 6
okapi 0:230d6c5f4ad7 7 #define DATA_NUM 3
okapi 0:230d6c5f4ad7 8
okapi 0:230d6c5f4ad7 9 ros::NodeHandle nh;
okapi 0:230d6c5f4ad7 10
okapi 0:230d6c5f4ad7 11 Timeout toff;
okapi 0:230d6c5f4ad7 12 int recieve_data[3];
okapi 0:230d6c5f4ad7 13 bool cb_flg = false;
okapi 0:230d6c5f4ad7 14
okapi 0:230d6c5f4ad7 15 Serial micon(D1,D0);
okapi 0:230d6c5f4ad7 16
okapi 0:230d6c5f4ad7 17 std_msgs::Int32MultiArray send_data;
okapi 0:230d6c5f4ad7 18 std_msgs::String dbg_msg;
okapi 0:230d6c5f4ad7 19 ros::Publisher chatter("/Chatter", &send_data);
okapi 0:230d6c5f4ad7 20 ros::Publisher debuger("/debuger", &dbg_msg);
okapi 0:230d6c5f4ad7 21
okapi 0:230d6c5f4ad7 22 void messageCb(const std_msgs::Int32MultiArray& msg){
okapi 0:230d6c5f4ad7 23 int i;
okapi 0:230d6c5f4ad7 24 for(i=0;i<DATA_NUM;i++){
okapi 0:230d6c5f4ad7 25 recieve_data[i] = msg.data[i];
okapi 0:230d6c5f4ad7 26 }
okapi 0:230d6c5f4ad7 27 for(i=0;i<DATA_NUM;i++){
okapi 0:230d6c5f4ad7 28 if(i+1<DATA_NUM) send_data.data[i] = recieve_data[i+1];
okapi 0:230d6c5f4ad7 29 else send_data.data[i] = recieve_data[0];
okapi 0:230d6c5f4ad7 30 }
okapi 0:230d6c5f4ad7 31 chatter.publish(&send_data);
okapi 0:230d6c5f4ad7 32 cb_flg = true;
okapi 0:230d6c5f4ad7 33 }
okapi 0:230d6c5f4ad7 34
okapi 0:230d6c5f4ad7 35 ros::Subscriber<std_msgs::Int32MultiArray> sub("/data", &messageCb);
okapi 0:230d6c5f4ad7 36
okapi 0:230d6c5f4ad7 37 int main(){
okapi 0:230d6c5f4ad7 38 micon.baud(115200);
okapi 0:230d6c5f4ad7 39 micon.format(8,Serial::None,1);
okapi 0:230d6c5f4ad7 40 send_data.data_length = DATA_NUM;
okapi 0:230d6c5f4ad7 41 send_data.data = (int32_t *)malloc(sizeof(int32_t)*DATA_NUM);
okapi 0:230d6c5f4ad7 42 int i,j;
okapi 0:230d6c5f4ad7 43 for(i=0;i<DATA_NUM;i++){
okapi 0:230d6c5f4ad7 44 send_data.data[i] = 0;
okapi 0:230d6c5f4ad7 45 }
okapi 0:230d6c5f4ad7 46 nh.getHardware()->setBaud(115200);
okapi 0:230d6c5f4ad7 47 nh.initNode();
okapi 0:230d6c5f4ad7 48 nh.subscribe(sub);
okapi 0:230d6c5f4ad7 49 nh.advertise(debuger);
okapi 0:230d6c5f4ad7 50 nh.advertise(chatter);
okapi 0:230d6c5f4ad7 51
okapi 0:230d6c5f4ad7 52 while(1){
okapi 0:230d6c5f4ad7 53 dbg_msg.data = "success";
okapi 0:230d6c5f4ad7 54 debuger.publish(&dbg_msg);
okapi 0:230d6c5f4ad7 55 if(cb_flg==true){
okapi 0:230d6c5f4ad7 56 for(j=0;j<DATA_NUM;j++){
okapi 0:230d6c5f4ad7 57 for(i=0;i<4;i++){
okapi 0:230d6c5f4ad7 58 micon.putc((char)(send_data.data[j]>>(i*8) & 0xff));
okapi 0:230d6c5f4ad7 59 wait_ms(1);
okapi 0:230d6c5f4ad7 60 }
okapi 0:230d6c5f4ad7 61 }
okapi 0:230d6c5f4ad7 62 cb_flg = false;
okapi 0:230d6c5f4ad7 63 }
okapi 0:230d6c5f4ad7 64 nh.spinOnce();
okapi 0:230d6c5f4ad7 65 wait_ms(100);
okapi 0:230d6c5f4ad7 66 }
okapi 0:230d6c5f4ad7 67 }