Create an action client/server implementation

ca_gazebo/include/ca_gazebo/color_sensor_plugin.h

@@ -50,4 +50,4 @@
       }; // GazeboRosColor
     } // gazebo
     #endif // GAZEBO_ROS_COLOR_SENSOR_HH
-
+

ca_gazebo/launch/create_empty_world.launch

@@ -30,7 +30,7 @@
 
   <!-- map_server -->
   <include file="$(find ca_move_base)/launch/map_server.launch">
-	<arg name="env" value="$(arg env)"/>
+	<arg name="env" value="empty"/>
   </include>
 
 </launch>

ca_tools/CMakeLists.txt

@@ -4,6 +4,7 @@ project(ca_tools)
 find_package(catkin REQUIRED
   actionlib_msgs
   message_generation
+  geometry_msgs
   )
 
 add_action_files(
@@ -14,6 +15,7 @@ add_action_files(
 generate_messages(
   DEPENDENCIES
   actionlib_msgs
+  geometry_msgs
 )
 
 catkin_package(

ca_tools/action/FollowLine.action

@@ -1,10 +1,23 @@
 # Goal
-int32 amount
-
+#
+# goal_seconds: receives a time in seconds that is the max-time to perform the action, if the robot doesn't reach the goal at least in that time
+# the result is a False
+# goal_position: receives the position in 2D where the robot needs to reach in order to complete the task
+int32 goal_seconds
+geometry_msgs/Point goal_position
 ---
 # Result
-string[] final_result
-
+#
+# goal_achieved: a boolean that indicates if the robot gets in the position in time or not
+# total_time: the time that took the robot to complete the task
+bool goal_achieved
+duration total_time
 ---
 # Feedback
-int32 number_of_minutes
+#
+# current_mission_duration: the that passed since the start of the action
+# accumulated_distance: the distance traveled since the start of the action
+# times_oop: the times that the robot went out-of-track
+duration current_mission_duration
+float64 accumulated_distance
+int32 times_oop

ca_tools/launch/action_launch.launch

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="goal_seconds" default="300"/>
+  <arg name="goal_pos_x" default="-1.7"/>
+  <arg name="goal_pos_y" default="1.7"/>
+
+  <!-- Action Client -->
+  <node pkg="ca_tools" type="action_client.py" name="follow_line_client"/>
+  <param name="get_seconds" type="double" value="$(arg goal_seconds)" />
+  <param name="get_pos_x" type="double" value="$(arg goal_pos_x)" />
+  <param name="get_pos_y" type="double" value="$(arg goal_pos_y)" />
+
+  <!-- Action Server -->
+  <node pkg="ca_tools" type="action_server.py" name="follow_line_action"/>
+
+  <!-- Robot Controller -->
+  <node pkg="ca_tools" type="robot_controller.py" name="robot_controller"/>
+</launch>

ca_tools/scripts/action_client.py

@@ -0,0 +1,27 @@
+#! /usr/bin/env python
+
+import rospy
+import actionlib
+from ca_tools.msg import FollowLineAction, FollowLineGoal
+from geometry_msgs.msg import Point
+
+
+def robot_client():
+    client = actionlib.SimpleActionClient('follow_line_action', FollowLineAction)
+    client.wait_for_server()
+    goal_secs = rospy.get_param("/get_seconds")
+    goal_pose = Point()
+    goal_pose.x = rospy.get_param("/get_pos_x")
+    goal_pose.y = rospy.get_param("/get_pos_y")
+    goal = FollowLineGoal(goal_seconds=goal_secs, goal_position=goal_pose)
+    client.send_goal(goal)
+    client.wait_for_result()
+    return _client.get_result()
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('follow_line_client')
+        result = robot_client()
+    except rospy.ROSInterruptException:
+        pass

ca_tools/scripts/action_server.py

@@ -0,0 +1,96 @@
+#! /usr/bin/env python
+
+import rospy
+import math
+import actionlib
+from geometry_msgs.msg import Point
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Bool
+from ca_tools.msg import FollowLineAction, FollowLineFeedback, FollowLineResult
+
+
+class RobotAction(object):
+    """
+    This class keeps track of the goals, check if the position goal is achieved, keep track of the total time and traveled distance since the 
+    start of the process
+    """
+    _ERROR_TOLERANCE = 0.1
+
+    def __init__(self, name):
+        self._accumulated_dist = 0.0
+        self._current_pos = Point()
+        self._last_pos = Point()
+        self._gts_first = True
+        self._current_pos.x = None
+        self._current_pos.y = None
+        self._gts_subscriber = rospy.Subscriber("/create1/gts", Odometry, self.gts_callback)
+        self._action_name = name
+        self._as = actionlib.SimpleActionServer(
+            self._action_name, FollowLineAction, execute_cb=self.execute_cb, auto_start = False)
+        self._as.start()
+        self._goal_reached = False
+#        self._curr_time = rospy.Time.now()
+        self._rate = rospy.Rate(2)
+
+    def gts_callback(self, data):
+        if self._gts_first:
+            self._current_pos.x = data.pose.pose.position.x
+            self._current_pos.y = data.pose.pose.position.y
+            self._gts_first = False
+        else:
+            self._last_pos.x = self._current_pos.x
+            self._last_pos.y = self._current_pos.y
+            self._current_pos.x = data.pose.pose.position.x
+            self._current_pos.y = data.pose.pose.position.y
+            self._accumulated_dist += math.hypot(abs( self._last_pos.x - self._current_pos.x), self._last_pos.y -
+                                     self._current_pos.y)
+
+    def goal_reach_control(self, goal_point):
+        x_limit = (self._current_pos.x <= (goal_point.x + self._ERROR_TOLERANCE)) and (self._current_pos.x
+        >= (goal_point.x - self._ERROR_TOLERANCE))
+        y_limit = (self._current_pos.y <= (goal_point.y + self._ERROR_TOLERANCE)) and (self._current_pos.y
+        >= (goal_point.y - self._ERROR_TOLERANCE))
+        return x_limit and y_limit
+
+    def execute_cb(self, goal):
+        # helper variables
+        success = True
+        goal_point = Point()
+        goal_point = goal.goal_position
+        goal_seconds = goal.goal_seconds
+        init_time = rospy.Time.now()
+        feedback = FollowLineFeedback()
+        result = FollowLineResult()
+
+        while not self._goal_reached:
+            if self._as.is_preempt_requested():
+                rospy.loginfo('%s: Preempted' % self._action_name)
+                self._as.set_preempted()
+                success = False
+                break
+            curr_time = rospy.Time.now() - init_time
+            feedback.accumulated_distance = self._accumulated_dist
+            feedback.current_mission_duration = curr_time
+            self._as.publish_feedback(feedback)
+            rospy.loginfo(feedback.accumulated_distance)
+            if self.goal_reach_control(goal_point):
+                self._goal_reached = True
+            self._rate.sleep()
+
+        if success:
+            if curr_time.secs < goal_seconds:
+                result.goal_achieved = True
+            else:
+                result.goal_achieved = False
+            result.total_time = curr_time
+            rospy.loginfo('%s: Succeeded' % self._action_name)
+            self._as.set_succeeded(result)
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('follow_line_action')
+        server = RobotAction(rospy.get_name())
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

ca_tools/scripts/robot_controller.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python
+
+import rospy
+from geometry_msgs.msg import Twist
+from std_msgs.msg import Bool
+
+
+class RobotController:
+    """
+    Class to control the Create Robot, had the const of velocity as a parameters
+    """
+    _FORWARD_VEL = 0.28
+    _ANGULAR_VEL = 0.32
+
+    def __init__(self):
+        """
+        Initialization of the Class, have the publisher and subscribers for /gts and the color sensors, and some
+        extra params like
+        :param stuck_timer : a timer that is started when the robot is not moving forward
+        :param stuck_flag : a flag that is set when the robot is not moving forward
+        """
+        self.twist_publisher = rospy.Publisher('/create1/cmd_vel', Twist, queue_size=10)
+
+        self.left_sensor_subscriber = rospy.Subscriber(
+            "/color_sensor_plugin/left_sensor", Bool, self.callback_left)
+        self.right_sensor_subscriber = rospy.Subscriber(
+            "/color_sensor_plugin/right_sensor", Bool, self.callback_right)
+
+        self.left_sensor_data = False
+        self.right_sensor_data = False
+
+        self.stuck_timer = rospy.Time.now()
+        self.stuck_flag = False
+
+        self.rate = rospy.Rate(50)
+
+    def callback_left(self,data):
+        """
+        A callback to get data from the left sensor
+        """
+        self.left_sensor_data = data.data
+
+    def callback_right(self,data):
+        """
+        A callback to get data from the right sensor
+        """
+        self.right_sensor_data = data.data
+
+    def publish_velocity(self, linear=0, angular=0):
+        """
+        Method used to publsih the velocity message and move the robot
+        """
+        vel = Twist()
+        vel.linear.x = linear
+        vel.angular.z = angular
+        self.twist_publisher.publish(vel)
+
+    def move_forward(self):
+        """
+        Method to move the robot forward
+        """
+        self.publish_velocity(linear=self._FORWARD_VEL)
+
+    def rotate_left(self):
+        """
+        Method to move to the left
+        """
+        self.publish_velocity(angular=self._ANGULAR_VEL)
+
+    def rotate_right(self):
+        """
+        Method to move to the right
+        """
+        self.publish_velocity(angular=-self._ANGULAR_VEL)
+
+    def solve_stuck(self):
+        """
+        Method that gets called when the 5 seconds passed and the robot still didn't move forward,
+        it forces a forward move
+        """
+        self.move_forward()
+        self.stuck_flag = False
+        rospy.sleep(0.1)
+
+    def control (self):
+        """
+        A method that checks the status of the right and left sensors and based on that calls the
+        moving methods
+        """
+        if self.left_sensor_data and self.right_sensor_data:
+            self.move_forward()
+            self.stuck_flag = False
+
+        if not self.left_sensor_data and self.right_sensor_data:
+            self.rotate_left()
+            if not self.stuck_flag:
+                self.stuck_timer = rospy.Time.now()
+                self.stuck_flag = True
+            if rospy.Time.now() - self.stuck_timer > rospy.Duration(5, 0) and self.stuck_flag:
+                self.solve_stuck()
+
+        if self.left_sensor_data and not self.right_sensor_data :
+            self.rotate_right()
+            if not self.stuck_flag:
+                self.stuck_timer = rospy.Time.now()
+                self.stuck_flag = True
+            if rospy.Time.now() - self.stuck_timer > rospy.Duration(5, 0) and self.stuck_flag:
+                self.solve_stuck()
+
+    def execution(self):
+        """
+        A method that loops the control method in order to constantly check
+        """
+        while not rospy.is_shutdown():
+            self.control()
+            self.rate.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('control_robot')
+        robot = RobotController()
+        robot.execution()
+    except rospy.ROSInterruptException:
+        pass