main.cpp

00001 /*
00002  * rosserial Publisher Example
00003  * Prints "hello world!"
00004  */
00005 
00006 /* ROS node program
00007 #include <ros/ros.h>
00008 #include <std_msgs/Float64MultiArray.h>
00009 
00010 int main(int argc,char **argv){
00011   ros::init(argc,argv,"serial_test");
00012   ros::NodeHandle n;
00013   ros::Publisher req_pub=n.advertise<std_msgs::Float64MultiArray>("req", 1000);
00014 
00015   ros::Rate loop_rate(174);
00016   while (ros::ok()){
00017     std_msgs::Float64MultiArray msg;
00018     msg.data.clear();
00019     for(int i=0;i<5;i++)msg.data.push_back(0);
00020     req_pub.publish(msg);
00021     ROS_INFO("send");
00022     ros::spinOnce();
00023     loop_rate.sleep();
00024   }
00025   return 0;
00026 }
00027 */
00028 
00029 #include <ros.h>
00030 #include "mbed.h"
00031 #include <std_msgs/Float64MultiArray.h>
00032 
00033 //ROS Float32Multiarray messages
00034 ros::NodeHandle  nh;
00035 std_msgs::Float64MultiArray send;
00036 ros::Publisher chatter("chatter", &send);
00037 
00038 //Functions
00039 void ros_callback( const std_msgs::Float64MultiArray& req);
00040 ros::Subscriber<std_msgs::Float64MultiArray> sub("req", ros_callback);
00041 
00042 DigitalOut led = LED1;
00043 double num=0;
00044 
00045 void setup()
00046 {
00047 //  Serial.begin(115200);
00048 //ROS
00049   nh.getHardware()->setBaud(115200);
00050   nh.initNode();
00051   nh.advertise(chatter);
00052   nh.subscribe(sub);
00053 
00054   send.data_length = 5;
00055 }
00056 
00057 void loop()
00058 {
00059     nh.spinOnce();
00060 }
00061 
00062 void ros_callback( const std_msgs::Float64MultiArray& req){
00063     send=req;
00064     num+=0.1;
00065     for(int i=0;i<5;i++)send.data[i]=num;
00066     chatter.publish(&send);
00067     led=!led;
00068 }
00069 
00070 int main(){
00071     setup();
00072     while(1)loop();
00073 }