TimeTF.cpp

00001 //#define COMPILE_TIMETF_CODE_ROSSERIAL
00002 #ifdef COMPILE_TIMETF_CODE_ROSSERIAL
00003 
00004 /*
00005  * rosserial Time and TF Example
00006  * Publishes a transform at current time
00007  */
00008 
00009 #include "mbed.h"
00010 #include <ros.h>
00011 #include <ros/time.h>
00012 #include <tf/transform_broadcaster.h>
00013 
00014 ros::NodeHandle  nh;
00015 
00016 geometry_msgs::TransformStamped t;
00017 tf::TransformBroadcaster broadcaster;
00018 
00019 char base_link[] = "/base_link";
00020 char odom[] = "/odom";
00021 
00022 int main() {
00023     nh.initNode();
00024     broadcaster.init(nh);
00025 
00026 
00027     while (1) {
00028         t.header.frame_id = odom;
00029         t.child_frame_id = base_link;
00030         t.transform.translation.x = 1.0;
00031         t.transform.rotation.x = 0.0;
00032         t.transform.rotation.y = 0.0;
00033         t.transform.rotation.z = 0.0;
00034         t.transform.rotation.w = 1.0;
00035         t.header.stamp = nh.now();
00036         broadcaster.sendTransform(t);
00037         nh.spinOnce();
00038         wait_ms(10);
00039     }
00040 }
00041 
00042 #endif