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