From a45e0a74aa1de917850d164b1fe9a9a4dd711efa Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Mon, 5 Aug 2019 16:19:53 -0400 Subject: [PATCH] Implemented all the things --- param/motion_command_coordinator.yaml | 2 - param/tasks.yaml | 10 +- .../iarc_tasks/block_roomba_task.py | 92 +++++---- .../iarc_tasks/go_to_roomba_task.py | 189 +++++++++++------- src/iarc7_motion/iarc_tasks/task_commands.py | 4 + .../task_utilities/task_topic_buffer.py | 28 ++- .../iarc_tasks/test_planner_task.py | 141 ++++++++++--- .../iarc_tasks/xyztranslation_task.py | 172 +++++++++++----- .../linear_motion_profile_generator.py | 30 ++- .../motion_command_coordinator.py | 22 +- src/iarc7_motion/state_monitor.py | 7 - src/iarc7_motion/task_command_handler.py | 20 +- src/iarc7_motion/transition_data.py | 19 +- 13 files changed, 494 insertions(+), 242 deletions(-) diff --git a/param/motion_command_coordinator.yaml b/param/motion_command_coordinator.yaml index bdfa53d..cd53589 100644 --- a/param/motion_command_coordinator.yaml +++ b/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 diff --git a/param/tasks.yaml b/param/tasks.yaml index 9978bee..762454a 100644 --- a/param/tasks.yaml +++ b/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 diff --git a/src/iarc7_motion/iarc_tasks/block_roomba_task.py b/src/iarc7_motion/iarc_tasks/block_roomba_task.py index fe8424b..d2c0514 100644 --- a/src/iarc7_motion/iarc_tasks/block_roomba_task.py +++ b/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 diff --git a/src/iarc7_motion/iarc_tasks/go_to_roomba_task.py b/src/iarc7_motion/iarc_tasks/go_to_roomba_task.py index 96e4f1a..6784cd7 100644 --- a/src/iarc7_motion/iarc_tasks/go_to_roomba_task.py +++ b/src/iarc7_motion/iarc_tasks/go_to_roomba_task.py @@ -1,6 +1,6 @@ +import math import rospy import tf2_ros -import tf2_geometry_msgs import threading from .abstract_task import AbstractTask @@ -9,17 +9,17 @@ TaskCanceled, TaskAborted, TaskFailed) -from iarc_tasks.task_commands import (VelocityCommand, NopCommand) +from iarc_tasks.task_commands import NopCommand, GlobalPlanCommand -from task_utilities.translate_stop_planner import TranslateStopPlanner +from iarc7_msgs.msg import PlanGoal, PlanAction, MotionPointStamped class GoToRoombaState(object): - init = 0 - translate = 1 - waiting = 2 + INIT = 1 + WAITING = 2 + PLAN_RECEIVED = 3 + COMPLETING = 4 class GoToRoombaTask(AbstractTask): - def __init__(self, task_request): super(GoToRoombaTask, self).__init__() @@ -31,89 +31,144 @@ def __init__(self, task_request): self._roomba_odometry = None - self._canceled = False; - self._transition = None + self._plan = None + self._feedback = None - self._lock = threading.RLock() + self._complete_time = None + self._sent_plan_time = None self._x_position = None self._y_position = None + self._starting_motion_point = None + + self._vel_start = None + + self._canceled = False; + self._transition = None + + self._linear_gen = self.topic_buffer.get_linear_motion_profile_generator() + + self._lock = threading.RLock() + try: - self._TRANSFORM_TIMEOUT = rospy.get_param('~transform_timeout') self._MIN_MANEUVER_HEIGHT = rospy.get_param('~min_maneuver_height') - self._z_position = rospy.get_param('~track_roomba_height') - ending_radius = rospy.get_param('~go_to_roomba_tolerance') + self._PLANNING_TIMEOUT = rospy.Duration(rospy.get_param('~planning_timeout')) + self._PLANNING_LAG = rospy.Duration(rospy.get_param('~planning_lag')) + self._HEIGHT = rospy.get_param('~go_to_roomba_height') + self._DONE_REPLAN_DIST = rospy.get_param('~done_replanning_radius') except KeyError as e: rospy.logerr('Could not lookup a parameter for go_to_roomba task') raise - # Check that we aren't being requested to go below the minimum maneuver height - # Error straight out if that's the case. If we are currently below the minimum height - # It will be caught and handled on the next update - if self._z_position < self._MIN_MANEUVER_HEIGHT : + # make sure height param is not dumb + if self._HEIGHT < self._MIN_MANEUVER_HEIGHT: raise ValueError('Requested z height was below the minimum maneuver height') - self._path_holder = TranslateStopPlanner(self._x_position, - self._y_position, - self._z_position, - ending_radius) - - self._state = GoToRoombaState.init + self._state = GoToRoombaState.INIT def get_desired_command(self): with self._lock: if self._canceled: return (TaskCanceled(),) - if self._state == GoToRoombaState.init: - if (not self.topic_buffer.has_roomba_message() - or not self.topic_buffer.has_odometry_message()): - self._state = GoToRoombaState.waiting + + # cannot do anything until we have a roomba msg + if not self.topic_buffer.has_roomba_message(): + return (TaskRunning(),NopCommand()) + + # waiting on LLM to finish executing the plan + if self._state == GoToRoombaState.COMPLETING: + if (rospy.Time.now() + rospy.Duration(.10)) >= self._complete_time: + return (TaskDone(),) else: - self._state = GoToRoombaState.translate + return (TaskRunning(), NopCommand()) + + if not self._check_roomba_in_sight(): + return (TaskFailed(msg='Cannnot see Roomba in Go To Roomba.')) - if self._state == GoToRoombaState.waiting: - if (not self.topic_buffer.has_roomba_message() - or not self.topic_buffer.has_odometry_message()): - return (TaskRunning(),NopCommand()) + expected_time = rospy.Time.now() + self._PLANNING_LAG + + self._starting_motion_point = self._linear_gen.expected_point_at_time(expected_time).motion_point + + _starting_pose = self._starting_motion_point.pose.position + + roomba_pose = self._roomba_odometry.pose.pose.position + roomba_twist = self._roomba_odometry.twist.twist.linear + + x_traveled = roomba_twist.x * self._PLANNING_LAG.to_sec() + y_traveled = roomba_twist.y * self._PLANNING_LAG.to_sec() + + self._goal_x = roomba_pose.x + x_traveled + self._goal_y = roomba_pose.y + y_traveled + + _distance_to_goal = math.sqrt( + (_starting_pose.x-self._goal_x)**2 + + (_starting_pose.y-self._goal_y)**2) + + if _distance_to_goal < self._DONE_REPLAN_DIST: + if self._plan is not None: + try: + self._complete_time = self._plan.motion_points[-1].header.stamp + except: + rospy.logerr('Planner returned an empty plan while GoToRoomba was in COMPLETING state') + return (TaskAborted(),) + self._state = GoToRoombaState.COMPLETING + return (TaskRunning(), GlobalPlanCommand(self._plan)) else: - self._state = GoToRoombaState.translate - - if self._state == GoToRoombaState.translate: - try: - transStamped = self.topic_buffer.get_tf_buffer().lookup_transform( - 'map', - 'quad', - rospy.Time(0), - rospy.Duration(self._TRANSFORM_TIMEOUT)) - except (tf2_ros.LookupException, - tf2_ros.ConnectivityException, - tf2_ros.ExtrapolationException) as ex: - rospy.logerr('GoToRoombaTask: Exception when looking up transform') - rospy.logerr(ex.message) - return (TaskAborted(msg = 'Exception when looking up transform during go_to_roomba'),) - - if (transStamped.transform.translation.z > self._MIN_MANEUVER_HEIGHT): - if not self._check_roomba_in_sight(): - return (TaskAborted(msg='The provided roomba is not in sight of quad'),) - - roomba_x = self._roomba_odometry.pose.pose.position.x - roomba_y = self._roomba_odometry.pose.pose.position.y - - self._path_holder.reinit_translation_stop_planner(roomba_x, - roomba_y, - self._z_position) - - hold_twist = self._path_holder.get_xyz_hold_response() - - if not self._path_holder.is_done(): - return (TaskRunning(), VelocityCommand(hold_twist)) - else: - return (TaskDone(), VelocityCommand(hold_twist)) + rospy.logerr('GoToRoombaTask: Plan is None but we are done') + return (TaskFailed(msg='Started too close to roomba to do anything'),) + + if self._state == GoToRoombaState.INIT: + self._sent_plan_time = rospy.Time.now() + self.topic_buffer.make_plan_request(self._generate_request(expected_time), + self._feedback_callback) + self._state = GoToRoombaState.WAITING + + if self._state == GoToRoombaState.PLAN_RECEIVED: + if self._feedback is not None: + self.topic_buffer.make_plan_request( + self._generate_request(expected_time, self._feedback.success), + self._feedback_callback) + + self._state = GoToRoombaState.WAITING + if self._feedback.success: + # send LLM the plan we received + return (TaskRunning(), GlobalPlanCommand(self._plan)) else: - return (TaskFailed(msg='Fell below minimum manuever height during translation'),) + rospy.logerr('GoToRoombaTask: In PLAN_RECEIVED state but no feedback') + return (TaskFailed(msg='In PLAN_RECEIVED state but no feedback'),) + + if (self._sent_plan_time is not None and + (rospy.Time.now() - self._sent_plan_time) > self._PLANNING_TIMEOUT): + return (TaskFailed(msg='GoToRoomba: planner took too long to plan'),) - return (TaskAborted(msg='Impossible state in go to roomba task reached')) + return (TaskRunning(), NopCommand()) + + def _generate_request(self, expected_time, reset_timer=True): + request = PlanGoal() + request.header.stamp = expected_time + + start = MotionPointStamped() + start.motion_point = self._starting_motion_point + + goal = MotionPointStamped() + goal.motion_point.pose.position.x = self._goal_x + goal.motion_point.pose.position.y = self._goal_y + goal.motion_point.pose.position.z = self._HEIGHT + + request.start = start + request.goal = goal + + if reset_timer: + self._sent_plan_time = rospy.Time.now() + + return request + + def _feedback_callback(self, status, msg): + with self._lock: + self._feedback = msg + self._plan = msg.plan + self._state = GoToRoombaState.PLAN_RECEIVED def _check_roomba_in_sight(self): for odometry in self.topic_buffer.get_roomba_message().data: diff --git a/src/iarc7_motion/iarc_tasks/task_commands.py b/src/iarc7_motion/iarc_tasks/task_commands.py index 78d4aca..71d7d12 100644 --- a/src/iarc7_motion/iarc_tasks/task_commands.py +++ b/src/iarc7_motion/iarc_tasks/task_commands.py @@ -33,6 +33,10 @@ def __init__(self, else: self.target_twist = target_twist +class GlobalPlanCommand(object): + def __init__(self, plan): + self.plan = plan + class ResetLinearProfileCommand(object): def __init__(self, target_twist=None, diff --git a/src/iarc7_motion/iarc_tasks/task_utilities/task_topic_buffer.py b/src/iarc7_motion/iarc_tasks/task_utilities/task_topic_buffer.py index f6bfcf3..d8dd68a 100644 --- a/src/iarc7_motion/iarc_tasks/task_utilities/task_topic_buffer.py +++ b/src/iarc7_motion/iarc_tasks/task_utilities/task_topic_buffer.py @@ -1,13 +1,15 @@ +import rospy +import tf2_ros +import actionlib +import time + from nav_msgs.msg import Odometry -from iarc7_msgs.msg import BoolStamped -from iarc7_msgs.msg import OdometryArray -from iarc7_motion.linear_motion_profile_generator import LinearMotionProfileGenerator +from iarc7_msgs.msg import BoolStamped, OdometryArray, PlanAction +from iarc7_motion.linear_motion_profile_generator import LinearMotionProfileGenerator from iarc_tasks.task_utilities.obstacle_avoid_helper import ObstacleAvoider - -import rospy -import tf2_ros +from iarc7_safety.iarc_safety_exception import IARCFatalSafetyException class TaskTopicBuffer(object): def __init__(self): @@ -17,7 +19,7 @@ def __init__(self): except KeyError as e: rospy.logerr('Could not lookup a parameter for task topic buffer') raise - + self._landed_message = None self._roomba_array = None self._drone_odometry = None @@ -41,6 +43,12 @@ def __init__(self): self._obstacle_avoider = ObstacleAvoider(self._tf_buffer) self._obstacle_avoider.wait_until_ready(self._startup_timeout) + # client to planner Action Server + self._planner_client = actionlib.SimpleActionClient('planner_request', PlanAction) + + if not self._planner_client.wait_for_server(self._startup_timeout): + raise IARCFatalSafetyException( + 'TaskTopicBuffer could not initialize planner action client') self._task_message_dictionary = {} @@ -62,6 +70,12 @@ def wait_until_ready(self, timeout): rospy.logerr('Have roomba: %s', self.has_roomba_message()) raise IARCFatalSafetyException('TaskTopicBuffer not ready') + def make_plan_request(self, request, feedback_callback): + self._planner_client.send_goal(request, done_cb=feedback_callback) + + def cancel_plan_goal(self): + self._planner_client.cancel_goal() + def _receive_roomba_status(self, data): self._roomba_array = data diff --git a/src/iarc7_motion/iarc_tasks/test_planner_task.py b/src/iarc7_motion/iarc_tasks/test_planner_task.py index db701cb..27bb2f3 100644 --- a/src/iarc7_motion/iarc_tasks/test_planner_task.py +++ b/src/iarc7_motion/iarc_tasks/test_planner_task.py @@ -1,47 +1,134 @@ -#!/usr/bin/env python +import math import rospy -import actionlib +import tf2_ros +import threading +import time from .abstract_task import AbstractTask - from iarc_tasks.task_states import (TaskRunning, TaskDone, TaskCanceled, - TaskAborted) -from iarc_tasks.task_commands import NopCommand + TaskAborted, + TaskFailed) +from iarc_tasks.task_commands import NopCommand, GlobalPlanCommand from iarc7_msgs.msg import PlanGoal, PlanAction, MotionPointStamped -class TestPlannerTask(AbstractTask): +class TestPlannerTask(AbstractTask): def __init__(self, task_request): super(TestPlannerTask, self).__init__() - # Creates the SimpleActionClient for requesting ground interaction - self._client = actionlib.SimpleActionClient('planner_request', PlanAction) - self._client.wait_for_server() - self._plan_canceled = False + self._canceled = False + + self._transition = None + self._plan = None + self._feedback = None + self._feedback_receieved = False + self._starting = True + + self._corrected_start_x = None + self._corrected_start_y = None + self._corrected_start_z = None + + self._vel_start = None + + self._lock = threading.RLock() + + self._linear_gen = self.topic_buffer.get_linear_motion_profile_generator() + + try: + self._PLANNING_LAG = rospy.Duration(rospy.get_param('~planning_lag')) + self._COORDINATE_FRAME_OFFSET = rospy.get_param('~planner_coordinate_frame_offset') + self._MIN_MANEUVER_HEIGHT = rospy.get_param('~min_maneuver_height') + except KeyError as e: + rospy.logerr('Could not lookup a parameter for TestPlannerTask task') + raise + + # Check that we aren't being requested to go below the minimum maneuver height + # Error straight out if that's the case. + if task_request.z_position < self._MIN_MANEUVER_HEIGHT : + raise ValueError('Requested z height was below the minimum maneuver height') + + self._corrected_goal_x = 5 + self._COORDINATE_FRAME_OFFSET + self._corrected_goal_y = 5 + self._COORDINATE_FRAME_OFFSET + self._corrected_goal_z = 1 def get_desired_command(self): - goal = PlanGoal() - goal.goal.motion_point.pose.position.x = 5 - goal.goal.motion_point.pose.position.y = 5 - self._client.send_goal(goal, None, None, self._feedback_callback) - if self._plan_canceled: - return (TaskDone(), NopCommand()) - else: - return (TaskRunning(),) - - def _feedback_callback(self, msg): - self._feedback = msg - rospy.logwarn('Feedback recieved') - self._client.cancel_goal() - self._plan_canceled = True + with self._lock: + rospy.logerr('TASK STARTS AT: ' + str(time.time())) + + if self._canceled: + return (TaskCanceled(),) + + expected_time = rospy.Time.now() + self._PLANNING_LAG + starting = self._linear_gen.expected_point_at_time(expected_time) + + _pose = starting.motion_point.pose.position + self._vel_start = starting.motion_point.twist.linear + + self._corrected_start_x = _pose.x + self._COORDINATE_FRAME_OFFSET + self._corrected_start_y = _pose.y + self._COORDINATE_FRAME_OFFSET + self._corrected_start_z = _pose.z + + done = (abs(self._corrected_start_x-self._corrected_goal_x ) < .05 and + abs(self._corrected_start_y-self._corrected_goal_y) < .05 and + abs(self._corrected_start_z-self._corrected_goal_z) < .05) + + if done: + if self._plan is not None: + return (TaskDone(), GlobalPlanCommand(self._plan)) + else: + return (TaskDone(),) + + if self._feedback_receieved or self._starting: + self._starting = False + self._feedback_receieved = False + self.topic_buffer.make_plan_request(self._generate_request(expected_time), + self._feedback_callback) + + if self._feedback is not None and self._feedback.success: + # send LLM the plan we recieved + return (TaskRunning(), GlobalPlanCommand(self._plan)) + else: + # planning failed, nop + return (TaskRunning(), NopCommand()) + + return (TaskRunning(), NopCommand()) + + def _generate_request(self, expected_time): + request = PlanGoal() + request.header.stamp = expected_time + + start = MotionPointStamped() + start.motion_point.pose.position.x = self._corrected_start_x + start.motion_point.pose.position.y = self._corrected_start_y + start.motion_point.pose.position.z = self._corrected_start_z + + start.motion_point.twist.linear.x = self._vel_start.x + start.motion_point.twist.linear.y = self._vel_start.y + start.motion_point.twist.linear.z = self._vel_start.z + + goal = MotionPointStamped() + goal.motion_point.pose.position.x = self._corrected_goal_x + goal.motion_point.pose.position.y = self._corrected_goal_y + goal.motion_point.pose.position.z = self._corrected_goal_z + + request.start = start + request.goal = goal + return request + + def _feedback_callback(self, status, msg): + with self._lock: + rospy.logerr('FEEDBACK AT: ' + str(time.time())) + self._feedback = msg + self._plan = msg.plan + self._feedback_receieved = True def cancel(self): - rospy.loginfo("TestPlannerTask canceling") - self.canceled = True + rospy.loginfo('TestPlannerTask canceled') + self._canceled = True return True - + def set_incoming_transition(self, transition): self._transition = transition diff --git a/src/iarc7_motion/iarc_tasks/xyztranslation_task.py b/src/iarc7_motion/iarc_tasks/xyztranslation_task.py index ae630cd..0618376 100644 --- a/src/iarc7_motion/iarc_tasks/xyztranslation_task.py +++ b/src/iarc7_motion/iarc_tasks/xyztranslation_task.py @@ -1,8 +1,8 @@ import math import rospy import tf2_ros - -from geometry_msgs.msg import TwistStamped +import threading +import cPickle as pickle from .abstract_task import AbstractTask from iarc_tasks.task_states import (TaskRunning, @@ -10,83 +10,155 @@ TaskCanceled, TaskAborted, TaskFailed) -from iarc_tasks.task_commands import (VelocityCommand, NopCommand) +from iarc_tasks.task_commands import NopCommand, GlobalPlanCommand -from task_utilities.translate_stop_planner import TranslateStopPlanner +from iarc7_msgs.msg import PlanGoal, PlanAction, MotionPointStamped class XYZTranslationTaskState(object): - init = 0 - translate = 1 + INIT = 1 + WAITING = 2 + PLAN_RECEIVED = 3 + COMPLETING = 4 class XYZTranslationTask(AbstractTask): - def __init__(self, task_request): super(XYZTranslationTask, self).__init__() - self._canceled = False; + self._canceled = False self._transition = None - self._x_position = task_request.x_position - self._y_position = task_request.y_position - self._z_position = task_request.z_position + self._plan = None + self._feedback = None + + self._complete_time = None + self._sent_plan_time = None + + self._starting_motion_point = None + + # self._list_info = [] + # self._request_list = [] + + self._lock = threading.RLock() + + self._linear_gen = self.topic_buffer.get_linear_motion_profile_generator() try: - self._TRANSLATION_XYZ_TOLERANCE = rospy.get_param('~translation_xyz_tolerance') - self._TRANSFORM_TIMEOUT = rospy.get_param('~transform_timeout') + self._PLANNING_TIMEOUT = rospy.Duration(rospy.get_param('~planning_timeout')) + self._PLANNING_LAG = rospy.Duration(rospy.get_param('~planning_lag')) + self._DONE_REPLAN_DIST = rospy.get_param('~done_replanning_radius') self._MIN_MANEUVER_HEIGHT = rospy.get_param('~min_maneuver_height') except KeyError as e: rospy.logerr('Could not lookup a parameter for xyztranslation task') raise # Check that we aren't being requested to go below the minimum maneuver height - # Error straight out if that's the case. If we are currently below the minimum height - # It will be caught and handled on the next update - if self._z_position < self._MIN_MANEUVER_HEIGHT : + # Error straight out if that's the case. + if task_request.z_position < self._MIN_MANEUVER_HEIGHT: raise ValueError('Requested z height was below the minimum maneuver height') - # fc status? - self._path_holder = TranslateStopPlanner(self._x_position, - self._y_position, - self._z_position) - self._state = XYZTranslationTaskState.init + self._goal_x = task_request.x_position + self._goal_y = task_request.y_position + self._goal_z = task_request.z_position + + self._state = XYZTranslationTaskState.INIT def get_desired_command(self): - if self._canceled: - return (TaskCanceled(),) - - if self._state == XYZTranslationTaskState.init: - self._state = XYZTranslationTaskState.translate - - if self._state == XYZTranslationTaskState.translate: - try: - transStamped = self.topic_buffer.get_tf_buffer().lookup_transform( - 'map', - 'quad', - rospy.Time(0), - rospy.Duration(self._TRANSFORM_TIMEOUT)) - except (tf2_ros.LookupException, - tf2_ros.ConnectivityException, - tf2_ros.ExtrapolationException) as ex: - rospy.logerr('XYZTranslation Task: Exception when looking up transform') - rospy.logerr(ex.message) - return (TaskAborted(msg = 'Exception when looking up transform during xyztranslation'),) - - if(transStamped.transform.translation.z > self._MIN_MANEUVER_HEIGHT): - hold_twist = self._path_holder.get_xyz_hold_response() - if not self._path_holder.is_done(): - return (TaskRunning(), VelocityCommand(hold_twist)) + with self._lock: + if self._canceled: + return (TaskCanceled(),) + + if self._state == XYZTranslationTaskState.COMPLETING: + if (rospy.Time.now() + rospy.Duration(.10)) >= self._complete_time: + # name = '/home/andrew/data.bin' + # file = open(name, 'wb') + # pickle.dump(self._list_info, file) + # file.close() + # self._linear_gen.dump_info() + return (TaskDone(),) + else: + return (TaskRunning(), NopCommand()) + + expected_time = rospy.Time.now() + self._PLANNING_LAG + self._starting_motion_point = self._linear_gen.expected_point_at_time(expected_time).motion_point + + _starting_pose = self._starting_motion_point.pose.position + _distance_to_goal = math.sqrt( + (_starting_pose.x-self._goal_x)**2 + + (_starting_pose.y-self._goal_y)**2) + + if _distance_to_goal < self._DONE_REPLAN_DIST: + if self._plan is not None: + try: + self._complete_time = self._plan.motion_points[-1].header.stamp + except: + rospy.logerr('Planner returned an empty plan while XYZ Translate was in COMPLETING state') + return (TaskAborted(),) + self._state = XYZTranslationTaskState.COMPLETING + return (TaskRunning(), GlobalPlanCommand(self._plan)) + else: + rospy.logerr('XYZTranslationTask: Plan is None but we are done') + return (TaskFailed(msg='Started too close to goal to do anything'),) + + if self._state == XYZTranslationTaskState.INIT: + self.topic_buffer.make_plan_request(self._generate_request(expected_time), + self._feedback_callback) + self._state = XYZTranslationTaskState.WAITING + + if self._state == XYZTranslationTaskState.PLAN_RECEIVED: + if self._feedback is not None: + self.topic_buffer.make_plan_request( + self._generate_request(expected_time, self._feedback.success), + self._feedback_callback) + + self._state = XYZTranslationTaskState.WAITING + if self._feedback.success: + # send LLM the plan we received + return (TaskRunning(), GlobalPlanCommand(self._plan)) else: - return (TaskDone(), VelocityCommand(hold_twist)) - else: - return (TaskFailed(msg='Fell below minimum manuever height during translation'),) + rospy.logerr('XYZTranslationTask: In PLAN_RECEIVED state but no feedback') + return (TaskFailed(msg='In PLAN_RECEIVED state but no feedback'),) - return (TaskAborted(msg='Impossible state in takeoff task reached')) + if (self._sent_plan_time is not None and + (rospy.Time.now() - self._sent_plan_time) > self._PLANNING_TIMEOUT): + return (TaskFailed(msg='XYZ Translate: planner took too long to plan'),) + + return (TaskRunning(), NopCommand()) + + def _generate_request(self, expected_time, reset_timer=True): + request = PlanGoal() + request.header.stamp = expected_time + + start = MotionPointStamped() + start.motion_point = self._starting_motion_point + + goal = MotionPointStamped() + goal.motion_point.pose.position.x = self._goal_x + goal.motion_point.pose.position.y = self._goal_y + goal.motion_point.pose.position.z = self._goal_z + + # self._request_list = [start, goal, None] + + request.start = start + request.goal = goal + + if reset_timer: + self._sent_plan_time = rospy.Time.now() + + return request + + def _feedback_callback(self, status, msg): + with self._lock: + self._feedback = msg + self._plan = msg.plan + # self._request_list[2] = msg.plan + # self._list_info.append(self._request_list) + self._state = XYZTranslationTaskState.PLAN_RECEIVED def cancel(self): rospy.loginfo('XYZTranslationTask canceled') self._canceled = True return True - + def set_incoming_transition(self, transition): self._transition = transition diff --git a/src/iarc7_motion/linear_motion_profile_generator.py b/src/iarc7_motion/linear_motion_profile_generator.py index 696d2fe..3605a48 100644 --- a/src/iarc7_motion/linear_motion_profile_generator.py +++ b/src/iarc7_motion/linear_motion_profile_generator.py @@ -8,7 +8,7 @@ from nav_msgs.msg import Path from iarc7_msgs.msg import MotionPointStamped, MotionPointStampedArray - +import cPickle as pickle # Convert a 3 element numpy array to a Vector3 message def np_to_msg(array, msg): @@ -68,7 +68,6 @@ def interpolate_motion_points(first, second, time): return result - # Generates fully defined motion profiles class LinearMotionProfileGenerator(object): def __init__(self, start_motion_point): @@ -95,6 +94,8 @@ def __init__(self, start_motion_point): self._override_start_position = None self._override_start_velocity = None + # self._info = [] + linear_motion_profile_generator = None @staticmethod @@ -104,6 +105,31 @@ def get_linear_motion_profile_generator(): MotionPointStamped()) return LinearMotionProfileGenerator.linear_motion_profile_generator + # def dump_info(self): + # name = '/home/andrew/new_data.bin' + # file = open(name, 'wb') + # pickle.dump(self._info, file) + # file.close() + + def set_global_plan(self, plan): + if len(self._last_motion_plan.motion_points) == 0: + # self._info.append((self._last_motion_plan, plan)) + self._last_motion_plan = plan + else: + new_plan = MotionPointStampedArray() + new_point_stamp = plan.motion_points[0].header.stamp + + for i in range(0, len(self._last_motion_plan.motion_points)): + current_stamp = self._last_motion_plan.motion_points[i].header.stamp + if current_stamp > new_point_stamp: + new_plan.motion_points = self._last_motion_plan.motion_points[:i] + break + + new_plan.motion_points.extend(plan.motion_points) + # self._info.append((self._last_motion_plan, plan)) + + self._last_motion_plan = new_plan + # Get a starting motion point for a given time def _get_start_point(self, time, diff --git a/src/iarc7_motion/motion_command_coordinator.py b/src/iarc7_motion/motion_command_coordinator.py index 54a564e..964fa2c 100755 --- a/src/iarc7_motion/motion_command_coordinator.py +++ b/src/iarc7_motion/motion_command_coordinator.py @@ -42,11 +42,7 @@ def __init__(self, action_server): # current state of motion coordinator self._task = None - self._first_task_seen = False - - # used for timeouts & extrapolating - self._time_of_last_task = None - self._timeout_vel_sent = False + self._task_just_ended = False # to keep things thread safe self._lock = threading.RLock() @@ -75,8 +71,6 @@ def __init__(self, action_server): try: # update rate for motion coordinator self._update_rate = rospy.get_param('~update_rate') - # task timeout values - self._task_timeout = rospy.Duration(rospy.get_param('~task_timeout')) # startup timeout self._startup_timeout = rospy.Duration(rospy.get_param('~startup_timeout')) except KeyError as e: @@ -127,11 +121,6 @@ def run(self): self._safety_land_requested = True self._state_monitor.signal_safety_active() - # set the time of last task to now if we have not seen a task yet - if not self._first_task_seen: - self._time_of_last_task = rospy.Time.now() - self._first_task_seen = True - closest_obstacle_dist = self._idle_obstacle_avoider.get_distance_to_obstacle() if self._task is None and self._action_server.has_new_task(): @@ -190,8 +179,7 @@ def run(self): # as soon as we set a task to None, start time # and send ending state to State Monitor if self._task is None: - self._time_of_last_task = rospy.Time.now() - self._timeout_vel_sent = False + self._task_just_ended = True self._state_monitor.set_last_task_end_state(task_state) # No task is running, run obstacle avoider else: @@ -205,14 +193,16 @@ def run(self): self._task_command_handler.send_timeout(avoid_twist, acceleration=acceleration) rospy.logwarn_throttle(1.0, 'Task running timeout. Running obstacle avoider') - rate.sleep() + if not self._task_just_ended: + rate.sleep() + + self._task_just_ended = False # fills out the Intermediary State for the task def _get_current_transition(self): state = TransitionData() state.last_twist = self._task_command_handler.get_last_twist() state.last_task_ending_state = self._task_command_handler.get_state() - state.timeout_sent = self._timeout_vel_sent return self._state_monitor.fill_out_transition(state) # callback for safety task completition diff --git a/src/iarc7_motion/state_monitor.py b/src/iarc7_motion/state_monitor.py index 1d4f077..3e1f7e2 100755 --- a/src/iarc7_motion/state_monitor.py +++ b/src/iarc7_motion/state_monitor.py @@ -190,13 +190,6 @@ def fill_out_transition(self, state): state.arm_status = self._arm_status return state - # Handles no task running timeouts - def get_timeout_twist(self): - twist = TwistStamped() - twist.header.stamp = rospy.Time.now() - return twist - - def signal_safety_active(self): with self._lock: self._state = RobotStates.SAFETY_ACTIVE diff --git a/src/iarc7_motion/task_command_handler.py b/src/iarc7_motion/task_command_handler.py index 7191f04..5a8b6cb 100644 --- a/src/iarc7_motion/task_command_handler.py +++ b/src/iarc7_motion/task_command_handler.py @@ -4,6 +4,7 @@ import traceback import actionlib import rospy +import time import iarc_tasks.task_states as task_states import iarc_tasks.task_commands as task_commands @@ -62,7 +63,8 @@ def __init__(self): task_commands.GroundInteractionCommand: self._handle_ground_interaction_command, task_commands.ConfigurePassthroughMode: self._handle_passthrough_mode_command, task_commands.AngleThrottleCommand: self._handle_passthrough_command, - task_commands.ResetLinearProfileCommand: self._handle_reset_linear_profile_command + task_commands.ResetLinearProfileCommand: self._handle_reset_linear_profile_command, + task_commands.GlobalPlanCommand: self._handle_global_plan_command } self._motion_profile_generator = LinearMotionProfileGenerator.get_linear_motion_profile_generator() @@ -114,7 +116,6 @@ def cancel_task(self): try: ready = self._task.cancel() if ready: - self._task = None self._task_state = task_states.TaskCanceled() return True else: @@ -183,6 +184,11 @@ def _get_task_command(self): return (task_commands.NopCommand(),) if issubclass(type(_task_state), task_states.TaskState): + if isinstance(self._task_state, task_states.TaskCanceled): + if not isinstance(_task_state, task_states.TaskCanceled): + rospy.logerr('TaskCommandHandler: Task was supposed to return TaskCanceled') + self._task = None + return (task_commands.NopCommand(),) self._task_state = _task_state else: rospy.logerr('Task provided unknown state') @@ -197,6 +203,9 @@ def _get_task_command(self): return task_request[1:] elif isinstance(self._task_state, task_states.TaskRunning): return task_request[1:] + elif isinstance(self._task_state, task_states.TaskCanceled): + self._task = None + return task_request[1:] else: self._task = None except (TypeError, IndexError) as e: @@ -207,8 +216,6 @@ def _get_task_command(self): self._task = None self._task_state = task_states.TaskAborted(msg='Exception getting task request') return (task_commands.NopCommand(),) - elif isinstance(self._task_state, task_states.TaskCanceled): - pass else: raise IARCFatalSafetyException('TaskCommandHandler ran with no task running.') @@ -306,6 +313,11 @@ def handle_ground_interaction_done(self, status, result): else: rospy.logerr('Ground interaction done callback received with no task callback available') + def _handle_global_plan_command(self, plan_command): + self._motion_profile_generator.set_global_plan(plan_command.plan) + self._last_twist = plan_command.plan.motion_points[-1].motion_point.twist + self._motion_point_pub.publish(plan_command.plan) + """ Sends motion point stamped array to LLM diff --git a/src/iarc7_motion/transition_data.py b/src/iarc7_motion/transition_data.py index 587dbb0..1abd1f5 100644 --- a/src/iarc7_motion/transition_data.py +++ b/src/iarc7_motion/transition_data.py @@ -2,9 +2,12 @@ class TransitionData(object): - def __init__(self, drone_odometry=None, roombas=None, obstacles=None, - timeout_sent=None, last_task_ending_state=None, - arm_status=None, last_twist=None): + def __init__(self, drone_odometry=None, + roombas=None, + obstacles=None, + last_task_ending_state=None, + arm_status=None, + last_twist=None): """ Transition Data @@ -14,7 +17,6 @@ def __init__(self, drone_odometry=None, roombas=None, obstacles=None, all roombas in sight of drone obstacles: odometry (position, velocity, etc.) of all obstacles in sight of drone - timeout_sent: whether or not timeout was sent last_task_ending_state: last task ending state arm_status: current arm status of drone last_twist: last twist (velocity request) sent to LLM @@ -22,7 +24,6 @@ def __init__(self, drone_odometry=None, roombas=None, obstacles=None, self.drone_odometry = drone_odometry self.roombas = roombas self.obstacles = obstacles - self.timeout_sent = timeout_sent self.last_task_ending_state = last_task_ending_state self.arm_status = arm_status self.last_twist = last_twist @@ -51,14 +52,6 @@ def obstacles(self): def obstacles(self, obstacles): self._obstacles = obstacles - @property - def timeout_sent(self): - return self._timeout_sent - - @timeout_sent.setter - def timeout_sent(self, timeout_sent): - self._timeout_sent = timeout_sent - @property def last_task_ending_state(self): return self._last_task_ending_state