[Personalrobots-commit] SF.net SVN: personalrobots:[23778] pkg/trunk/sandbox/turtlesim

[<jfa...@us...>]

Revision: 23778
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23778&view=rev
Author:   jfaustwg
Date:     2009-09-03 20:39:13 +0000 (Thu, 03 Sep 2009)

Log Message:
-----------
Wrap theta around [0,2PI), rather than just letting it grow.  Add a mimic tutorial node, and a multi-turtlesim launch file.

Use a fixed 60hz timestep, rather than real time, to make things perfectly reproducable

Modified Paths:
--------------
    pkg/trunk/sandbox/turtlesim/CMakeLists.txt
    pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp
    pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp

Added Paths:
-----------
    pkg/trunk/sandbox/turtlesim/launch/
    pkg/trunk/sandbox/turtlesim/launch/multisim.launch
    pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp

Modified: pkg/trunk/sandbox/turtlesim/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/turtlesim/CMakeLists.txt	2009-09-03 20:28:55 UTC (rev 23777)
+++ pkg/trunk/sandbox/turtlesim/CMakeLists.txt	2009-09-03 20:39:13 UTC (rev 23778)
@@ -28,4 +28,5 @@
 rosbuild_link_boost(turtlesim thread)
 target_link_libraries(turtlesim ${wxWidgets_LIBRARIES})
 
-rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
\ No newline at end of file
+rosbuild_add_executable(draw_square tutorials/draw_square.cpp)
+rosbuild_add_executable(mimic tutorials/mimic.cpp)

Added: pkg/trunk/sandbox/turtlesim/launch/multisim.launch
===================================================================
--- pkg/trunk/sandbox/turtlesim/launch/multisim.launch	                        (rev 0)
+++ pkg/trunk/sandbox/turtlesim/launch/multisim.launch	2009-09-03 20:39:13 UTC (rev 23778)
@@ -0,0 +1,8 @@
+<launch>
+	<group ns="turtlesim1">
+	       <node pkg="turtlesim" name="sim" type="turtlesim"/>
+	</group>
+	<group ns="turtlesim2">
+	       <node pkg="turtlesim" name="sim" type="turtlesim"/>
+	</group>
+</launch>
\ No newline at end of file

Modified: pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp	2009-09-03 20:28:55 UTC (rev 23777)
+++ pkg/trunk/sandbox/turtlesim/src/turtle_frame.cpp	2009-09-03 20:39:13 UTC (rev 23778)
@@ -127,11 +127,16 @@
 
   Vector2 old_pos = pos_;
 
+#if 0
   ros::WallTime now = ros::WallTime::now();
   double dt = (now - last_turtle_update_).toSec();
   last_turtle_update_ = now;
+#else
+  // just use a 60hz step, to avoid the randomness that using real time brings.
+  double dt = 0.016;
+#endif
 
-  orient_ += ang_vel_ * dt;
+  orient_ = fmod(orient_ + ang_vel_ * dt, 2*PI);
   pos_.x += sin(orient_ + PI) * lin_vel_ * dt;
   pos_.y += cos(orient_ + PI) * lin_vel_ * dt;
 

Modified: pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp	2009-09-03 20:28:55 UTC (rev 23777)
+++ pkg/trunk/sandbox/turtlesim/tutorials/draw_square.cpp	2009-09-03 20:39:13 UTC (rev 23778)
@@ -56,7 +56,7 @@
     g_state = TURN;
     g_goal.x = g_pose->x;
     g_goal.y = g_pose->y;
-    g_goal.theta = g_pose->theta + PI/4.0;
+    g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
     printGoal();
   }
   else

Added: pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp
===================================================================
--- pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp	                        (rev 0)
+++ pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp	2009-09-03 20:39:13 UTC (rev 23778)
@@ -0,0 +1,40 @@
+#include <ros/ros.h>
+#include <turtlesim/Pose.h>
+#include <turtlesim/Velocity.h>
+
+class Mimic
+{
+public:
+  Mimic();
+
+private:
+  void poseCallback(const turtlesim::PoseConstPtr& pose);
+
+  ros::Publisher vel_pub_;
+  ros::Subscriber pose_sub_;
+};
+
+Mimic::Mimic()
+{
+  ros::NodeHandle input_nh("input");
+  ros::NodeHandle output_nh("output");
+  vel_pub_ = output_nh.advertise<turtlesim::Velocity>("command_velocity", 1);
+  pose_sub_ = input_nh.subscribe<turtlesim::Pose>("turtle_pose", 1, &Mimic::poseCallback, this);
+}
+
+void Mimic::poseCallback(const turtlesim::PoseConstPtr& pose)
+{
+  turtlesim::Velocity vel;
+  vel.angular = pose->angular_velocity;
+  vel.linear = pose->linear_velocity;
+  vel_pub_.publish(vel);
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "turtle_mimic");
+  Mimic mimic;
+
+  ros::spin();
+}
+


Property changes on: pkg/trunk/sandbox/turtlesim/tutorials/mimic.cpp
___________________________________________________________________
Added: svn:mergeinfo
   + 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.