arduino代码

libraries/Rosserial_Arduino_Library/extras/tests/array_test/array_test.pde

@@ -0,0 +1,49 @@
+/* 
+ * rosserial::geometry_msgs::PoseArray Test
+ * Sums an array, publishes sum 
+ */
+
+#include <ros.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseArray.h>
+
+
+ros::NodeHandle nh;
+
+
+bool set_; 
+
+
+geometry_msgs::Pose sum_msg;
+ros::Publisher p("sum", &sum_msg);
+
+void messageCb(const geometry_msgs::PoseArray& msg){
+  sum_msg.position.x = 0;
+  sum_msg.position.y = 0;
+  sum_msg.position.z = 0;
+  for(int i = 0; i < msg.poses_length; i++)
+  {
+    sum_msg.position.x += msg.poses[i].position.x;
+    sum_msg.position.y += msg.poses[i].position.y;
+    sum_msg.position.z += msg.poses[i].position.z;
+  }
+  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+}
+
+ros::Subscriber<geometry_msgs::PoseArray> s("poses",messageCb);
+
+void setup()
+{ 
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.subscribe(s);
+  nh.advertise(p);
+}
+
+void loop()
+{  
+  p.publish(&sum_msg);
+  nh.spinOnce();
+  delay(10);
+}
+

libraries/Rosserial_Arduino_Library/extras/tests/float64_test/float64_test.pde

@@ -0,0 +1,38 @@
+/* 
+ * rosserial::std_msgs::Float64 Test
+ * Receives a Float64 input, subtracts 1.0, and publishes it
+ */
+
+#include <ros.h>
+#include <std_msgs/Float64.h>
+
+
+ros::NodeHandle nh;
+
+float x; 
+
+void messageCb( const std_msgs::Float64& msg){
+  x = msg.data - 1.0;
+  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
+}
+
+std_msgs::Float64 test;
+ros::Subscriber<std_msgs::Float64> s("your_topic", &messageCb);
+ros::Publisher p("my_topic", &test);
+
+void setup()
+{
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.advertise(p);
+  nh.subscribe(s);
+}
+
+void loop()
+{
+  test.data = x;
+  p.publish( &test );
+  nh.spinOnce();
+  delay(10);
+}
+

libraries/Rosserial_Arduino_Library/extras/tests/time_test/time_test.pde

@@ -0,0 +1,30 @@
+/* 
+ * rosserial::std_msgs::Time Test
+ * Publishes current time
+ */
+
+#include <ros.h>
+#include <ros/time.h>
+#include <std_msgs/Time.h>
+
+
+ros::NodeHandle  nh;
+
+std_msgs::Time test;
+ros::Publisher p("my_topic", &test);
+
+void setup()
+{
+  pinMode(13, OUTPUT);
+  nh.initNode();
+  nh.advertise(p);
+}
+
+void loop()
+{  
+  test.data = nh.now();
+  p.publish( &test );
+  nh.spinOnce();
+  delay(10);
+}
+