modify for Hydro version
Fork of rosserial_mbed_lib by
tests/float64_test.cpp@3:1cf99502f396, 2011-11-12 (annotated)
- Committer:
- nucho
- Date:
- Sat Nov 12 23:54:45 2011 +0000
- Revision:
- 3:1cf99502f396
This program supported the revision of 167 of rosserial.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nucho | 3:1cf99502f396 | 1 | //#define COMPLIE_FLOAT64_CODE_ROSSERIAL |
nucho | 3:1cf99502f396 | 2 | #ifdef COMPILE_FLOAT64_CODE_ROSSERIAL |
nucho | 3:1cf99502f396 | 3 | |
nucho | 3:1cf99502f396 | 4 | /* |
nucho | 3:1cf99502f396 | 5 | * rosserial::std_msgs::Float64 Test |
nucho | 3:1cf99502f396 | 6 | * Receives a Float64 input, subtracts 1.0, and publishes it |
nucho | 3:1cf99502f396 | 7 | */ |
nucho | 3:1cf99502f396 | 8 | |
nucho | 3:1cf99502f396 | 9 | #include "mbed.h" |
nucho | 3:1cf99502f396 | 10 | #include <ros.h> |
nucho | 3:1cf99502f396 | 11 | #include <std_msgs/Float64.h> |
nucho | 3:1cf99502f396 | 12 | |
nucho | 3:1cf99502f396 | 13 | |
nucho | 3:1cf99502f396 | 14 | ros::NodeHandle nh; |
nucho | 3:1cf99502f396 | 15 | |
nucho | 3:1cf99502f396 | 16 | float x; |
nucho | 3:1cf99502f396 | 17 | DigitalOut myled(LED1); |
nucho | 3:1cf99502f396 | 18 | |
nucho | 3:1cf99502f396 | 19 | void messageCb( const std_msgs::Float64& msg) { |
nucho | 3:1cf99502f396 | 20 | x = msg.data - 1.0; |
nucho | 3:1cf99502f396 | 21 | myled = !myled; // blink the led |
nucho | 3:1cf99502f396 | 22 | } |
nucho | 3:1cf99502f396 | 23 | |
nucho | 3:1cf99502f396 | 24 | std_msgs::Float64 test; |
nucho | 3:1cf99502f396 | 25 | ros::Subscriber<std_msgs::Float64> s("your_topic", &messageCb); |
nucho | 3:1cf99502f396 | 26 | ros::Publisher p("my_topic", &test); |
nucho | 3:1cf99502f396 | 27 | |
nucho | 3:1cf99502f396 | 28 | int main() { |
nucho | 3:1cf99502f396 | 29 | nh.initNode(); |
nucho | 3:1cf99502f396 | 30 | nh.advertise(p); |
nucho | 3:1cf99502f396 | 31 | nh.subscribe(s); |
nucho | 3:1cf99502f396 | 32 | while (1) { |
nucho | 3:1cf99502f396 | 33 | test.data = x; |
nucho | 3:1cf99502f396 | 34 | p.publish( &test ); |
nucho | 3:1cf99502f396 | 35 | nh.spinOnce(); |
nucho | 3:1cf99502f396 | 36 | wait_ms(10); |
nucho | 3:1cf99502f396 | 37 | } |
nucho | 3:1cf99502f396 | 38 | } |
nucho | 3:1cf99502f396 | 39 | #endif |