Implemented all the things

Author: asaba96

param/motion_command_coordinator.yaml

@@ -16,8 +16,6 @@ update_rate             : 25
 
 # Startup timeout
 startup_timeout: 15.0
-# If there is no task for this many seconds a zero velocity command will be sent
-task_timeout: 1.0
 
 # Resolution to generate linear motion profiles with
 linear_motion_profile_timestep: 0.020

param/tasks.yaml

@@ -5,8 +5,13 @@
 max_translation_speed: 1.0
 # maximum z velocity
 max_z_velocity: 2.0
-
 max_velocity_time_duration: 30.0
+# planner lag, in seconds
+planning_lag: .100
+# timeout for time to get a plan
+planning_timeout: .5
+# distance from goal to stop replanning
+done_replanning_radius: 1.0
 
 # TAKEOFF TASK
 # Delay between arming and taking off
@@ -36,11 +41,10 @@ translation_xyz_tolerance : 0.3
 control_lag             : 0.3
 
 # GO TO ROOMBA
-go_to_roomba_tolerance: 0.3
+go_to_roomba_height: 1.5
 
 # TRACK ROOMBA
 # height at which roomba tracking takes place
-# also used for GO TO ROOMBA
 track_roomba_height: 1.5
 # maximum radial distance from roomba
 max_roomba_dist: 2.0

src/iarc7_motion/iarc_tasks/block_roomba_task.py

@@ -5,13 +5,10 @@
 import tf2_ros
 import tf2_geometry_msgs
 import threading
-import numpy as np
 
 from geometry_msgs.msg import TwistStamped
-from geometry_msgs.msg import Point
 from geometry_msgs.msg import PointStamped
-from geometry_msgs.msg import Vector3Stamped
-from geometry_msgs.msg import Vector3
+from iarc7_msgs.msg import MotionPointStamped, MotionPointStampedArray
 
 from task_utilities.acceleration_limiter import AccelerationLimiter
 
@@ -25,12 +22,13 @@
                                       NopCommand)
 
 class BlockRoombaTaskState(object):
-    init = 0
-    waiting = 1
-    descent = 2
+    INIT = 0
+    WAITING = 1
+    PLANNING = 2
+    DESCENDING = 3
+    LANDED = 4
 
 class BlockRoombaTask(AbstractTask):
-
     def __init__(self, task_request):
         super(BlockRoombaTask, self).__init__()
 
@@ -39,67 +37,66 @@ def __init__(self, task_request):
         if self._roomba_id == '':
             raise ValueError('An invalid roomba id was provided to BlockRoombaTask')
 
+        self._distance_to_roomba = None
         self._roomba_odometry = None
         self._roomba_point = None
         self._limiter = AccelerationLimiter()
 
         self._canceled = False
-        self._last_update_time = None
         self._current_velocity = None
         self._transition = None
 
+        self._target = (0,0)
+
         self._lock = threading.RLock()
 
         try:
             self._TRANSFORM_TIMEOUT = rospy.get_param('~transform_timeout')
-            self._MIN_MANEUVER_HEIGHT = rospy.get_param('~min_maneuver_height')
             self._MAX_TRANSLATION_SPEED = rospy.get_param('~max_translation_speed')
             self._MAX_START_TASK_DIST = rospy.get_param('~block_roomba_max_start_dist')
             self._MAX_Z_VELOCITY = rospy.get_param('~max_z_velocity')
             self._K_X = rospy.get_param('~k_term_tracking_x')
             self._K_Y = rospy.get_param('~k_term_tracking_y')
-            self._descent_velocity = rospy.get_param('~block_descent_velocity')
+            self._DESCENT_VELOCITY = rospy.get_param('~block_descent_velocity')
+
             _roomba_diameter = rospy.get_param('~roomba_diameter')
-            _drone_width = rospy.get_param('~drone_landing_gear_width') 
+            _drone_width = rospy.get_param('~drone_landing_gear_width')
         except KeyError as e:
             rospy.logerr('Could not lookup a parameter for block roomba task')
             raise
-        self._overshoot = (_roomba_diameter + _drone_width)/2
-        self._state = BlockRoombaTaskState.init
+
+        # the roomba diameter/2 (radius) is acting as a buffer
+        self._overshoot = _roomba_diameter + _drone_width/2
+        self._state = BlockRoombaTaskState.INIT
 
     def get_desired_command(self):
         with self._lock:
             if self._canceled:
                 return (TaskCanceled(),)
 
-            if (self._state == BlockRoombaTaskState.init):
+            if self._state == BlockRoombaTaskState.INIT:
                 if (not self.topic_buffer.has_roomba_message()
                  or not self.topic_buffer.has_odometry_message()
                  or not self.topic_buffer.has_landing_message()):
-                    self._state = BlockRoombaTaskState.waiting
+                    self._state = BlockRoombaTaskState.WAITING
                 else:
-                    self._state = BlockRoombaTaskState.descent
+                    self._state = BlockRoombaTaskState.PLANNING
 
-            if (self._state == BlockRoombaTaskState.waiting):
+            if self._state == BlockRoombaTaskState.WAITING:
                 if (not self.topic_buffer.has_roomba_message()
                  or not self.topic_buffer.has_odometry_message()
                  or not self.topic_buffer.has_landing_message()):
                     return (TaskRunning(), NopCommand())
                 else:
-                    self._state = BlockRoombaTaskState.descent
+                    self._state = BlockRoombaTaskState.PLANNING
 
-            if (self._state == BlockRoombaTaskState.descent):
+            if self._state == BlockRoombaTaskState.PLANNING:
                 try:
                     roomba_transform = self.topic_buffer.get_tf_buffer().lookup_transform(
                                         'level_quad',
                                         self._roomba_id,
                                         rospy.Time(0),
                                         rospy.Duration(self._TRANSFORM_TIMEOUT))
-                    drone_transform = self.topic_buffer.get_tf_buffer().lookup_transform(
-                                        'level_quad',
-                                        'map',
-                                        rospy.Time(0),
-                                        rospy.Duration(self._TRANSFORM_TIMEOUT))
                 except (tf2_ros.LookupException,
                         tf2_ros.ConnectivityException,
                         tf2_ros.ExtrapolationException) as ex:
@@ -126,33 +123,40 @@ def get_desired_command(self):
                 roomba_y_velocity = self._roomba_odometry.twist.twist.linear.y
                 roomba_velocity = math.sqrt(roomba_x_velocity**2 + roomba_y_velocity**2)
 
-                roomba_vector = Vector3Stamped()
+                drone_height = self.topic_buffer.get_odometry_message().pose.pose.position.z
+                time_to_descend = drone_height/self._DESCENT_VELOCITY
+
+                roomba_vector_x = roomba_x_velocity/roomba_velocity
+                roomba_vector_y = roomba_y_velocity/roomba_velocity
 
-                roomba_vector.vector.x = roomba_x_velocity/roomba_velocity
-                roomba_vector.vector.y = roomba_y_velocity/roomba_velocity
-                roomba_vector.vector.z = 0.0
+                goal_x = (self._roomba_point.point.x + self._overshoot * roomba_vector_x)/time_to_descend
+                goal_y = (self._roomba_point.point.y + self._overshoot * roomba_vector_y)/time_to_descend
 
-                # p-controller
-                x_vel_target = ((self._roomba_point.point.x + self._overshoot * roomba_vector.vector.x)
-                                    * self._K_X + roomba_x_velocity)
-                y_vel_target = ((self._roomba_point.point.y + self._overshoot * roomba_vector.vector.y)
-                                    * self._K_Y + roomba_y_velocity)
-                
-                z_vel_target = self._descent_velocity
+                # we need a better way to plan
+                self._target = (goal_x, goal_y)
 
-                #caps velocity
+                self._state = BlockRoombaTaskState.DESCENDING
+
+            if self._state == BlockRoombaTaskState.DESCENDING:
+
+                x_vel_target = self._target[0]
+                y_vel_target = self._target[1]
+
+                z_vel_target = self._DESCENT_VELOCITY
+
+                # cap velocity
                 vel_target = math.sqrt(x_vel_target**2 + y_vel_target**2)
 
                 if vel_target > self._MAX_TRANSLATION_SPEED:
                     x_vel_target = x_vel_target * (self._MAX_TRANSLATION_SPEED/vel_target)
                     y_vel_target = y_vel_target * (self._MAX_TRANSLATION_SPEED/vel_target)
-                
+
                 if (abs(z_vel_target) > self._MAX_Z_VELOCITY):
                     z_vel_target = z_vel_target/abs(z_vel_target) * self._MAX_Z_VELOCITY
                     rospy.logwarn("Max Z velocity reached in block roomba")
 
                 desired_vel = [x_vel_target, y_vel_target, z_vel_target]
-               
+
                 odometry = self.topic_buffer.get_odometry_message()
                 drone_vel_x = odometry.twist.twist.linear.x
                 drone_vel_y = odometry.twist.twist.linear.y
@@ -171,7 +175,7 @@ def get_desired_command(self):
                 velocity.twist.linear.z = desired_vel[2]
 
                 self._current_velocity = desired_vel
-                
+
                 return (TaskRunning(), VelocityCommand(velocity))
 
             return (TaskAborted(msg='Illegal state reached in Block Roomba Task' ),)
@@ -183,14 +187,14 @@ def _check_max_roomba_range(self):
         for odometry in self.topic_buffer.get_roomba_message().data:
             if odometry.child_frame_id == self._roomba_id:
                 self._roomba_odometry = odometry
-                self._roomba_found =  True 
-                _distance_to_roomba = math.sqrt(self._roomba_point.point.x**2 + 
+                self._roomba_found =  True
+                self._distance_to_roomba = math.sqrt(self._roomba_point.point.x**2 +
                             self._roomba_point.point.y**2)
-                return (_distance_to_roomba <= (self._MAX_START_TASK_DIST + self._overshoot))
+                return (self._distance_to_roomba <= (self._MAX_START_TASK_DIST + self._overshoot))
         return False
 
     def cancel(self):
-        rospy.loginfo('HitRoomba Task canceled')
+        rospy.loginfo('BlockRoombaTask canceled')
         self._canceled = True
         return True