diff --git a/param/arena_settings_10m.yaml b/param/arena_settings_10m.yaml index 05ecf9a..209da2f 100644 --- a/param/arena_settings_10m.yaml +++ b/param/arena_settings_10m.yaml @@ -1,5 +1,5 @@ arena_starting_position_x: 0.0 arena_starting_position_y: 0.0 -arena_size_x: 15.0 -arena_size_y: 15.0 +arena_size_x: 10.0 +arena_size_y: 10.0 diff --git a/param/arena_settings_20m.yaml b/param/arena_settings_20m.yaml new file mode 100644 index 0000000..28c77ea --- /dev/null +++ b/param/arena_settings_20m.yaml @@ -0,0 +1,5 @@ +arena_starting_position_x: -9.5 +arena_starting_position_y: 9.5 + +arena_size_x: 20.0 +arena_size_y: 20.0 diff --git a/scripts/mission7.py b/scripts/mission7.py new file mode 100755 index 0000000..55c3de8 --- /dev/null +++ b/scripts/mission7.py @@ -0,0 +1,371 @@ +#! /usr/bin/env python +import sys +import rospy +import math +import numpy as np + +import actionlib +from iarc7_msgs.msg import BoolStamped +from iarc7_motion.msg import QuadMoveGoal, QuadMoveAction +from iarc7_safety.SafetyClient import SafetyClient +from iarc7_msgs.msg import OdometryArray +from nav_msgs.msg import Odometry +from actionlib_msgs.msg import GoalStatus + +from iarc7_abstract.arena_position_estimator import ArenaPositionEstimator + +from tf.transformations import euler_from_quaternion +from control_button_interpreter import ControlButtonInterpreter + +TRANSLATION_HEIGHT = 1.1 +MIN_GOTO_DISTANCE = 0.5 +USE_PLANNER = False + +TARGET_NUM_ROOMBAS = 4 +MAX_FLIGHT_DURATION = 6.0 * 60 + +# Used if the planner is disabled +TRANSLATION_VELOCITY = 0.6 +PAUSE_TIME = 1.2 + +SEARCH_POINTS = np.asarray( +( + (-5.0, 6.0), # In a little + (-100.0, -100.0), # Pause + (-5.0, -1.0), # Right side + (-100.0, -100.0), # Pause + (-5.0, 5.0) # Left side +)) + +# SEARCH_POINTS = np.asarray( +# ( +# (-7.0, 7.0), # In a little +# (-7.0, -2.0), # Right side +# (-7.0, 7.0) # Left side +# )) + +def target_roomba_law(roombas, odom): + # Sort roombas by their distance to the drone + sorted_roombas = sorted([(roomba_distance(r, odom), r) for r in roombas]) + return sorted_roombas[0][1] + +def construct_velocity_goal(arena_pos, odom, height=TRANSLATION_HEIGHT): + map_pos = arena_position_estimator.arena_to_map(arena_pos) + #supposed_to_be_at = arena_position_estimator.arena_to_map(supposed_to_be_at) + if arena_pos[0] == -100.0 and arena_pos[1] == -100.0: + v_x = 0.0 + v_y = 0.0 + length_of_time = PAUSE_TIME + else: + supposed_to_be_at = (odom.pose.pose.position.x, odom.pose.pose.position.y) + diff_x = map_pos[0] - supposed_to_be_at[0] + diff_y = map_pos[1] - supposed_to_be_at[1] + print arena_pos + print map_pos + print diff_x, diff_y + hypot = math.sqrt(diff_x**2 + diff_y**2) + u_x = diff_x / hypot + u_y = diff_y / hypot + v_x = u_x * TRANSLATION_VELOCITY + v_y = u_y * TRANSLATION_VELOCITY + length_of_time = hypot / TRANSLATION_VELOCITY + + return QuadMoveGoal(movement_type="velocity_test", + x_velocity=v_x, + y_velocity=v_y, + z_position=height, + velocity_duration=length_of_time) + + +def construct_xyz_goal(arena_pos, height=TRANSLATION_HEIGHT): + map_pos = arena_position_estimator.arena_to_map(arena_pos) + return QuadMoveGoal(movement_type="xyztranslate", x_position=map_pos[0], y_position=map_pos[1], z_position=height) + +def roomba_distance(roomba_odom, drone_odom): + x_diff = drone_odom.pose.pose.position.x - roomba_odom.pose.pose.position.x + y_diff = drone_odom.pose.pose.position.y - roomba_odom.pose.pose.position.y + return math.sqrt(x_diff**2 + y_diff**2) + +def find_roomba_by_id(roombas, desired_id): + roomba_list = [x for x in roombas if x.child_frame_id == desired_id] + if len(roomba_list) == 0: + return None + return roomba_list[0] + +def roomba_yaw(roomba): + orientation_list = [roomba.pose.pose.orientation.x, + roomba.pose.pose.orientation.y, + roomba.pose.pose.orientation.z, + roomba.pose.pose.orientation.w] + + angles = euler_from_quaternion(orientation_list) + return angles[2] + +def construct_goto_roomba_goal(roomba): + return QuadMoveGoal(movement_type="xyztranslate", + x_position = roomba.pose.pose.position.x, + y_position = roomba.pose.pose.position.y, + z_position = TRANSLATION_HEIGHT) + +def did_task_finish(client): + return did_task_fail(client) or did_task_succeed(client) + +def did_task_fail(client, state=None): + if state is None: + state = client.get_state() + return (state == GoalStatus.ABORTED + or state == GoalStatus.REJECTED + or state == GoalStatus.PREEMPTED + or state == GoalStatus.RECALLED) + +def did_task_succeed(client, state=None): + if state is None: + state = client.get_state() + return (state == GoalStatus.SUCCEEDED) + +def guide_roomba_law(roomba): + roomba_heading = roomba_yaw(roomba) + rospy.loginfo('Roomba heading: {} degrees'.format(roomba_heading * 180.0/math.pi)) + if -5 * math.pi / 4 > roomba_heading and roomba_heading <= 1 * math.pi /4: + # Block + return 1 + elif 1 * math.pi / 4 > roomba_heading and roomba_heading <= 3 * math.pi / 4: + # Hit + return 2 + else: + # Do nothing + return 0 + +class Mission7(object): + def __init__(self): + self.safety_client = SafetyClient('mission7') + # Since this abstract is top level in the control chain there is no need to check + # for a safety state. We can also get away with not checking for a fatal state since + # all nodes below will shut down. + assert(self.safety_client.form_bond()) + if rospy.is_shutdown(): return + + # Creates the SimpleActionClient, passing the type of the action + # (QuadMoveAction) to the constructor. (Look in the action folder) + self._client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) + + # Waits until the action server has started up and started + # listening for goals. + self._client.wait_for_server() + if rospy.is_shutdown(): return + + self._avail_roomba = None + self._search_state = 0 + self._roomba_sub = rospy.Subscriber('/roombas', OdometryArray, self.roomba_callback) + self._odom_sub = rospy.Subscriber('/odometry/filtered/', Odometry, self.odom_callback) + self._supposed_to_be_at = (rospy.get_param('~arena_starting_position_x'), + rospy.get_param('~arena_starting_position_y')) + + def roomba_callback(self, data): + self._avail_roombas = data.data + + def odom_callback(self, data): + self._odom = data + + def begin_translate(self, arena_pos, height=TRANSLATION_HEIGHT): + if USE_PLANNER: + map_pos = construct_xyz_goal(arena_pos, height=height) + self._client.send_goal(map_pos) + else: + self._client.send_goal(construct_velocity_goal(arena_pos, self._odom, height=height)) + self._supposed_to_be_at = arena_pos + + def basic_goal(self, goal): + self._client.send_goal(QuadMoveGoal(movement_type=goal)) + self._client.wait_for_result() + rospy.loginfo('{} success: {}'.format(goal, self._client.get_result())) + + def wait_for_roomba(self): + rate = rospy.Rate(30) + while True: + if did_task_fail(self._client): + return (False, None) + elif did_task_succeed(self._client): + return (True, None) + if self._avail_roombas is not None and len(self._avail_roombas) > 0: + target_roomba = target_roomba_law(self._avail_roombas, self._odom) + if target_roomba is not None: + return (True, target_roomba) + rate.sleep() + + def search_for_roomba(self): + rospy.loginfo('Entering search') + # Decide which waypoint to go to + if self._search_state != 0: + self._search_state = 1 + + # Execute search waypoints + while True: + self.begin_translate(SEARCH_POINTS[self._search_state]) + (xyz_translate_ok, roomba) = self.wait_for_roomba() + if not xyz_translate_ok: + continue + + if roomba is None: + self._search_state = self._search_state + 1 if self._search_state + 1 < SEARCH_POINTS.shape[0] else 1 + elif self._search_state > 0: + break + rospy.loginfo('Flight time {}'.format((rospy.Time.now() - self.flight_start_time).to_sec())) + if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): + return None + + # Todo better initial search position + self._search_state = 1 + return roomba + + def track_roomba_to_completion(self, roomba): + track_state = 0 + roomba_id = roomba.child_frame_id + rospy.loginfo('Entering track roomba: {}'.format(roomba_id)) + + rate = rospy.Rate(30) + while True: + roomba = find_roomba_by_id(self._avail_roombas, roomba_id) + if roomba is None: + rospy.loginfo('Roomba no longer is tracked {}'.format(roomba_id)) + return False + + d = roomba_distance(roomba, self._odom) + + # Need to decide what kind of positional action to take + if track_state == 0: + + # Regardless of the positional action necessary, make sure the last task is canceled + if not did_task_finish(self._client): + rospy.loginfo('Canceling goal') + self._client.cancel_goal() + + if d >= MIN_GOTO_DISTANCE: + #self._client.send_goal(construct_goto_roomba_goal(roomba)) + #rospy.loginfo('GOTO ROOMBA') + #track_state = 1 + pass + else: + self._client.send_goal(QuadMoveGoal(movement_type="track_roomba", + frame_id=roomba_id, + x_overshoot=0.0, y_overshoot=0.0)) + rospy.loginfo('TRACKING ROOMBA') + track_state = 2 + + # Going to roomba + if track_state == 1: + state = self._client.get_state() + if did_task_succeed(self._client): + rospy.loginfo('GOTO ROOMBA SUCCESS') + track_state = 0 + if did_task_fail(self._client): + rospy.loginfo('GOTO ROOMBA FAILED') + track_state = 0 + + if d < MIN_GOTO_DISTANCE: + self._client.cancel_goal() + self._client.send_goal(QuadMoveGoal(movement_type="track_roomba", + frame_id=roomba_id, + x_overshoot=0.0, y_overshoot=0.0, time_to_track=100000.0)) + rospy.loginfo('TRACKING ROOMBA') + track_state = 2 + + if track_state == 2: + state = self._client.get_state() + if did_task_finish(self._client): + rospy.loginfo('Track roomba kicked back') + track_state = 0 + else: + action = guide_roomba_law(roomba) + action = 2 + # If block + if action == 1: + self._client.cancel_goal() + rospy.loginfo('SENDING BLOCK ROOMBA') + self._client.send_goal(QuadMoveGoal(movement_type="block_roomba", frame_id = roomba_id)) + track_state = 3 + # If hit + elif action == 2: + self._client.cancel_goal() + rospy.loginfo('SENDING HIT ROOMBA') + self._client.send_goal(QuadMoveGoal(movement_type="hit_roomba", frame_id = roomba_id)) + track_state = 4 + + if track_state == 3: + if did_task_fail(self._client): + rospy.loginfo('BLOCK FAILED') + track_state = 2 + if did_task_succeed(self._client): + rospy.loginfo('BLOCK SUCCEEDED') + track_state = 2 + + if track_state == 4: + if did_task_fail(self._client): + rospy.loginfo('HIT FAILED') + track_state = 2 + if did_task_succeed(self._client): + rospy.loginfo('HIT SUCCEEDED') + track_state = 2 + break + + rate.sleep() + return True + + def attempt_mission7(self): + + + #control_buttons.wait_for_start() + + # Takeoff + + self.flight_start_time = rospy.Time.now() + + self.basic_goal('takeoff') + + mission7_completed = False + gotten_roombas = 0 + while not mission7_completed: + roomba = self.search_for_roomba() + + if roomba is not None: + got_roomba = self.track_roomba_to_completion(roomba) + if got_roomba: + gotten_roombas += 1 + + if gotten_roombas > TARGET_NUM_ROOMBAS: + break + + if rospy.Time.now() > self.flight_start_time + rospy.Duration(MAX_FLIGHT_DURATION): + break + + rate = rospy.Rate(30) + landing_completed = False + while not landing_completed: + # self.goto_safe_landing_spot() + self.basic_goal('land') + while not did_task_finish(self._client): + rate.sleep() + + if did_task_succeed(self._client): + landing_completed = True + + rospy.loginfo('LANDING COMPLETED') + +if __name__ == '__main__': + # Initializes a rospy node so that the SimpleActionClient can + # publish and subscribe over ROS. + rospy.init_node('mission7') + arena_position_estimator = ArenaPositionEstimator() + + mission7 = Mission7() + + pub = rospy.Publisher('/start_roombas', BoolStamped, queue_size=100, latch=True) + msg = BoolStamped() + msg.header.stamp = rospy.Time.now() + pub.publish(msg) + + control_buttons = ControlButtonInterpreter() + + mission7.attempt_mission7() + + rospy.spin() diff --git a/scripts/takeoff_translate_land.py b/scripts/takeoff_translate_land.py index bc40880..0f9fc76 100755 --- a/scripts/takeoff_translate_land.py +++ b/scripts/takeoff_translate_land.py @@ -34,32 +34,35 @@ def takeoff_land(): if rospy.is_shutdown(): return rospy.logwarn("Takeoff success: {}".format(client.get_result())) - rospy.sleep(2.0) + Z_HEIGHT = 1.5 + TRANSLATION_DISTANCE = 1.5 - # Fly around in square all diagonals when possible. - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=4.0, z_position=3.0) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) + for i in range(100): + # Fly around in square all diagonals when possible. + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=TRANSLATION_DISTANCE, y_position=0.0, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 1 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=-4.0, z_position=8.0) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=TRANSLATION_DISTANCE, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 2 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=4.0, y_position=-4.0, z_position=1.0) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-TRANSLATION_DISTANCE, y_position=0.0, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 3 success: {}".format(client.get_result())) - goal = QuadMoveGoal(movement_type="xyztranslate", x_position=-4.0, y_position=4.0, z_position=3.0) - client.send_goal(goal) - client.wait_for_result() - if rospy.is_shutdown(): return - rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) + # Fly around in square all diagonals when possible. + goal = QuadMoveGoal(movement_type="xyztranslate", x_position=0.0, y_position=-TRANSLATION_DISTANCE, z_position=Z_HEIGHT) + client.send_goal(goal) + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Waypoint 4 success: {}".format(client.get_result())) # Test land goal = QuadMoveGoal(movement_type="land") diff --git a/scripts/test_obstacle_avoidance.py b/scripts/test_obstacle_avoidance.py new file mode 100755 index 0000000..99d640f --- /dev/null +++ b/scripts/test_obstacle_avoidance.py @@ -0,0 +1,66 @@ +#! /usr/bin/env python +import sys +import rospy +from iarc7_msgs.msg import OdometryArray +import actionlib +import tf2_ros +from iarc7_motion.msg import QuadMoveGoal, QuadMoveAction +from iarc7_safety.SafetyClient import SafetyClient + +def velocity_test(): + safety_client = SafetyClient('test_obstacle_avoidance_abstract') + # Since this abstract is top level in the control chain there is no need to check + # for a safety state. We can also get away with not checking for a fatal state since + # all nodes below will shut down. + assert(safety_client.form_bond()) + if rospy.is_shutdown(): return + + # Creates the SimpleActionClient, passing the type of the action + # (QuadMoveAction) to the constructor. (Look in the action folder) + client = actionlib.SimpleActionClient("motion_planner_server", QuadMoveAction) + + # Waits until the action server has started up and started + # listening for goals. + client.wait_for_server() + if rospy.is_shutdown(): return + + X_VELOCITY = 0.5 + Y_VELOCITY = 0.5 + TRANSLATE_DELAY = 7.0 + + # Test takeoff + goal = QuadMoveGoal(movement_type="takeoff") + # Sends the goal to the action server. + client.send_goal(goal) + # Waits for the server to finish performing the action. + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Takeoff success: {}".format(client.get_result())) + + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=X_VELOCITY, y_velocity=Y_VELOCITY, z_position=0.8) + # Sends the goal to the action server. + client.send_goal(goal) + rospy.sleep(TRANSLATE_DELAY ) + client.cancel_goal() + rospy.logwarn("Translation 1 canceled") + + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-X_VELOCITY, y_velocity=-Y_VELOCITY, z_position=0.8) + # Sends the goal to the action server. + client.send_goal(goal) + rospy.sleep(TRANSLATE_DELAY ) + client.cancel_goal() + rospy.logwarn("Returned Home") + + # Test land + goal = QuadMoveGoal(movement_type="land") + # Sends the goal to the action server. + client.send_goal(goal) + # Waits for the server to finish performing the action. + client.wait_for_result() + if rospy.is_shutdown(): return + rospy.logwarn("Land success: {}".format(client.get_result())) + +if __name__ == '__main__': + rospy.init_node('test_obstacle_avoidance_abstract') + velocity_test() + rospy.spin() diff --git a/scripts/velocity_test.py b/scripts/velocity_test.py index 4681fb2..c7fa487 100755 --- a/scripts/velocity_test.py +++ b/scripts/velocity_test.py @@ -26,9 +26,10 @@ def velocity_test(): X_VELOCITY = 0.5 Y_VELOCITY = 0.5 - X_DELAY = 2.0 - Y_DELAY = 2.0 + X_DELAY = 5.0 + Y_DELAY = 5.0 STOP_DELAY = 2.0 + HEIGHT = 1.1 # Test takeoff goal = QuadMoveGoal(movement_type="takeoff") @@ -40,56 +41,56 @@ def velocity_test(): rospy.logwarn("Takeoff success: {}".format(client.get_result())) for i in range(0, 1): - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=X_VELOCITY, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=X_VELOCITY, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(X_DELAY) client.cancel_goal() rospy.logwarn("Translation 1 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 1 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=-Y_VELOCITY, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=-Y_VELOCITY, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(Y_DELAY) client.cancel_goal() rospy.logwarn("Translation 2 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 2 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-X_VELOCITY, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=-X_VELOCITY, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(X_DELAY) client.cancel_goal() rospy.logwarn("Translation 3 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) client.cancel_goal() rospy.logwarn("Stop 3 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=Y_VELOCITY, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=Y_VELOCITY, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(Y_DELAY) client.cancel_goal() rospy.logwarn("Translation 4 canceled") - goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=0.8) + goal = QuadMoveGoal(movement_type="velocity_test", x_velocity=0.0, y_velocity=0.0, z_position=HEIGHT) # Sends the goal to the action server. client.send_goal(goal) rospy.sleep(STOP_DELAY) diff --git a/src/iarc7_abstract/arena_position_estimator.py b/src/iarc7_abstract/arena_position_estimator.py index db1bd89..f057121 100644 --- a/src/iarc7_abstract/arena_position_estimator.py +++ b/src/iarc7_abstract/arena_position_estimator.py @@ -9,15 +9,16 @@ def __init__(self): arena_starting_position_x = rospy.get_param('~arena_starting_position_x') arena_starting_position_y = rospy.get_param('~arena_starting_position_y') - size_x = rospy.get_param('~arena_size_x') - size_y = rospy.get_param('~arena_size_y') + self._size_x = rospy.get_param('~arena_size_x') + self._size_y = rospy.get_param('~arena_size_y') # left, right, top, bottom - self.arena_line_positions = [-arena_starting_position_x + size_y/2, - arena_starting_position_x - size_y/2, - -arena_starting_position_y + size_x/2, - arena_starting_position_y - size_x/2] + self.arena_line_positions = [-arena_starting_position_y + self._size_y/2, + -(arena_starting_position_y + self._size_y/2), + -arena_starting_position_x + self._size_x/2, + -(arena_starting_position_x + self._size_x/2)] + print 'STARTING LINE POSITIONS', self.arena_line_positions self.odom = None rospy.Subscriber('/floor_detector/boundaries', Boundary, self.boundary_callback) @@ -27,6 +28,7 @@ def odom_callback(self, data): self.odom = data def boundary_callback(self, data): + return if data.boundary_type == 0: self.arena_line_positions[0] = data.position self.arena_line_positions[1] = data.position - 20.0 @@ -42,8 +44,8 @@ def boundary_callback(self, data): def arena_to_map(self, arena_pos): # Arena pos (0,0) is the center of the arena - from_left_corner = (arena_pos[0] + 10.0, arena_pos[1] + 10.0) - return (self.arena_line_positions[3] + from_left_corner[0], self.arena_line_positions[0] + from_left_corner[1]) + from_left_corner = (arena_pos[0] + self._size_x/2, - arena_pos[1] + self._size_y/2) + return (self.arena_line_positions[3] + from_left_corner[0], self.arena_line_positions[0] - from_left_corner[1]) def distance_to_left(self): if self.odom is not None: