HAVRM mine / Mbed 2 deprecated rosserial_mbed_F64MA

rosserial mbed for Float64MultiArray (In this program, the topic name is "chatter"). you need the node at PC who send Float64MultiArray (In this program, the topic name is "req". And the number of data is same as the returning data ("chatter", in this program, 5 data)). The example is written at comment.

Dependencies:   mbed ros_lib_indigo

Fork of rosserial_mbed_hello_world_publisher by Gary Servin

main.cpp

Committer:
hirokimineshita
Date:
2016-10-25
Revision:
1:c7411b1ccd0f
Parent:
0:17fd7572aedb
Child:
2:b740e3e4c6c9

File content as of revision 1:c7411b1ccd0f:

/*
 * rosserial Publisher Example
 * Prints "hello world!"
 */
/*
#include"mbed.h"
#include <ros.h>
#include <std_msgs/String.h>

ros::NodeHandle  nh;

std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);

char hello[13] = "hello world0";

DigitalOut led = LED1;

int main() {
    int num=0;
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    nh.advertise(chatter);

    while (1) {
        led = !led;
        hello[11]=num+48;
        str_msg.data = hello;
        chatter.publish( &str_msg );
        nh.spinOnce();
        wait_ms(2);
        num++;
        if(num>9)num=0;
    }
}
*/
/*
#include"mbed.h"
#include <ros.h>
#include <std_msgs/Float64MultiArray.h>

ros::NodeHandle  nh;

std_msgs::Float64MultiArray str_msg;

double data[5]={0.1,0.2,0.3,0.4,0};

DigitalOut led = LED1;

int main() {
    nh.getHardware()->setBaud(115200);
    nh.initNode();
    str_msg.layout.dim_length=1;
    str_msg.data_length=5;
    str_msg.layout.dim = (std_msgs::MultiArrayDimension *)
    malloc(sizeof(std_msgs::MultiArrayDimension) * 2);
    str_msg.layout.dim[0].label = "chatter";
    str_msg.layout.dim[0].size = 5;
    str_msg.layout.dim[0].stride = 1*5;
    str_msg.layout.data_offset = 0;
    str_msg.data = (double *)malloc(sizeof(double)*5);
    ros::Publisher chatter("chatter", &str_msg);
    nh.advertise(chatter);
    while (1) {
        led = !led;
        data[4]+=0.1;
        for(int i=0;i<5;i++)str_msg.data[i] = data[i];
        chatter.publish( &str_msg );
        nh.spinOnce();
        wait_ms(1000);
    }
}*/

#include <ros.h>
#include "mbed.h"
#include <std_msgs/Float64MultiArray.h>

//ROS Float32Multiarray messages
ros::NodeHandle  nh;
std_msgs::Float64MultiArray send;
ros::Publisher chatter("chatter", &send);

//Functions
void ros_callback( const std_msgs::Float64MultiArray& req);
ros::Subscriber<std_msgs::Float64MultiArray> sub("req", ros_callback);

DigitalOut led = LED1;
double num=0;

void setup()
{
//  Serial.begin(115200);
//ROS
  nh.getHardware()->setBaud(115200);
  nh.initNode();
  nh.advertise(chatter);
  nh.subscribe(sub);

  send.data_length = 5;
}

void loop()
{
    nh.spinOnce();
}

void ros_callback( const std_msgs::Float64MultiArray& req){
    send=req;
    num+=0.1;
    for(int i=0;i<5;i++)send.data[i]=num;
    chatter.publish(&send);
    led=!led;
}

int main(){
    setup();
    while(1)loop();
}