diff --git a/ca_gazebo/include/ca_gazebo/color_sensor_plugin.h b/ca_gazebo/include/ca_gazebo/color_sensor_plugin.h
index 1bf0cfaeb..edbabf3d1 100644
--- a/ca_gazebo/include/ca_gazebo/color_sensor_plugin.h
+++ b/ca_gazebo/include/ca_gazebo/color_sensor_plugin.h
@@ -50,4 +50,4 @@
}; // GazeboRosColor
} // gazebo
#endif // GAZEBO_ROS_COLOR_SENSOR_HH
-
\ No newline at end of file
+
diff --git a/ca_gazebo/launch/create_empty_world.launch b/ca_gazebo/launch/create_empty_world.launch
index 79bb84d68..d9c1cc023 100644
--- a/ca_gazebo/launch/create_empty_world.launch
+++ b/ca_gazebo/launch/create_empty_world.launch
@@ -30,7 +30,7 @@
-
+
diff --git a/ca_tools/CMakeLists.txt b/ca_tools/CMakeLists.txt
index 7441e97c8..33dac4b74 100644
--- a/ca_tools/CMakeLists.txt
+++ b/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(
diff --git a/ca_tools/action/FollowLine.action b/ca_tools/action/FollowLine.action
index 4d9195590..8e6f6d8ee 100644
--- a/ca_tools/action/FollowLine.action
+++ b/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
diff --git a/ca_tools/launch/action_launch.launch b/ca_tools/launch/action_launch.launch
new file mode 100644
index 000000000..84d8f5358
--- /dev/null
+++ b/ca_tools/launch/action_launch.launch
@@ -0,0 +1,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/ca_tools/scripts/action_client.py b/ca_tools/scripts/action_client.py
new file mode 100755
index 000000000..2cb88d6d9
--- /dev/null
+++ b/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
diff --git a/ca_tools/scripts/action_server.py b/ca_tools/scripts/action_server.py
new file mode 100755
index 000000000..c52be511a
--- /dev/null
+++ b/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
diff --git a/ca_tools/scripts/robot_controller.py b/ca_tools/scripts/robot_controller.py
new file mode 100755
index 000000000..a32cdddbe
--- /dev/null
+++ b/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