Personal fork
Dependencies: Servo mbed rosserial_mbed_lib
Fork of rosserial_mbed by
examples/TimeTF.cpp@0:06fc856e99ca, 2011-08-19 (annotated)
- Committer:
- nucho
- Date:
- Fri Aug 19 09:06:16 2011 +0000
- Revision:
- 0:06fc856e99ca
- Child:
- 3:dff241b66f84
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
nucho | 0:06fc856e99ca | 1 | //#define COMPILE_TIMETF_CODE_ROSSERIAL |
nucho | 0:06fc856e99ca | 2 | #ifdef COMPILE_TIMETF_CODE_ROSSERIAL |
nucho | 0:06fc856e99ca | 3 | |
nucho | 0:06fc856e99ca | 4 | /* |
nucho | 0:06fc856e99ca | 5 | * rosserial Time and TF Example |
nucho | 0:06fc856e99ca | 6 | * Publishes a transform at current time |
nucho | 0:06fc856e99ca | 7 | */ |
nucho | 0:06fc856e99ca | 8 | |
nucho | 0:06fc856e99ca | 9 | #include "mbed.h" |
nucho | 0:06fc856e99ca | 10 | #include <ros.h> |
nucho | 0:06fc856e99ca | 11 | #include <ros/time.h> |
nucho | 0:06fc856e99ca | 12 | |
nucho | 0:06fc856e99ca | 13 | ros::NodeHandle nh; |
nucho | 0:06fc856e99ca | 14 | |
nucho | 0:06fc856e99ca | 15 | #include <tf/transform_broadcaster.h> |
nucho | 0:06fc856e99ca | 16 | |
nucho | 0:06fc856e99ca | 17 | geometry_msgs::TransformStamped t; |
nucho | 0:06fc856e99ca | 18 | tf::TransformBroadcaster broadcaster; |
nucho | 0:06fc856e99ca | 19 | |
nucho | 0:06fc856e99ca | 20 | char base_link[] = "/base_link"; |
nucho | 0:06fc856e99ca | 21 | char odom[] = "/odom"; |
nucho | 0:06fc856e99ca | 22 | |
nucho | 0:06fc856e99ca | 23 | int main() { |
nucho | 0:06fc856e99ca | 24 | nh.initNode(); |
nucho | 0:06fc856e99ca | 25 | |
nucho | 0:06fc856e99ca | 26 | while (1) { |
nucho | 0:06fc856e99ca | 27 | t.header.frame_id = odom; |
nucho | 0:06fc856e99ca | 28 | t.child_frame_id = base_link; |
nucho | 0:06fc856e99ca | 29 | t.transform.translation.x = 1.0; |
nucho | 0:06fc856e99ca | 30 | t.transform.rotation.x = 0.0; |
nucho | 0:06fc856e99ca | 31 | t.transform.rotation.y = 0.0; |
nucho | 0:06fc856e99ca | 32 | t.transform.rotation.z = 0.0; |
nucho | 0:06fc856e99ca | 33 | t.transform.rotation.w = 1.0; |
nucho | 0:06fc856e99ca | 34 | t.header.stamp = nh.now(); |
nucho | 0:06fc856e99ca | 35 | broadcaster.sendTransform(t); |
nucho | 0:06fc856e99ca | 36 | nh.spinOnce(); |
nucho | 0:06fc856e99ca | 37 | wait_ms(10); |
nucho | 0:06fc856e99ca | 38 | } |
nucho | 0:06fc856e99ca | 39 | } |
nucho | 0:06fc856e99ca | 40 | |
nucho | 0:06fc856e99ca | 41 | #endif |