Diff: examples/Odom.cpp

Revision:

0:cb1dffdc7d05

diff -r 000000000000 -r cb1dffdc7d05 examples/Odom.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/examples/Odom.cpp	Sun Feb 15 10:54:04 2015 +0000
@@ -0,0 +1,56 @@
+//#define COMPILE_ODOM_CODE_ROSSERIAL
+#ifdef  COMPILE_ODOM_CODE_ROSSERIAL
+
+/*
+ * rosserial Planar Odometry Example
+ */
+
+#include <ros.h>
+#include <ros/time.h>
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+
+ros::NodeHandle  nh;
+
+geometry_msgs::TransformStamped t;
+tf::TransformBroadcaster broadcaster;
+
+double x = 1.0;
+double y = 0.0;
+double theta = 1.57;
+
+char base_link[] = "/base_link";
+char odom[] = "/odom";
+
+int main(void) {
+    nh.initNode();
+    broadcaster.init(nh);
+
+
+    while (1) {
+        // drive in a circle
+        double dx = 0.2;
+        double dtheta = 0.18;
+        x += cos(theta)*dx*0.1;
+        y += sin(theta)*dx*0.1;
+        theta += dtheta*0.1;
+        if (theta > 3.14)
+            theta=-3.14;
+
+        // tf odom->base_link
+        t.header.frame_id = odom;
+        t.child_frame_id = base_link;
+
+        t.transform.translation.x = x;
+        t.transform.translation.y = y;
+
+        t.transform.rotation = tf::createQuaternionFromYaw(theta);
+        t.header.stamp = nh.now();
+
+        broadcaster.sendTransform(t);
+        nh.spinOnce();
+
+        wait_ms(10);
+    }
+}
+#endif
\ No newline at end of file