Odom.cpp

00001 //#define COMPILE_ODOM_CODE_ROSSERIAL
00002 #ifdef  COMPILE_ODOM_CODE_ROSSERIAL
00003 
00004 /*
00005  * rosserial Planar Odometry Example
00006  */
00007 
00008 #include <ros.h>
00009 #include <ros/time.h>
00010 #include <tf/tf.h>
00011 #include <tf/transform_broadcaster.h>
00012 
00013 ros::NodeHandle  nh;
00014 
00015 geometry_msgs::TransformStamped t;
00016 tf::TransformBroadcaster broadcaster;
00017 
00018 double x = 1.0;
00019 double y = 0.0;
00020 double theta = 1.57;
00021 
00022 char base_link[] = "/base_link";
00023 char odom[] = "/odom";
00024 
00025 int main(void) {
00026     nh.initNode();
00027     broadcaster.init(nh);
00028 
00029 
00030     while (1) {
00031         // drive in a circle
00032         double dx = 0.2;
00033         double dtheta = 0.18;
00034         x += cos(theta)*dx*0.1;
00035         y += sin(theta)*dx*0.1;
00036         theta += dtheta*0.1;
00037         if (theta > 3.14)
00038             theta=-3.14;
00039 
00040         // tf odom->base_link
00041         t.header.frame_id = odom;
00042         t.child_frame_id = base_link;
00043 
00044         t.transform.translation.x = x;
00045         t.transform.translation.y = y;
00046 
00047         t.transform.rotation = tf::createQuaternionFromYaw(theta);
00048         t.header.stamp = nh.now();
00049 
00050         broadcaster.sendTransform(t);
00051         nh.spinOnce();
00052 
00053         wait_ms(10);
00054     }
00055 }
00056 #endif