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,12 @@
 # Goal
-int32 amount
-
+int32 goal_seconds
+geometry_msgs/Point goal_position
 ---
 # Result
-string[] final_result
-
+bool goal_achieved
+duration total_time
 ---
 # Feedback
-int32 number_of_minutes
+duration current_time
+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="action_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="action_server"/>
+
+  <!-- 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('action_server', FollowLineAction)
+    _client.wait_for_server()
+    _goal_seconds = rospy.get_param("/get_seconds")
+    _goal_position = Point()
+    _goal_position.x = rospy.get_param("/get_pos_x")
+    _goal_position.y = rospy.get_param("/get_pos_y")
+    goal = FollowLineGoal(goal_seconds=_goal_seconds, goal_position=_goal_position)
+    _client.send_goal(goal)
+    _client.wait_for_result()
+    return _client.get_result()
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('action_client')
+        result = robot_client()
+    except rospy.ROSInterruptException:
+        pass

ca_tools/scripts/action_server.py

@@ -0,0 +1,92 @@
+#! /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):
+    _ERROR_TOLERANCE = 0.1
+
+    def __init__(self, name):
+        self._curr_dist = 0.0
+        self._current_pos = Point()
+        self._last_pos = Point()
+        self._gts_first = True
+        self._feedback = FollowLineFeedback()
+        self._result = FollowLineResult()
+        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._r = 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._curr_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()
+
+        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
+            self._curr_time = rospy.Time.now() - _init_time
+            self._feedback.accumulated_distance = self._curr_dist
+            self._feedback.current_time = self._curr_time
+            self._as.publish_feedback(self._feedback)
+            rospy.loginfo(self._feedback.accumulated_distance)
+            if self.goal_reach_control(_goal_point):
+                self._goal_reached = True
+            self._r.sleep()
+
+        if success:
+            if self._curr_time.secs < _goal_seconds:
+                self._result.goal_achieved = True
+            else:
+                self._result.goal_achieved = False
+            self._result.total_time = self._curr_time
+            rospy.loginfo('%s: Succeeded' % self._action_name)
+            self._as.set_succeeded(self._result)
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('action_server')
+        server = RobotAction(rospy.get_name())
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass

ca_tools/scripts/robot_controller.py

@@ -0,0 +1,128 @@
+#!/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.r = rospy.Rate(50)
+
+    def callback_left(self,data):
+        """
+        A callback to get data form the left sensor
+        """
+        self.left_sensor_data = data.data
+
+    def callback_right(self,data):
+        """
+        A callback to get data form 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
+        :param _linear: gets
+        :param _angular:
+        :return:
+        """
+        _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 move_left(self):
+        """
+        Method to move to the left
+        """
+        self.publish_velocity(_angular=self._ANGULAR_VEL)
+
+    def move_right(self):
+        """
+        Method to move to the right
+        """
+        self.publish_velocity(_angular=-self._ANGULAR_VEL)
+
+    def solve_stuck(self):
+        """
+        Method that's get 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 call 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.move_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.move_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.r.sleep()
+
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('control_robot')
+        robot = RobotController()
+        robot.execution()
+    except rospy.ROSInterruptException:
+        pass