Software Challenge Urban Pistek

software_training_assignment/CMakeLists.txt

@@ -55,7 +55,7 @@ find_package(catkin REQUIRED COMPONENTS
  )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
+ add_service_files(
     FILES
     resetTurtle.srv
 )

software_training_assignment/package.xml

@@ -49,19 +49,19 @@
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>turtlesim</build_depend>
-  <build_depend>message_generation</build_depend>
+  <depend>roscpp</depend>
+  <depend>rospy</depend>
+  <depend>std_msgs</depend>
+  <depend>turtlesim</depend>
+  <depend>message_generation</depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>rospy</exec_depend> 
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>turtlesim</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
+  <depend>roscpp</depend>
+  <depend>rospy</depend> 
+  <depend>std_msgs</depend>
+  <depend>turtlesim</depend>
+  <depend>message_runtime</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

software_training_assignment/src/main.cpp

@@ -83,25 +83,29 @@ int main(int argc, char **argv) {
         }
     }
 
-    turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]); 
-    //Publisher for turtle distances 
-    ros::Publisher turtleDist_pub = nh.advertise<turtleDistances>("turtle_distnaces", 10); 
-    turtleDist_pub.publish(turtleDist); 
-
-    //Creating a Action Client
-    actionlib::SimpleActionClient<actionlib> ac("moveTurtle", true); 
-    ROS_INFO("Waiting for Action Server to start"); 
-    ac.waitForServer(); 
-    //Define the waypoint for which the turtle should move to 
-    int x = 0; 
-    int y = 0; 
-    int goal[2] = {x, y}; 
-    ac.sendGoal(goal); 
-
-    bool waitForAction = ac.waitForResult(ros::Duration(30.0)); 
-    if(waitForAction){
-        ROS_INFO("Action Finished");
-    } else {
-        ROS_INFO("Action Failed");
+    //Infinite loop to run
+    while(true){
+        turtleDistance turtleDist(x_coords[0], y_coords[0], x_coords[1], y_coords[1]); 
+        //Publisher for turtle distances 
+        ros::Publisher turtleDist_pub = nh.advertise<turtleDistances>("turtle_distnaces", 10); 
+        turtleDist_pub.publish(turtleDist); 
+
+        //Creating a Action Client
+        actionlib::SimpleActionClient<actionlib> ac("moveTurtle", true); 
+        ROS_INFO("Waiting for Action Server to start"); 
+        ac.waitForServer(); 
+        //Define the waypoint for which the turtle should move to 
+        int x = 0; 
+        int y = 0; 
+        int goal[2] = {x, y}; 
+        ac.sendGoal(goal); 
+
+        bool waitForAction = ac.waitForResult(ros::Duration(30.0)); 
+        if(waitForAction){
+            ROS_INFO("Action Finished");
+        } else {
+            ROS_INFO("Action Failed");
+        }
     }
+
 }

software_training_assignment/src/moveTurtle.cpp

@@ -4,43 +4,80 @@ Action server for moving the turtle
 
 #include <ros/ros.h> 
 #include <actionlib/server/simple_action_server.h>
+#include <software_training_assignment/moveTurtle.h>
 //Subscribe to the geometry_msgs/Twist topic 
 #include <geometry_msgs/Twist.h>
+#include <math.h>
 
-bool moveTurtle(int x, int y){
-
+//Class to define the action server
+class MoveTurtleAction {
+    protected: 
+        ros::NodeHandle nh; 
+        actionlib::SimpleActionServer<software_training_assignment::moveTurtle> as;
+        std::string action_name; 
+        software_training_assignment::moveTurtleFeedback feedback; 
+        software_training_assignment::moveTurtleResult result; 
 
-    return success; 
-}
+    public: 
 
-int main(int argc, char **argv) {
-    ros::init(argc, argv, "moveTurtle"); 
-    ros::NodeHandle nh; 
+        MoveTurtleAction(std::string name) : 
+            as(nh, name, boost::bind(&MoveTurtleAction::executeCB, this, _1), false, action_name(name)) {
+                as.start();
+            }
+
+        ~MoveTurtleAction(void){}
+
+        void executeCB(const software_training_assignment::moveTurtleGoalConstPtr &goal){
+
+            bool success = false; 
+            geometry_msgs::Twist vel_msg;
+            ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
+            int end_pos_x = goal->abs_x;
+            int end_pos_y = goal->abs_y; 
+            double distance = sqrt(end_pos_x*end_pos_x + end_pos_y*end_pos_y);
+            double elapsed_dist = 0;
+
+            //start time
+            double t0 = ros::Time::now().toSec();
+            //move turtle to the end position using the unit vector for the velocity
+            vel_msg.linear.x = (end_pos_x/distance);
+            vel_msg.linear.y = (end_pos_y/distance);
+            vel_msg.linear.z = 0;
+            vel_msg.angular.x = 0; 
+            vel_msg.angular.y = 0;
+            vel_msg.angular.z = 0;
 
-    //Create the action
-    actionlib::SimpleActionServer<actionlib> as;
-    std::string action_name  = "moveTurtle"; 
+            //publisher message
+            pub.publish(vel_msg);
+
+            while(elapsed_dist < distance){
+                double t1 = ros::Time::now().toSec();
+                double t_diff = t1 - t0; 
 
-    //Create messages to be published to feedback and result
-    actionlib::Feedback feedback; 
-    actionlib::Result result; 
+                elapsed_dist = sqrt(vel_msg.linear.x*vel_msg.linear.x + vel_msg.linear.y*vel_msg.linear.y)*t_diff;
+                feedback.abs_distance = elapsed_dist; 
+            }
 
-    //Get the pointer to the action goal 
-    const actionlib::GoalConstPtr &goal; 
-    //Set the end destination for the action
-    int x = goal->x; 
-    int y = goal->y; 
+            //stop the turtle from moving
+            vel_msg.linear.x = 0;
+            vel_msg.linear.y = 0;
+            pub.publish(vel_msg);
 
-    //Subscribe to the turtleX/cmd_vel topic
-    ros::Subscriber sub = nh.subscribe("cmd_vel", 10, moveTurtle);
-    //Access the message type parameters
-    geometry_msgs::Twist::vector3::x = x; 
+            //Get the stop time
+            double t_end = ros::Time::now().toSec();
 
-    //Create the action server 
-    as(nh, action_name); 
-    as.start(); 
+            //set the result
+            result.time = (int) (t_end - t0); 
 
-    ros::spin(); 
+        }
+};
+
+int main(int argc, char **argv) {
+
+    ros::init(argc, argv, "moveTurtle");
+    MoveTurtleAction moveTurtle("moveTurtle");
+    ros::spin();
 
     return 0; 
+
 }

software_training_assignment/src/resetTurtle.cpp

@@ -4,25 +4,26 @@ Server for the Reset Turtle Service
 
 #include <ros/ros.h>
 #include <string> 
-
 //Including the service files for resetTurtle service
 #include <software_training_assignment/resetTurtle.h>
 
-//Struct for the service information 
-struct turtleInfo {
-    std::string name; 
-    int x_coord; 
-    int y_coord; 
-}
-
 //Defining the service class 
 class TurtleReset {
 public:
+    int start_x = 25; //Starting x pos for moving turtle
+    int start_y = 10; //Starting y pos for moving turtle
+    std::string turtle_name = "moving_turtle";
     TurtleReset() {}
 
     bool resetTurtlePosition(software_training_assignment::resetTurtleRequest &request, 
                             software_training_assignment::resetTurtleResponse &response){
         ROS_INFO("Turtle Position Reset"); 
+
+        request.x = start_x; 
+        request.y = start_y;
+        request.name = turtle_name; 
+        response.success = true; 
+
         return true; 
     };
 }
@@ -38,7 +39,6 @@ int main(int argc, char **argv) {
     ros::ServiceServer resetTurtle = nh.advertiseService(
         "resetTurtle", &TurtleReset::resetTurtlePosition, &turtle); 
 
-    ros::spin(); //Creates a infinite loop for the server
-
+    ros::spin(); //Creates a infinite loop to process callbacks
     return 0; 
 }