arduino代码

libraries/Rosserial_Arduino_Library/examples/Odom/Odom.pde

@@ -0,0 +1,53 @@
+/* 
+ * 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";
+
+void setup()
+{
+  nh.initNode();
+  broadcaster.init(nh);
+}
+
+void loop()
+{  
+  // 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();
+  
+  delay(10);
+}