draw_square.cpp
Go to the documentation of this file.
00001 #include <boost/bind.hpp>
00002 #include <ros/ros.h>
00003 #include <turtlesim/Pose.h>
00004 #include <geometry_msgs/Twist.h>
00005 #include <std_srvs/Empty.h>
00006 
00007 turtlesim::PoseConstPtr g_pose;
00008 turtlesim::Pose g_goal;
00009 
00010 enum State
00011 {
00012   FORWARD,
00013   STOP_FORWARD,
00014   TURN,
00015   STOP_TURN,
00016 };
00017 
00018 State g_state = FORWARD;
00019 State g_last_state = FORWARD;
00020 bool g_first_goal_set = false;
00021 
00022 #define PI 3.141592
00023 
00024 void poseCallback(const turtlesim::PoseConstPtr& pose)
00025 {
00026   g_pose = pose;
00027 }
00028 
00029 bool hasReachedGoal()
00030 {
00031   return fabsf(g_pose->x - g_goal.x) < 0.1 && fabsf(g_pose->y - g_goal.y) < 0.1 && fabsf(g_pose->theta - g_goal.theta) < 0.01;
00032 }
00033 
00034 bool hasStopped()
00035 {
00036   return g_pose->angular_velocity < 0.0001 && g_pose->linear_velocity < 0.0001;
00037 }
00038 
00039 void printGoal()
00040 {
00041   ROS_INFO("New goal [%f %f, %f]", g_goal.x, g_goal.y, g_goal.theta);
00042 }
00043 
00044 void commandTurtle(ros::Publisher twist_pub, float linear, float angular)
00045 {
00046   geometry_msgs::Twist twist;
00047   twist.linear.x = linear;
00048   twist.angular.z = angular;
00049   twist_pub.publish(twist);
00050 }
00051 
00052 void stopForward(ros::Publisher twist_pub)
00053 {
00054   if (hasStopped())
00055   {
00056     ROS_INFO("Reached goal");
00057     g_state = TURN;
00058     g_goal.x = g_pose->x;
00059     g_goal.y = g_pose->y;
00060     g_goal.theta = fmod(g_pose->theta + PI/2.0, 2*PI);
00061     printGoal();
00062   }
00063   else
00064   {
00065     commandTurtle(twist_pub, 0, 0);
00066   }
00067 }
00068 
00069 void stopTurn(ros::Publisher twist_pub)
00070 {
00071   if (hasStopped())
00072   {
00073     ROS_INFO("Reached goal");
00074     g_state = FORWARD;
00075     g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
00076     g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
00077     g_goal.theta = g_pose->theta;
00078     printGoal();
00079   }
00080   else
00081   {
00082     commandTurtle(twist_pub, 0, 0);
00083   }
00084 }
00085 
00086 
00087 void forward(ros::Publisher twist_pub)
00088 {
00089   if (hasReachedGoal())
00090   {
00091     g_state = STOP_FORWARD;
00092     commandTurtle(twist_pub, 0, 0);
00093   }
00094   else
00095   {
00096     commandTurtle(twist_pub, 1.0, 0.0);
00097   }
00098 }
00099 
00100 void turn(ros::Publisher twist_pub)
00101 {
00102   if (hasReachedGoal())
00103   {
00104     g_state = STOP_TURN;
00105     commandTurtle(twist_pub, 0, 0);
00106   }
00107   else
00108   {
00109     commandTurtle(twist_pub, 0.0, 0.4);
00110   }
00111 }
00112 
00113 void timerCallback(const ros::TimerEvent&, ros::Publisher twist_pub)
00114 {
00115   if (!g_pose)
00116   {
00117     return;
00118   }
00119 
00120   if (!g_first_goal_set)
00121   {
00122     g_first_goal_set = true;
00123     g_state = FORWARD;
00124     g_goal.x = cos(g_pose->theta) * 2 + g_pose->x;
00125     g_goal.y = sin(g_pose->theta) * 2 + g_pose->y;
00126     g_goal.theta = g_pose->theta;
00127     printGoal();
00128   }
00129 
00130   if (g_state == FORWARD)
00131   {
00132     forward(twist_pub);
00133   }
00134   else if (g_state == STOP_FORWARD)
00135   {
00136     stopForward(twist_pub);
00137   }
00138   else if (g_state == TURN)
00139   {
00140     turn(twist_pub);
00141   }
00142   else if (g_state == STOP_TURN)
00143   {
00144     stopTurn(twist_pub);
00145   }
00146 }
00147 
00148 int main(int argc, char** argv)
00149 {
00150   ros::init(argc, argv, "draw_square");
00151   ros::NodeHandle nh;
00152   ros::Subscriber pose_sub = nh.subscribe("turtle1/pose", 1, poseCallback);
00153   ros::Publisher twist_pub = nh.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);
00154   ros::ServiceClient reset = nh.serviceClient<std_srvs::Empty>("reset");
00155   ros::Timer timer = nh.createTimer(ros::Duration(0.016), boost::bind(timerCallback, _1, twist_pub));
00156 
00157   std_srvs::Empty empty;
00158   reset.call(empty);
00159 
00160   ros::spin();
00161 }


turtlesim
Author(s): Josh Faust
autogenerated on Fri Aug 28 2015 12:38:24