From 357a857b4651c13af37208d7b5f5b44dd705f6b5 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 27 Mar 2024 15:23:53 -0400 Subject: [PATCH 01/35] - do not announce social navigation (avoiding ...) while Narrow/Tight navgation - announce "person ahead" when stop-reasoner reasons "trying to avoid people" or "people are on my way" Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/interface.py | 14 ++++++++------ cabot_ui/cabot_ui/navgoal.py | 20 ++++++++++++++++++++ cabot_ui/cabot_ui/navigation.py | 14 +++++++------- cabot_ui/scripts/cabot_ui_manager.py | 5 ++++- 4 files changed, 39 insertions(+), 14 deletions(-) diff --git a/cabot_ui/cabot_ui/interface.py b/cabot_ui/cabot_ui/interface.py index 77d6fc8e..ea35374b 100644 --- a/cabot_ui/cabot_ui/interface.py +++ b/cabot_ui/cabot_ui/interface.py @@ -291,8 +291,8 @@ def enter_goal(self, goal): def exit_goal(self, goal): pass - def announce_social(self, message): - self._activity_log("cabot/interface", "notify", "social") + def announce_social(self, message, type="social"): + self._activity_log("cabot/interface", type, message) if self.last_social_announce is None or \ self._node.get_clock().now() - self.last_social_announce > UserInterface.SOCIAL_ANNOUNCE_INTERVAL: self.speak(i18n.localized_string(message)) @@ -339,17 +339,19 @@ def door_passed(self): def speak_stop_reason(self, code): message = None if code == StopReason.AVOIDING_PEOPLE: - message = "TRYING_TO_AVOID_PEOPLE" + # message = "TRYING_TO_AVOID_PEOPLE" + message = "PERSON_AHEAD" + elif code == StopReason.THERE_ARE_PEOPLE_ON_THE_PATH: + # message = "PEOPLE_ARE_ON_MY_WAY" + message = "PERSON_AHEAD" elif code == StopReason.AVOIDING_OBSTACLE: message = "TRYING_TO_AVOID_OBSTACLE" - elif code == StopReason.THERE_ARE_PEOPLE_ON_THE_PATH: - message = "PEOPLE_ARE_ON_MY_WAY" elif code == StopReason.UNKNOWN: message = "PLEASE_WAIT_FOR_A_SECOND" elif code == StopReason.NO_TOUCH: message = "NOT_DETECT_TOUCH" if message: - self.announce_social(message) + self.announce_social(message, type="stop-reason") def please_follow_behind(self): self._activity_log("cabot/interface", "navigation", "please_follow_behind") diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 9556176f..1625770d 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -520,6 +520,14 @@ def done_change_parameters_callback(result): def current_statement(self): return self._current_statement + @property + def is_social_navigation_enabled(self): + return True + + @property + def is_stop_reason_enabled(self): + return True + def __str__(self): ret = F"{type(self)}, ({hex(id(self))})\n" for key in self.__dict__: @@ -744,6 +752,10 @@ def __init__(self, delegate, navcog_route, anchor, target_poi=None, set_back=(0, self.route_index = 0 super(NavGoal, self).__init__(delegate, angle=180, floor=navcog_route[-1].floor, pose_msg=last_pose, **kwargs) + @property + def is_social_navigation_enabled(self): + return self.navcog_routes[self.route_index][2] == geojson.NavigationMode.Standard + def _extract_pois(self): """extract pois along the route""" temp = [] @@ -1245,6 +1257,14 @@ def nav_params(self): } } + @property + def is_social_navigation_enabled(self): + return False + + @property + def is_stop_reason_enabled(self): + return False + def _enter(self): # change queue_interval setting if self.queue_interval is not None: diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index ee2b5b12..c142bf21 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -102,9 +102,6 @@ def exit_goal(self, goal): def could_not_get_current_location(self): CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") - def announce_social(self, message): - CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") - def please_call_elevator(self, pos): CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") @@ -955,7 +952,8 @@ def _check_social(self, current_pose): return # do not provide social navigation messages while queue navigation - if isinstance(self._current_goal, navgoal.QueueNavGoal): + # do not provide social navigation messages while narrow/tight + if self._current_goal and not self._current_goal.is_social_navigation_enabled: return self.social_navigation.current_pose = current_pose @@ -963,6 +961,11 @@ def _check_social(self, current_pose): if message is not None: self.delegate.announce_social(message) + def process_stop_reason(self, code): + if self._current_goal and not self._current_goal.is_stop_reason_enabled: + return + self.delegate.speak_stop_reason(code) + def _check_goal(self, current_pose): self._logger.info(F"navigation.{util.callee_name()} called", throttle_duration_sec=1) goal = self._current_goal @@ -1019,9 +1022,6 @@ def enter_goal(self, goal): def exit_goal(self, goal): self.delegate.exit_goal(goal) - def announce_social(self, messages): - self.delegate.announce_social(messages) - def navigate_to_pose(self, goal_pose, behavior_tree, gh_cb, done_cb, namespace=""): self._logger.info(F"{namespace}/navigate_to_pose") self.delegate.activity_log("cabot/navigation", "navigate_to_pose") diff --git a/cabot_ui/scripts/cabot_ui_manager.py b/cabot_ui/scripts/cabot_ui_manager.py index 93bad2a7..11c1cb25 100755 --- a/cabot_ui/scripts/cabot_ui_manager.py +++ b/cabot_ui/scripts/cabot_ui_manager.py @@ -220,6 +220,9 @@ def exit_goal(self, goal): def announce_social(self, message): self._interface.announce_social(message) + def speak_stop_reason(self, code): + self._interface.speak_stop_reason(code) + def please_call_elevator(self, pos): self._interface.please_call_elevator(pos) @@ -514,7 +517,7 @@ def done_callback(): if event.subtype == "stop-reason": if self._status_manager.state == State.in_action: code = StopReason[event.param] - self._interface.speak_stop_reason(code) + self._navigation.process_stop_reason(code) def _process_exploration_event(self, event): if event.type != ExplorationEvent.TYPE: From b7c2dddad738574495570ef7f74dc025a2c4b548 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 27 Mar 2024 15:33:15 -0400 Subject: [PATCH 02/35] do not announce about obstacles Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/interface.py | 3 ++- cabot_ui/cabot_ui/social_navigation.py | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/cabot_ui/cabot_ui/interface.py b/cabot_ui/cabot_ui/interface.py index ea35374b..eb96ff1e 100644 --- a/cabot_ui/cabot_ui/interface.py +++ b/cabot_ui/cabot_ui/interface.py @@ -345,7 +345,8 @@ def speak_stop_reason(self, code): # message = "PEOPLE_ARE_ON_MY_WAY" message = "PERSON_AHEAD" elif code == StopReason.AVOIDING_OBSTACLE: - message = "TRYING_TO_AVOID_OBSTACLE" + # message = "TRYING_TO_AVOID_OBSTACLE" + pass elif code == StopReason.UNKNOWN: message = "PLEASE_WAIT_FOR_A_SECOND" elif code == StopReason.NO_TOUCH: diff --git a/cabot_ui/cabot_ui/social_navigation.py b/cabot_ui/cabot_ui/social_navigation.py index 83457184..4600fcb8 100644 --- a/cabot_ui/cabot_ui/social_navigation.py +++ b/cabot_ui/cabot_ui/social_navigation.py @@ -113,10 +113,10 @@ def _update(self): self._set_message("AVOIDING_A_PERSON", "AVOID", 10) elif self._people_count > 1: self._set_message("AVOIDING_PEOPLE", "AVOID", 10) - elif self._obstacles_count == 1: - self._set_message("AVOIDING_AN_OBSTACLE", "AVOID", 10) - elif self._obstacles_count > 1: - self._set_message("AVOIDING_OBSTACLES", "AVOID", 10) + # elif self._obstacles_count == 1: + # self._set_message("AVOIDING_AN_OBSTACLE", "AVOID", 10) + # elif self._obstacles_count > 1: + # self._set_message("AVOIDING_OBSTACLES", "AVOID", 10) self._turn = None # check event From db23c30ad8947713ae971b1bf85f37431737414a Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Thu, 28 Mar 2024 13:11:30 -0400 Subject: [PATCH 03/35] wip: emit sounds for avoiding obstacle Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navigation.py | 1 + cabot_ui/cabot_ui/social_navigation.py | 8 ++++---- cabot_ui/scripts/cabot_ui_manager.py | 20 ++++++++++++++++++-- 3 files changed, 23 insertions(+), 6 deletions(-) diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index c142bf21..cccb2cf2 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -964,6 +964,7 @@ def _check_social(self, current_pose): def process_stop_reason(self, code): if self._current_goal and not self._current_goal.is_stop_reason_enabled: return + self._logger.info(f"process_stop_reason is callded: {code}") self.delegate.speak_stop_reason(code) def _check_goal(self, current_pose): diff --git a/cabot_ui/cabot_ui/social_navigation.py b/cabot_ui/cabot_ui/social_navigation.py index 4600fcb8..83457184 100644 --- a/cabot_ui/cabot_ui/social_navigation.py +++ b/cabot_ui/cabot_ui/social_navigation.py @@ -113,10 +113,10 @@ def _update(self): self._set_message("AVOIDING_A_PERSON", "AVOID", 10) elif self._people_count > 1: self._set_message("AVOIDING_PEOPLE", "AVOID", 10) - # elif self._obstacles_count == 1: - # self._set_message("AVOIDING_AN_OBSTACLE", "AVOID", 10) - # elif self._obstacles_count > 1: - # self._set_message("AVOIDING_OBSTACLES", "AVOID", 10) + elif self._obstacles_count == 1: + self._set_message("AVOIDING_AN_OBSTACLE", "AVOID", 10) + elif self._obstacles_count > 1: + self._set_message("AVOIDING_OBSTACLES", "AVOID", 10) self._turn = None # check event diff --git a/cabot_ui/scripts/cabot_ui_manager.py b/cabot_ui/scripts/cabot_ui_manager.py index 11c1cb25..d8a4591e 100755 --- a/cabot_ui/scripts/cabot_ui_manager.py +++ b/cabot_ui/scripts/cabot_ui_manager.py @@ -218,10 +218,26 @@ def exit_goal(self, goal): self._interface.exit_goal(goal) def announce_social(self, message): - self._interface.announce_social(message) + self._logger.info(f"cabot_ui_manager.announce_social is called: {str(message)}") + sound_only = ["AVOIDING_AN_OBSTACLE", "AVOIDING_OBSTACLES"] + if message in sound_only: + e = NavigationEvent("sound", str(message)) + msg = std_msgs.msg.String() + msg.data = str(e) + self._eventPub.publish(msg) + else: + self._interface.announce_social(message) def speak_stop_reason(self, code): - self._interface.speak_stop_reason(code) + self._logger.info(f"cabot_ui_manager.speak_stop_reason is called: {str(code)}") + sound_only = [StopReason.AVOIDING_OBSTACLE] + if code in sound_only: + e = NavigationEvent("sound", str(code)) + msg = std_msgs.msg.String() + msg.data = str(e) + self._eventPub.publish(msg) + else: + self._interface.speak_stop_reason(code) def please_call_elevator(self, pos): self._interface.please_call_elevator(pos) From 8e90428be896fc5529a7e371a0b992a93eb7e9eb Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 30 Mar 2024 21:10:30 -0400 Subject: [PATCH 04/35] refactoring social navigation Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/interface.py | 39 ++---- cabot_ui/cabot_ui/navigation.py | 49 +++++-- cabot_ui/cabot_ui/social_navigation.py | 185 ++++++++++++++++++++----- cabot_ui/cabot_ui/stop_reasoner.py | 4 +- cabot_ui/cabot_ui/turn_detector.py | 3 + cabot_ui/i18n/en.yaml | 15 +- cabot_ui/i18n/ja.yaml | 15 +- cabot_ui/scripts/cabot_ui_manager.py | 43 ++---- cabot_ui/src/stop_reasoner.cpp | 2 +- cabot_ui/src/stop_reasoner.hpp | 4 +- cabot_ui/src/stop_reasons_node.cpp | 14 +- 11 files changed, 243 insertions(+), 130 deletions(-) diff --git a/cabot_ui/cabot_ui/interface.py b/cabot_ui/cabot_ui/interface.py index eb96ff1e..952cde26 100644 --- a/cabot_ui/cabot_ui/interface.py +++ b/cabot_ui/cabot_ui/interface.py @@ -30,11 +30,11 @@ import cabot_msgs.msg import cabot_msgs.srv from cabot_ui import visualizer, i18n +from cabot_ui.event import NavigationEvent from cabot_ui.turn_detector import Turn -from cabot_ui.stop_reasoner import StopReason +from cabot_ui.social_navigation import SNMessage from cabot_common import vibration - class UserInterface(object): SOCIAL_ANNOUNCE_INTERVAL = Duration(seconds=15.0) NOTIFY_TURN_INTERVAL = Duration(seconds=5.0) @@ -46,6 +46,7 @@ def __init__(self, node: Node): self.note_pub = node.create_publisher(std_msgs.msg.Int8, "/cabot/notification", 10, callback_group=MutuallyExclusiveCallbackGroup()) self.activity_log_pub = node.create_publisher(cabot_msgs.msg.Log, "/cabot/activity_log", 10, callback_group=MutuallyExclusiveCallbackGroup()) self.pose_log_pub = node.create_publisher(cabot_msgs.msg.PoseLog, "/cabot/pose_log", 10, callback_group=MutuallyExclusiveCallbackGroup()) + self.event_pub = self._node.create_publisher(std_msgs.msg.String, "/cabot/event", 10, callback_group=MutuallyExclusiveCallbackGroup()) self.lang = node.declare_parameter("language", "en").value self.site = node.declare_parameter("site", '').value @@ -291,12 +292,16 @@ def enter_goal(self, goal): def exit_goal(self, goal): pass - def announce_social(self, message, type="social"): - self._activity_log("cabot/interface", type, message) - if self.last_social_announce is None or \ - self._node.get_clock().now() - self.last_social_announce > UserInterface.SOCIAL_ANNOUNCE_INTERVAL: - self.speak(i18n.localized_string(message)) - self.last_social_announce = self._node.get_clock().now() + def announce_social(self, message: SNMessage): + self._activity_log("cabot/interface", message.type.name, message.code.name) + self.speak(i18n.localized_string(message.code.name)) + + def request_sound(self, sound: SNMessage): + self._activity_log("cabot/interface", sound.type.name, sound.code.name) + e = NavigationEvent("sound", sound.code.name) + msg = std_msgs.msg.String() + msg.data = str(e) + self.event_pub.publish(msg) def set_pause_control(self, flag): self._activity_log("cabot/interface", "pause_control", str(flag)) @@ -336,24 +341,6 @@ def door_passed(self): self._activity_log("cabot/interface", "navigation", "door passed") self.speak(i18n.localized_string("DOOR_POI_PASSED")) - def speak_stop_reason(self, code): - message = None - if code == StopReason.AVOIDING_PEOPLE: - # message = "TRYING_TO_AVOID_PEOPLE" - message = "PERSON_AHEAD" - elif code == StopReason.THERE_ARE_PEOPLE_ON_THE_PATH: - # message = "PEOPLE_ARE_ON_MY_WAY" - message = "PERSON_AHEAD" - elif code == StopReason.AVOIDING_OBSTACLE: - # message = "TRYING_TO_AVOID_OBSTACLE" - pass - elif code == StopReason.UNKNOWN: - message = "PLEASE_WAIT_FOR_A_SECOND" - elif code == StopReason.NO_TOUCH: - message = "NOT_DETECT_TOUCH" - if message: - self.announce_social(message, type="stop-reason") - def please_follow_behind(self): self._activity_log("cabot/interface", "navigation", "please_follow_behind") self.speak(i18n.localized_string("FOLLOW_BEHIND_PLEASE_NARROW")) diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index cccb2cf2..cf9a90a6 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -50,7 +50,7 @@ from cabot_ui.turn_detector import TurnDetector, Turn from cabot_ui import navgoal from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil -from cabot_ui.social_navigation import SocialNavigation +from cabot_ui.social_navigation import SocialNavigation, SNMessage from cabot_msgs.srv import LookupTransform import queue_msgs.msg from mf_localization_msgs.msg import MFLocalizeStatus @@ -129,6 +129,12 @@ def please_follow_behind(self): def please_return_position(self): CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") + def announce_social(self, message: SNMessage): + CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") + + def request_sound(self, message: SNMessage): + CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") + class BufferProxy(): def __init__(self, node): @@ -217,7 +223,7 @@ def __init__(self, node: Node, tf_node: Node, datautil_instance=None, anchor_fil self._logger = node.get_logger() self.visualizer = visualizer.instance(node) - self.delegate = NavigationInterface() + self.delegate: NavigationInterface = NavigationInterface() self.buffer = BufferProxy(tf_node) # TF listening is at high frequency and increases the CPU usage substantially @@ -276,7 +282,7 @@ def current_ros_pose(self, frame=None): self._logger.debug("cannot get current_ros_pose") raise RuntimeError("no transformation") - def current_local_pose(self, frame=None): + def current_local_pose(self, frame=None) -> geoutil.Pose: """get current local location""" if frame is None: frame = self._global_map_name @@ -326,6 +332,7 @@ class Navigation(ControlBase, navgoal.GoalInterface): ACTIONS = {"navigate_to_pose": nav2_msgs.action.NavigateToPose, "navigate_through_poses": nav2_msgs.action.NavigateThroughPoses} NS = ["", "/local"] + TURN_NEARBY_THRESHOLD = 2 def __init__(self, node: Node, tf_node: Node, srv_node: Node, act_node: Node, soc_node: Node, datautil_instance=None, anchor_file=None, wait_for_action=True): @@ -342,6 +349,7 @@ def __init__(self, node: Node, tf_node: Node, srv_node: Node, act_node: Node, so self.queue_wait_pois = [] self.speed_pois = [] self.turns = [] + self.notified_turns = [] self.i_am_ready = False self._sub_goals = None @@ -488,8 +496,8 @@ def _plan_callback(self, path): if 0 < abs(t1.angle) and abs(t1.angle) < math.pi/3 and \ 0 < abs(t2.angle) and abs(t2.angle) < math.pi/3: self.turns.pop(i+1) - except ValueError: - pass + except: # noqa: 722 + self._logger.error(traceback.format_exc()) self.visualizer.visualize() @@ -526,6 +534,7 @@ def _goal_updated_callback(self, msg): # wrap execution by a queue def set_destination(self, destination): + self.social_navigation.set_active(True) self._process_queue.append((self._set_destination, destination)) def _set_destination(self, destination): @@ -629,6 +638,7 @@ def _pause_navigation(self, callback): callback() self.turns = [] + self.notified_turns = [] if self._current_goal: self._goal_index -= 1 @@ -652,6 +662,7 @@ def _resume_navigation(self, callback): # wrap execution by a queue def cancel_navigation(self, callback=None): self._process_queue.append((self._cancel_navigation, callback)) + self.social_navigation.set_active(False) def _cancel_navigation(self, callback): """callback for cancel topic""" @@ -684,6 +695,7 @@ def _navigate_next_sub_goal(self): self._current_goal = None self.delegate.have_completed() self.delegate.activity_log("cabot/navigation", "completed") + self.social_navigation.set_active(False) def _navigate_sub_goal(self, goal): self._logger.info(F"navigation.{util.callee_name()} called") @@ -701,6 +713,7 @@ def _navigate_sub_goal(self, goal): self.info_pois = [] self.queue_wait_pois = [] self.turns = [] + self.notified_turns = [] self._logger.info(F"goal: {goal}") try: @@ -861,18 +874,28 @@ def _check_turn(self, current_pose): turn.passed = True self._logger.info(F"notify turn {turn}") - self.delegate.notify_turn(turn=turn) - if turn.turn_type == Turn.Type.Avoiding: # give avoiding announce self._logger.info("social_navigation avoiding turn") self.social_navigation.turn = turn + + if self._check_already_notified_turn_nearby(turn): + self.delegate.notify_turn(turn=turn) except: # noqa: E722 import traceback self._logger.error(traceback.format_exc()) self._logger.error("could not convert pose for checking turn POI", throttle_duration_sec=3) + def _check_already_notified_turn_nearby(self, turn: Turn): + result = True + for other in self.notified_turns: + if other.distance_to(turn) < Navigation.TURN_NEARBY_THRESHOLD: + result = False + self.notified_turns.append(turn) + self._logger.info(f"_check_already_notified_turn_nearby, {result}, {turn}") + return result + def _check_queue_wait(self, current_pose): if not isinstance(self._current_goal, navgoal.QueueNavGoal) or not self.queue_wait_pois: return @@ -950,22 +973,22 @@ def _check_queue_wait(self, current_pose): def _check_social(self, current_pose): if self.social_navigation is None: return + self._logger.info(F"navigation.{util.callee_name()} called", throttle_duration_sec=1) # do not provide social navigation messages while queue navigation # do not provide social navigation messages while narrow/tight if self._current_goal and not self._current_goal.is_social_navigation_enabled: return + if self._current_goal and not self._current_goal.is_stop_reason_enabled: + return self.social_navigation.current_pose = current_pose message = self.social_navigation.get_message() if message is not None: self.delegate.announce_social(message) - - def process_stop_reason(self, code): - if self._current_goal and not self._current_goal.is_stop_reason_enabled: - return - self._logger.info(f"process_stop_reason is callded: {code}") - self.delegate.speak_stop_reason(code) + sound = self.social_navigation.get_sound() + if sound is not None: + self.delegate.request_sound(sound) def _check_goal(self, current_pose): self._logger.info(F"navigation.{util.callee_name()} called", throttle_duration_sec=1) diff --git a/cabot_ui/cabot_ui/social_navigation.py b/cabot_ui/cabot_ui/social_navigation.py index 83457184..20397e0e 100644 --- a/cabot_ui/cabot_ui/social_navigation.py +++ b/cabot_ui/cabot_ui/social_navigation.py @@ -19,12 +19,69 @@ # SOFTWARE. import rclpy.node +import rclpy.time +import rclpy.clock from rclpy.duration import Duration from cabot_ui.turn_detector import Turn from people_msgs.msg import People import tf2_geometry_msgs # noqa: to register class for transform from geometry_msgs.msg import PointStamped from nav_msgs.msg import Odometry +from cabot_msgs.msg import StopReason + +from dataclasses import dataclass +import enum + +@dataclass +class SNMessage: + class Type(enum.Enum): + Message = enum.auto() + Sound = enum.auto() + + class Code(enum.Enum): + PERSON_AHEAD = enum.auto() + OBSTACLE_AHEAD = enum.auto() + PLEASE_WAIT_FOR_A_SECOND = enum.auto() + NOT_DETECT_TOUCH = enum.auto() + # not used, avoid + AVOIDING_A_PERSON = enum.auto() + AVOIDING_PEOPLE = enum.auto() + AVOIDING_AN_OBSTACLE = enum.auto() + AVOIDING_OBSTACLES = enum.auto() + # not used, stop + TRYING_TO_AVOID_PEOPLE = enum.auto() + PEOPLE_ARE_IN_MY_WAY = enum.auto() + TRYING_TO_AVOID_OBSTACLE = enum.auto() + # not used, speed control + A_PERSON_IN_THE_WAY = enum.auto() + PEOPLE_IN_THE_WAY = enum.auto() + # not used, follow + FOLLOWING_A_PERSON = enum.auto() + FOLLOWING_PEOPLE = enum.auto() + + class Category(enum.Enum): + AVOID = enum.auto() + STOP = enum.auto() + IN_THE_WAY = enum.auto() + FOLLOWING = enum.auto() + + type: Type + code: Code + category: Category + time: rclpy.time.Time + priority: int + + @staticmethod + def empty(type: Type, clock: rclpy.clock.Clock): + return SNMessage(type=type, code=None, category=None, time=clock.now(), priority=0) + + @staticmethod + def empty_message(clock: rclpy.clock.Clock): + return SNMessage.empty(SNMessage.Type.Message, clock) + + @staticmethod + def empty_sound(clock: rclpy.clock.Clock): + return SNMessage.empty(SNMessage.Type.Sound, clock) class SocialNavigation(object): @@ -41,17 +98,25 @@ def __init__(self, node: rclpy.node.Node, buffer): self._people_count = 0 self._obstacles_count = 0 self._event = None - self._message = None - self._last_message = None - self._priority = 0 - self._last_message_time = node.get_clock().now() - self._last_category = None + + self._message: SNMessage = SNMessage.empty_message(node.get_clock()) + self._last_message: SNMessage = SNMessage.empty_message(node.get_clock()) + self._sound: SNMessage = SNMessage.empty_sound(node.get_clock()) + self._last_sound: SNMessage = SNMessage.empty_sound(node.get_clock()) + + self._stop_reason = None + self._is_active = False odom_topic = node.declare_parameter("odom_topic", "/odom").value people_topic = node.declare_parameter("people_topic", "/people").value obstacles_topic = node.declare_parameter("obstacles_topic", "/obstacles").value + stop_reason_topic = node.declare_parameter("stop_reason_topic", "/stop_reason").value self.odom_topic = node.create_subscription(Odometry, odom_topic, self._odom_callback, 10) self.people_sub = node.create_subscription(People, people_topic, self._people_callback, 10) self.obstacles_sub = node.create_subscription(People, obstacles_topic, self._obstacles_callback, 10) + self.stop_reason_sub = node.create_subscription(StopReason, stop_reason_topic, self._stop_reason_callback, 10) + + def set_active(self, active): + self._is_active = active def _odom_callback(self, msg): self._latest_odom = msg @@ -103,70 +168,124 @@ def _obstacles_callback(self, msg): self._obstacles_count = count self._update() - def _update(self): - self._logger.info(F"social navigation update turn={self._turn}", throttle_duration_sec=1) + def _stop_reason_callback(self, msg: StopReason): + if msg.summary: + self._stop_reason = StopReason[msg.reason] + self._update() + def _update(self): + ''' + Message types: + - Avoiding something + - Stop due to + - Speed is control by + - Person (People) - always say PERSON_AHEAD since 2024.03.28 + - Obstacle (Obstacles) - always play sound since 2024.03.28 + ''' if self._turn is not None and self._turn.turn_type == Turn.Type.Avoiding: + self._logger.info(F"social navigation update turn={self._turn}") self._logger.info(F"avoiding turn, people count = {self._people_count}, " F"obstacle count = {self._obstacles_count}") if self._people_count == 1: - self._set_message("AVOIDING_A_PERSON", "AVOID", 10) + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.Category.AVOID, 10) + # self._set_message(SNMessage.Code.AVOIDING_A_PERSON, SNMessage.Category.AVOID, 10) elif self._people_count > 1: - self._set_message("AVOIDING_PEOPLE", "AVOID", 10) + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.Category.AVOID, 10) + # self._set_message(SNMessage.Code.AVOIDING_PEOPLE, SNMessage.Category.AVOID, 10) elif self._obstacles_count == 1: - self._set_message("AVOIDING_AN_OBSTACLE", "AVOID", 10) + self._set_sound(SNMessage.Code.OBSTACLE_AHEAD, SNMessage.Category.AVOID, 10) + # self._set_sound(SNMessage.Code.AVOIDING_AN_OBSTACLE, SNMessage.Category.AVOID) elif self._obstacles_count > 1: - self._set_message("AVOIDING_OBSTACLES", "AVOID", 10) + self._set_sound(SNMessage.Code.OBSTACLE_AHEAD, SNMessage.Category.AVOID, 10) + # self._set_sound(SNMessage.Code.AVOIDING_OBSTACLES, SNMessage.Category.AVOID) self._turn = None + if self._stop_reason is not None: + self._logger.info(F"social navigation stop_reason {self._stop_reason}") + code = self._stop_reason + if code == StopReason.AVOIDING_PEOPLE: + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.STOP, 7) + # self._set_message(SNMessage.Code.TRYING_TO_AVOID_PEOPLE, SNMessage.STOP, 7) + elif code == StopReason.THERE_ARE_PEOPLE_IN_THE_PATH: + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.STOP, 7) + # self._set_message(SNMessage.Code.PEOPLE_ARE_IN_MY_WAY, SNMessage.STOP, 7) + elif code == StopReason.AVOIDING_OBSTACLE: + self._set_sound(SNMessage.Code.OBSTACLE_AHEAD, SNMessage.Category.AVOID) + # self._set_sound(SNMessage.Code.TRYING_TO_AVOID_OBSTACLE, SNMessage.STOP, 7) + elif code == StopReason.UNKNOWN: + self._set_message(SNMessage.Code.PLEASE_WAIT_FOR_A_SECOND, SNMessage.STOP, 7) + elif code == StopReason.NO_TOUCH: + self._set_message(SNMessage.Code.NOT_DETECT_TOUCH, SNMessage.STOP, 7) + self._stop_reason = None + # check event if self._event is not None: + self._logger.info(F"social navigation event {self._event}") param = self._event.param - self._logger.info(F"event {param}") - # trying to avoid people - if param == "bt_navigator_avoid_person": # trying to avoid people - self._set_message("AVOIDING_A_PERSON", "AVOID", 10) - if param == "bt_navigator_avoid_people": # trying to avoid people - self._set_message("AVOIDING_PEOPLE", "AVOID", 10) # stops due to people on the path if param == "people_speed_stopped": if self._people_count == 1: - self._set_message("A_PERSON_IN_THE_WAY", "IN_THE_WAY", 5) + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.Category.IN_THE_WAY, 5) + # self._set_message(SNMessage.Code.A_PERSON_IN_THE_WAY, SNMessage.Category.IN_THE_WAY, 5) elif self._people_count > 1: - self._set_message("PEOPLE_IN_THE_WAY", "IN_THE_WAY", 5) + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.Category.IN_THE_WAY, 5) + # self._set_message(SNMessage.Code.PEOPLE_IN_THE_WAY, SNMessage.Category.IN_THE_WAY, 5) # following people if param == "people_speed_following": if self._people_count == 1: - self._set_message("FOLLOWING_A_PERSON", "FOLLWING", 1) + self._set_message(SNMessage.Code.FOLLOWING_A_PERSON, SNMessage.Category.FOLLWING, 1) elif self._people_count > 1: - self._set_message("FOLLOWING_PEOPLE", "FOLLWING", 1) + self._set_message(SNMessage.Code.FOLLOWING_PEOPLE, SNMessage.Category.FOLLWING, 1) # delete event after check self._event = None - def _set_message(self, message, category, priority): - self._logger.info(F"set_message {message} {priority}") + def _set_message(self, code, category, priority): + self._logger.info(F"set_message {code} {category} {priority}") now = self._node.get_clock().now() - if self._priority < priority and \ - (self._last_category != category or - (now - self._last_message_time) > Duration(seconds=15.0)): + if self._last_message.priority < priority and \ + (self._last_message.category != category or + (now - self._last_message.time) > Duration(seconds=15.0)): self._priority = priority - self._message = message - self._last_category = category + self._message.code = code + self._message.category = category + self._message.priority = priority - def get_message(self): + def _set_sound(self, code, category, priority): + self._logger.info(F"set_sound {code} {category} {priority}") now = self._node.get_clock().now() + if self._last_sound.priority < priority and \ + (self._last_sound.category != category or + (now - self._last_sound.time) > Duration(seconds=15.0)): + self._priority = priority + self._sound.code = code + self._sound.category = category + self._sound.priority = priority - if self._message is not None and (now - self._last_message_time) > Duration(seconds=5.0): - self._last_message_time = now + def get_message(self) -> SNMessage: + if not self._is_active: + return + now = self._node.get_clock().now() + + if self._message.code is not None and (now - self._last_message.time) > Duration(seconds=5.0): self._last_message = self._message - self._message = None - self._priority = 0 + self._message = SNMessage.empty_message(self._node.get_clock()) return self._last_message return None + def get_sound(self) -> SNMessage: + if not self._is_active: + return + now = self._node.get_clock().now() + + if self._sound.code is not None and (now - self._last_sound.time) > Duration(seconds=5.0): + self._last_sound = self._sound + self._sound = SNMessage.empty_sound(self._node.get_clock()) + return self._last_sound + return None + @property def path(self): return self._path diff --git a/cabot_ui/cabot_ui/stop_reasoner.py b/cabot_ui/cabot_ui/stop_reasoner.py index d13b6640..979b18a5 100644 --- a/cabot_ui/cabot_ui/stop_reasoner.py +++ b/cabot_ui/cabot_ui/stop_reasoner.py @@ -177,7 +177,7 @@ class StopReason(enum.Enum): NO_TOUCH = enum.auto() STOPPED_BUT_UNDER_THRESHOLD = enum.auto() NO_CMD_VEL = enum.auto() - THERE_ARE_PEOPLE_ON_THE_PATH = enum.auto() + THERE_ARE_PEOPLE_IN_THE_PATH = enum.auto() WAITING_FOR_ELEVATOR = enum.auto() AVOIDING_OBSTACLE = enum.auto() AVOIDING_PEOPLE = enum.auto() @@ -438,7 +438,7 @@ def update(self): if self.people_speed.minimum is not None: if self.people_speed.minimum < 0.9: logger.debug("%.2f, people_speed minimum=%.2f, average=%.2f", now().nanoseconds/1e9, self.people_speed.minimum, self.people_speed.average) - return (duration, StopReason.THERE_ARE_PEOPLE_ON_THE_PATH) + return (duration, StopReason.THERE_ARE_PEOPLE_IN_THE_PATH) if self.waiting_for_elevator: return (duration, StopReason.WAITING_FOR_ELEVATOR) diff --git a/cabot_ui/cabot_ui/turn_detector.py b/cabot_ui/cabot_ui/turn_detector.py index 6b1ebdd9..0f474403 100644 --- a/cabot_ui/cabot_ui/turn_detector.py +++ b/cabot_ui/cabot_ui/turn_detector.py @@ -73,6 +73,9 @@ def __str__(self): def __repr__(self): return self.__str__() + def distance_to(self, other): + return np.sqrt((self.start.x - other.start.x) ** 2 + (self.start.y - other.start.y) ** 2) + class TurnDetector: def __init__(self): diff --git a/cabot_ui/i18n/en.yaml b/cabot_ui/i18n/en.yaml index 1d1ed3ea..0ecbe671 100644 --- a/cabot_ui/i18n/en.yaml +++ b/cabot_ui/i18n/en.yaml @@ -32,14 +32,21 @@ YOU_HAVE_ARRIVED_WITH_NAME: "you have arrived at {0}" YOU_HAVE_ARRIVED: "you have arrived" ## Social +PERSON_AHEAD: "person ahead" AVOIDING_A_PERSON: "avoiding a person" AVOIDING_PEOPLE: "avoiding people" AVOIDING_AN_OBSTACLE: "avoiding an obstacle" AVOIDING_OBSTACLES: "avoiding obstacles" +TRYING_TO_AVOID_PEOPLE: "trying to avoid people" +TRYING_TO_AVOID_OBSTACLE: "trying to avoid obstacle" +PLEASE_WAIT_FOR_A_SECOND: "please wait for a second" A_PERSON_IN_THE_WAY: "a person is in the way" -PEOPLE_IN_THE_WAY: "people are in the way" +PEOPLE_ARE_IN_MY_WAY: "people are in the way" +PEOPLE_IN_THE_WAY: "people in the way" FOLLOWING_A_PERSON: "following a person" FOLLOWING_PEOPLE: "following some people" + +## queue GOING_TO_GET_IN_LINE: "We are going to get in line" @@ -95,12 +102,6 @@ HANDLE_LEFT_ABOUT_TURN: "Left about turn" HANDLE_LEFT_TURN: "Left turn" HANDLE_LEFT_DEV: "Left deviation" -## Stop Reason -TRYING_TO_AVOID_PEOPLE: "trying to avoid people" -TRYING_TO_AVOID_OBSTACLE: "trying to avoid obstacle" -PEOPLE_ARE_ON_MY_WAY: "people are on my way" -PLEASE_WAIT_FOR_A_SECOND: "please wait for a second" - ## Narrow FOLLOW_BEHIND_PLEASE_NARROW: "Pass through a narrow road. follow behind suitcase." RETURN_TO_POSITION_PLEASE: "Passed through a narrow road. return to first position." diff --git a/cabot_ui/i18n/ja.yaml b/cabot_ui/i18n/ja.yaml index 066b7d8f..44d0c097 100644 --- a/cabot_ui/i18n/ja.yaml +++ b/cabot_ui/i18n/ja.yaml @@ -32,14 +32,21 @@ YOU_HAVE_ARRIVED_WITH_NAME: "{0}に到着しました。" YOU_HAVE_ARRIVED: "到着しました。" ## Social +PERSON_AHEAD: "前に人がいます" AVOIDING_A_PERSON: "人をよけます。" AVOIDING_PEOPLE: "複数の人をよけます。" AVOIDING_AN_OBSTACLE: "障害物を避けます。" AVOIDING_OBSTACLES: "複数の障害物を避けます。" +TRYING_TO_AVOID_PEOPLE: "人を避けようとしています" +TRYING_TO_AVOID_OBSTACLE: "障害物を避けようとしています" +PEOPLE_ARE_IN_MY_WAY: "進行方向に人がいるため進めません" +PLEASE_WAIT_FOR_A_SECOND: "少々お待ちください" A_PERSON_IN_THE_WAY: "ルート上に人。" PEOPLE_IN_THE_WAY: "ルート上に複数の人。" FOLLOWING_A_PERSON: "前の人に合わせています。" FOLLOWING_PEOPLE: "前の人たちに合わせています。" + +## queue GOING_TO_GET_IN_LINE: "列に並びます。" @@ -95,12 +102,6 @@ HANDLE_LEFT_ABOUT_TURN: "左に旋回します" HANDLE_LEFT_TURN: "左に曲がります" HANDLE_LEFT_DEV: "左に少し曲がります" -## Stop Reason -TRYING_TO_AVOID_PEOPLE: "人を避けようとしています" -TRYING_TO_AVOID_OBSTACLE: "障害物を避けようとしています" -PEOPLE_ARE_ON_MY_WAY: "進行方向に人がいるため進めません" -PLEASE_WAIT_FOR_A_SECOND: "少々お待ちください" - ## Narrow FOLLOW_BEHIND_PLEASE_NARROW: "狭い道を通ります。スーツケースの後ろをついてきてください。" RETURN_TO_POSITION_PLEASE: "細い道を抜けました。元の位置に戻ってください。" @@ -111,4 +112,4 @@ NOT_DETECT_TOUCH: "ハンドルのタッチを検出できません。" ## Facility RIGHT_SIDE: "右側" LEFT_SIDE: "左側" -APPROACEHD_TO_FACILITY: "{1}に{0}があります" \ No newline at end of file +APPROACEHD_TO_FACILITY: "{1}に{0}があります" diff --git a/cabot_ui/scripts/cabot_ui_manager.py b/cabot_ui/scripts/cabot_ui_manager.py index d8a4591e..8dd309e6 100755 --- a/cabot_ui/scripts/cabot_ui_manager.py +++ b/cabot_ui/scripts/cabot_ui_manager.py @@ -146,7 +146,7 @@ def create_menu(self): except: # noqa: #722 self._logger.error(traceback.format_exc()) - # navigation delegate + # region NavigationInterface def activity_log(self, category="", text="", memo=""): self._interface.activity_log(category, text, memo) @@ -211,37 +211,15 @@ def passed_poi(self, poi=None): def could_not_get_current_location(self): self._interface.could_not_get_current_location() + def please_call_elevator(self, pos): + self._interface.please_call_elevator(pos) + def enter_goal(self, goal): self._interface.enter_goal(goal) def exit_goal(self, goal): self._interface.exit_goal(goal) - def announce_social(self, message): - self._logger.info(f"cabot_ui_manager.announce_social is called: {str(message)}") - sound_only = ["AVOIDING_AN_OBSTACLE", "AVOIDING_OBSTACLES"] - if message in sound_only: - e = NavigationEvent("sound", str(message)) - msg = std_msgs.msg.String() - msg.data = str(e) - self._eventPub.publish(msg) - else: - self._interface.announce_social(message) - - def speak_stop_reason(self, code): - self._logger.info(f"cabot_ui_manager.speak_stop_reason is called: {str(code)}") - sound_only = [StopReason.AVOIDING_OBSTACLE] - if code in sound_only: - e = NavigationEvent("sound", str(code)) - msg = std_msgs.msg.String() - msg.data = str(e) - self._eventPub.publish(msg) - else: - self._interface.speak_stop_reason(code) - - def please_call_elevator(self, pos): - self._interface.please_call_elevator(pos) - def elevator_opening(self): self._interface.elevator_opening() @@ -269,7 +247,13 @@ def please_follow_behind(self): def please_return_position(self): self._interface.please_return_position() - ### + def announce_social(self, message): + self._interface.announce_social(message) + + def request_sound(self, sound): + self._interface.request_sound(sound) + # endregion NavigationInterface + def _event_callback(self, msg): event = BaseEvent.parse(msg.data) if event is None: @@ -530,11 +514,6 @@ def done_callback(): self._interface.set_pause_control(False) self._navigation.set_pause_control(False) - if event.subtype == "stop-reason": - if self._status_manager.state == State.in_action: - code = StopReason[event.param] - self._navigation.process_stop_reason(code) - def _process_exploration_event(self, event): if event.type != ExplorationEvent.TYPE: return diff --git a/cabot_ui/src/stop_reasoner.cpp b/cabot_ui/src/stop_reasoner.cpp index c870d16d..12ade046 100644 --- a/cabot_ui/src/stop_reasoner.cpp +++ b/cabot_ui/src/stop_reasoner.cpp @@ -315,7 +315,7 @@ std::tuple StopReasoner::update() get_current_time().nanoseconds() / 1e9, people_speed_.minimum(get_current_time()), people_speed_.average(get_current_time())); - return std::make_tuple(duration, StopReason::THERE_ARE_PEOPLE_ON_THE_PATH); + return std::make_tuple(duration, StopReason::THERE_ARE_PEOPLE_IN_THE_PATH); } } diff --git a/cabot_ui/src/stop_reasoner.hpp b/cabot_ui/src/stop_reasoner.hpp index ddd99c67..1a15d89b 100644 --- a/cabot_ui/src/stop_reasoner.hpp +++ b/cabot_ui/src/stop_reasoner.hpp @@ -49,7 +49,7 @@ enum class StopReason NO_TOUCH, STOPPED_BUT_UNDER_THRESHOLD, NO_CMD_VEL, - THERE_ARE_PEOPLE_ON_THE_PATH, + THERE_ARE_PEOPLE_IN_THE_PATH, WAITING_FOR_ELEVATOR, AVOIDING_OBSTACLE, AVOIDING_PEOPLE, @@ -69,7 +69,7 @@ class StopReasonUtil case StopReason::NO_TOUCH: return "NO_TOUCH"; case StopReason::STOPPED_BUT_UNDER_THRESHOLD: return "STOPPED_BUT_UNDER_THRESHOLD"; case StopReason::NO_CMD_VEL: return "NO_CMD_VEL"; - case StopReason::THERE_ARE_PEOPLE_ON_THE_PATH: return "THERE_ARE_PEOPLE_ON_THE_PATH"; + case StopReason::THERE_ARE_PEOPLE_IN_THE_PATH: return "THERE_ARE_PEOPLE_IN_THE_PATH"; case StopReason::WAITING_FOR_ELEVATOR: return "WAITING_FOR_ELEVATOR"; case StopReason::AVOIDING_OBSTACLE: return "AVOIDING_OBSTACLE"; case StopReason::AVOIDING_PEOPLE: return "AVOIDING_PEOPLE"; diff --git a/cabot_ui/src/stop_reasons_node.cpp b/cabot_ui/src/stop_reasons_node.cpp index bd88328a..881a7edb 100644 --- a/cabot_ui/src/stop_reasons_node.cpp +++ b/cabot_ui/src/stop_reasons_node.cpp @@ -59,7 +59,6 @@ class StopReasonsNode : public rclcpp::Node prev_code_(cabot_ui::StopReason::NONE) { stop_reason_pub_ = this->create_publisher("/stop_reason", 10); - event_pub_ = this->create_publisher("/cabot/event", 10); odom_sub_ = this->create_subscription(ODOM_TOPIC, 10, std::bind(&StopReasonsNode::odom_callback, this, _1)); event_sub_ = this->create_subscription(EVENT_TOPIC, 10, std::bind(&StopReasonsNode::event_callback, this, _1)); cmd_vel_sub_ = this->create_subscription(CMD_VEL_TOPIC, 10, std::bind(&StopReasonsNode::cmd_vel_callback, this, _1)); @@ -160,20 +159,22 @@ class StopReasonsNode : public rclcpp::Node auto [duration, code] = reasoner_->update(); stop_reason_filter_->update(duration, code); std::tie(duration, code) = stop_reason_filter_->event(); - if (code != cabot_ui::StopReason::NONE) { cabot_msgs::msg::StopReason msg; msg.header.stamp = this->get_clock()->now(); msg.reason = cabot_ui::StopReasonUtil::toStr(code); msg.duration = duration; + msg.summary = false; stop_reason_pub_->publish(msg); } std::tie(duration, code) = stop_reason_filter_->summary(); if (code != cabot_ui::StopReason::NONE) { - std_msgs::msg::String msg; - msg.data = "navigation;stop-reason;" + cabot_ui::StopReasonUtil::toStr(code); - event_pub_->publish(msg); - RCLCPP_INFO(this->get_logger(), "%.2f, %s, %.2f", this->get_clock()->now().nanoseconds() / 1e9f, cabot_ui::StopReasonUtil::toStr(code).c_str(), duration); + cabot_msgs::msg::StopReason msg; + msg.header.stamp = this->get_clock()->now(); + msg.reason = cabot_ui::StopReasonUtil::toStr(code); + msg.duration = duration; + msg.summary = true; + stop_reason_pub_->publish(msg); } stop_reason_filter_->conclude(); } @@ -183,7 +184,6 @@ class StopReasonsNode : public rclcpp::Node std::shared_ptr stop_reason_filter_; cabot_ui::StopReason prev_code_; rclcpp::Publisher::SharedPtr stop_reason_pub_; - rclcpp::Publisher::SharedPtr event_pub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr event_sub_; rclcpp::Subscription::SharedPtr cmd_vel_sub_; From fb3deee1f033c358f8e6cc9b1b2a3b11c892cd9c Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 30 Mar 2024 21:11:36 -0400 Subject: [PATCH 05/35] update run_test.py to spawn obstacle Signed-off-by: Daisuke Sato --- cabot_navigation2/test/run_test.py | 133 ++++++++++++++++++++++++----- 1 file changed, 113 insertions(+), 20 deletions(-) diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py index 6e1cac0c..fed643ea 100755 --- a/cabot_navigation2/test/run_test.py +++ b/cabot_navigation2/test/run_test.py @@ -26,13 +26,15 @@ import inspect import sys import math +import numpy import time import traceback import uuid import yaml import logging import re - +from dataclasses import dataclass, fields +from typing import Any from optparse import OptionParser import rclpy import rclpy.node @@ -41,7 +43,8 @@ from tf_transformations import quaternion_from_euler from cabot_common.util import callee_name - +from people_msgs.msg import People, Person +from nav_msgs.msg import Path from geometry_msgs.msg import PoseWithCovarianceStamped from mf_localization_msgs.srv import StartLocalization, StopLocalization, MFSetInt from gazebo_msgs.srv import SetEntityState @@ -466,15 +469,25 @@ def clean_door(self, case, test_action): def done_callback(future): case['done'] = True - future = ElevatorDoorSimulator.instance().clean(callback=done_callback) + future = ObstacleManager.instance().clean(callback=done_callback) if not future: return self.futures[uuid] = future + @wait_test() + def spawn_obstacle(self, case, test_action): + uuid = test_action['uuid'] + self.futures[uuid] = ObstacleManager.instance().spawn_obstacle(**test_action) + + def done_callback(future): + logger.debug(future.result()) + case['done'] = True + self.futures[uuid].add_done_callback(done_callback) + @wait_test() def spawn_door(self, case, test_action): uuid = test_action['uuid'] - self.futures[uuid] = ElevatorDoorSimulator.instance().spawn_door(**test_action) + self.futures[uuid] = ObstacleManager.instance().spawn_door(**test_action) def done_callback(future): logger.debug(future.result()) @@ -484,7 +497,7 @@ def done_callback(future): @wait_test() def delete_door(self, case, test_action): uuid = test_action['uuid'] - self.futures[uuid] = ElevatorDoorSimulator.instance().delete_door(**test_action) + self.futures[uuid] = ObstacleManager.instance().delete_door(**test_action) def done_callback(future): logger.debug(future.result()) @@ -750,23 +763,87 @@ def terminate(self, test_action): sys.exit(0) -class ElevatorDoorSimulator: +@dataclass +class Door: + name: str + x: float + y: float + z: float + yaw: float + width: float + height: float + depth: float + + @staticmethod + def from_dict(**kwargs): + valid_fields = {field.name for field in fields(Door)} + filtered_kwargs = {k: v for k, v in kwargs.items() if k in valid_fields} + return Door(**filtered_kwargs) + + +class ObstacleManager: _instance = None @classmethod def instance(cls): - if not ElevatorDoorSimulator._instance: - ElevatorDoorSimulator._instance = ElevatorDoorSimulator() - return ElevatorDoorSimulator._instance + if not ObstacleManager._instance: + ObstacleManager._instance = ObstacleManager() + return ObstacleManager._instance def __init__(self): self.spawn_entity_client = node.create_client(SpawnEntity, '/spawn_entity') self.delete_entity_client = node.create_client(DeleteEntity, '/delete_entity') + self.plan_sub = node.create_subscription(Path, '/plan', self.plan_callback, 10) + self.obstacle_pub = node.create_publisher(People, '/obstacles', 10) + self.timer = node.create_timer(0.2, self.timer_callback) self.remaining = [] + self.last_plan = None + + def plan_callback(self, msg): + self.last_plan = msg + + def timer_callback(self): + if not self.last_plan: + return + obstacle_point = None + for pose in self.last_plan.poses: + for obstacle in self.remaining: + if self.is_point_in_rotated_rect(pose.pose.position, obstacle): + obstacle_point = pose.pose.position + break + if obstacle_point: + msg = Person() + msg.name = obstacle.name + msg.position.x = obstacle_point.x + msg.position.y = obstacle_point.y + msg.position.z = obstacle_point.z + msg.reliability = 1.0 + msg.tags.append("stationary") + pmsg = People() + pmsg.people.append(msg) + pmsg.header.stamp = node.get_clock().now().to_msg() + pmsg.header.frame_id = "map_global" + self.obstacle_pub.publish(pmsg) + + def is_point_in_rotated_rect(self, point, obstacle): + margin = 0.45 + # Convert yaw to radians + yaw = obstacle.yaw + # Translate point to origin based on rect position + translated_point_x = point.x - obstacle.x + translated_point_y = point.y - obstacle.y + # Rotate point around origin (0,0) in the opposite direction of the rectangle's rotation + cos_yaw, sin_yaw = numpy.cos(-yaw), numpy.sin(-yaw) + rotated_point_x = translated_point_x * cos_yaw - translated_point_y * sin_yaw + rotated_point_y = translated_point_x * sin_yaw + translated_point_y * cos_yaw + # Check if the rotated point is within the rectangle bounds + return -margin-obstacle.width / 2 <= rotated_point_x <= obstacle.width / 2 + margin and \ + -margin-obstacle.height / 2 <= rotated_point_y <= obstacle.height / 2 + margin def clean(self, callback): + self.last_path = None if self.remaining: - future = self.delete_door(name=self.ramaining[0]) + future = self.delete_door(name=self.ramaining[0].name) def done_callback(future): self.clean(callback) @@ -781,16 +858,30 @@ def delete_door(self, **kwargs): future = self.delete_entity_client.call_async(request) def callback(future): - self.remaining.remove(name) + self.remaining = [door for door in self.remaining if door.name != name] future.add_done_callback(callback) return future def spawn_door(self, **kwargs): - name = kwargs['name'] - x = kwargs['x'] - y = kwargs['y'] - z = kwargs['z'] - yaw = kwargs['yaw'] + return self.spawn_obstacle(**dict( + dict( + width=0.01, + height=2.0, + depth=2.0 + ), + **kwargs) + ) + + def spawn_obstacle(self, **kwargs): + door = Door.from_dict(**kwargs) + name = door.name + x = door.x + y = door.y + z = door.z + yaw = door.yaw + width = door.width + height = door.height + depth = door.depth door_xml = f""" @@ -798,18 +889,18 @@ def spawn_door(self, **kwargs): true - {x} {y} {z+1} 0 {math.pi/2} {yaw} + {x} {y} {z+depth/2.0} 0 0 {yaw} - 2 2 0.01 + {width} {height} {depth} - 2 2 0.01 + {width} {height} {depth} @@ -825,7 +916,8 @@ def spawn_door(self, **kwargs): future = self.spawn_entity_client.call_async(request) def callback(future): - self.remaining.append(name) + self.remaining.append(door) + logger.debug(F"spawn result = {future.result()}, {door}, {len(self.remaining)}") future.add_done_callback(callback) return future @@ -912,6 +1004,7 @@ def main(): rclpy.init() node = rclpy.node.Node("test_node") manager = PedestrianManager(node) + ObstacleManager.instance() ros2Handler = ROS2LogHandler(node) logger.addHandler(ros2Handler) From 7c89f997fae0b9a3f5eb998607dee558116c9b75 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 30 Mar 2024 22:52:41 -0400 Subject: [PATCH 06/35] improve turn detector Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navigation.py | 9 ++++++--- cabot_ui/cabot_ui/turn_detector.py | 14 +++++++++++++- 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index cf9a90a6..35da59e2 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -867,10 +867,11 @@ def _check_turn(self, current_pose): self._logger.info("check turn", throttle_duration_sec=1) if self.turns is not None: for turn in self.turns: + if turn.passed: + continue try: - turn_pose = self.buffer.transform(turn.pose, self._global_map_name) - dist = current_pose.distance_to(geoutil.Point(xy=turn_pose.pose.position)) - if dist < 0.25 and not turn.passed: + dist = turn.distance_to(current_pose) + if dist < 0.25: turn.passed = True self._logger.info(F"notify turn {turn}") @@ -978,8 +979,10 @@ def _check_social(self, current_pose): # do not provide social navigation messages while queue navigation # do not provide social navigation messages while narrow/tight if self._current_goal and not self._current_goal.is_social_navigation_enabled: + self._logger.info("social navigation is disabled") return if self._current_goal and not self._current_goal.is_stop_reason_enabled: + self._logger.info("stop reason is disabled") return self.social_navigation.current_pose = current_pose diff --git a/cabot_ui/cabot_ui/turn_detector.py b/cabot_ui/cabot_ui/turn_detector.py index 0f474403..efa671ff 100644 --- a/cabot_ui/cabot_ui/turn_detector.py +++ b/cabot_ui/cabot_ui/turn_detector.py @@ -28,6 +28,7 @@ import matplotlib.pyplot as plt from scipy import signal from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil +from cabot_ui import geoutil smoothParam = 0.015 lookAheadStepsA = int(2/0.02) @@ -74,7 +75,18 @@ def __repr__(self): return self.__str__() def distance_to(self, other): - return np.sqrt((self.start.x - other.start.x) ** 2 + (self.start.y - other.start.y) ** 2) + if isinstance(other, Turn): + return np.sqrt((self.start.x - other.start.x) ** 2 + (self.start.y - other.start.y) ** 2) + elif isinstance(other, geoutil.Pose): + pose = geoutil.Pose(pose_msg=self.pose) + pose.r = pose.r + math.pi + dist_TR = other.distance_to(pose) + pose_TR = geoutil.Pose.pose_from_points(pose, other) + yaw = geoutil.diff_angle(pose.orientation, pose_TR.orientation) + adjusted = dist_TR * math.cos(yaw) + CaBotRclpyUtil.debug(f"dist={dist_TR}, yaw={yaw}, adjusted={adjusted}") + return adjusted + class TurnDetector: From 658b67f890163d1ddf9a27b626bd0a38bdbe33a2 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 30 Mar 2024 22:57:57 -0400 Subject: [PATCH 07/35] fix lint errors Signed-off-by: Daisuke Sato --- cabot_navigation2/test/run_test.py | 1 - cabot_ui/cabot_ui/interface.py | 1 + cabot_ui/cabot_ui/social_navigation.py | 1 + cabot_ui/cabot_ui/turn_detector.py | 1 - cabot_ui/scripts/cabot_ui_manager.py | 1 - 5 files changed, 2 insertions(+), 3 deletions(-) diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py index fed643ea..7c537372 100755 --- a/cabot_navigation2/test/run_test.py +++ b/cabot_navigation2/test/run_test.py @@ -34,7 +34,6 @@ import logging import re from dataclasses import dataclass, fields -from typing import Any from optparse import OptionParser import rclpy import rclpy.node diff --git a/cabot_ui/cabot_ui/interface.py b/cabot_ui/cabot_ui/interface.py index 952cde26..e3e18c62 100644 --- a/cabot_ui/cabot_ui/interface.py +++ b/cabot_ui/cabot_ui/interface.py @@ -35,6 +35,7 @@ from cabot_ui.social_navigation import SNMessage from cabot_common import vibration + class UserInterface(object): SOCIAL_ANNOUNCE_INTERVAL = Duration(seconds=15.0) NOTIFY_TURN_INTERVAL = Duration(seconds=5.0) diff --git a/cabot_ui/cabot_ui/social_navigation.py b/cabot_ui/cabot_ui/social_navigation.py index 20397e0e..3e242ab7 100644 --- a/cabot_ui/cabot_ui/social_navigation.py +++ b/cabot_ui/cabot_ui/social_navigation.py @@ -32,6 +32,7 @@ from dataclasses import dataclass import enum + @dataclass class SNMessage: class Type(enum.Enum): diff --git a/cabot_ui/cabot_ui/turn_detector.py b/cabot_ui/cabot_ui/turn_detector.py index efa671ff..16a28d06 100644 --- a/cabot_ui/cabot_ui/turn_detector.py +++ b/cabot_ui/cabot_ui/turn_detector.py @@ -88,7 +88,6 @@ def distance_to(self, other): return adjusted - class TurnDetector: def __init__(self): pass diff --git a/cabot_ui/scripts/cabot_ui_manager.py b/cabot_ui/scripts/cabot_ui_manager.py index 8dd309e6..b1f29b43 100755 --- a/cabot_ui/scripts/cabot_ui_manager.py +++ b/cabot_ui/scripts/cabot_ui_manager.py @@ -56,7 +56,6 @@ from cabot_ui.status import State, StatusManager from cabot_ui.interface import UserInterface from cabot_ui.navigation import Navigation, NavigationInterface -from cabot_ui.stop_reasoner import StopReason from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil from diagnostic_updater import Updater, FunctionDiagnosticTask From c3c1a706c7af37a95af14ac387e5eaf8803c6d82 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sun, 31 Mar 2024 20:17:14 -0400 Subject: [PATCH 08/35] implement CaBotRotationShimController Signed-off-by: Daisuke Sato --- cabot_navigation2/CMakeLists.txt | 47 ++++-- cabot_navigation2/controllers_plugins.xml | 30 ++++ .../cabot_rotation_shim_controller.hpp | 41 ++++++ cabot_navigation2/params/nav2_params.yaml | 8 +- .../cabot_rotation_shim_controller.cpp | 134 ++++++++++++++++++ 5 files changed, 249 insertions(+), 11 deletions(-) create mode 100644 cabot_navigation2/controllers_plugins.xml create mode 100644 cabot_navigation2/include/cabot_navigation2/cabot_rotation_shim_controller.hpp create mode 100644 cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp diff --git a/cabot_navigation2/CMakeLists.txt b/cabot_navigation2/CMakeLists.txt index e434e18b..d4419d8c 100644 --- a/cabot_navigation2/CMakeLists.txt +++ b/cabot_navigation2/CMakeLists.txt @@ -12,7 +12,7 @@ if(NOT CMAKE_CXX_STANDARD) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -Wfatal-errors) endif() # find dependencies @@ -21,6 +21,7 @@ find_package(geometry_msgs REQUIRED) find_package(nav2_lifecycle_manager REQUIRED) find_package(nav2_costmap_2d REQUIRED) find_package(nav2_core REQUIRED) +find_package(nav2_rotation_shim_controller REQUIRED) find_package(angles REQUIRED) find_package(nav_msgs REQUIRED) find_package(pluginlib REQUIRED) @@ -69,8 +70,6 @@ set(dependencies people_msgs queue_msgs sensor_msgs - dwb_core - dwb_critics diagnostic_updater yaml_cpp_vendor # OpenMP @@ -86,7 +85,8 @@ add_library(cabot_critics SHARED target_compile_definitions(cabot_critics PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(cabot_critics - ${dependencies} + dwb_core + dwb_critics ) ### goal checker @@ -98,7 +98,8 @@ add_library(cabot_goals SHARED target_compile_definitions(cabot_goals PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(cabot_goals - ${dependencies} + nav2_core + pluginlib ) ### costmap layers @@ -111,7 +112,10 @@ add_library(cabot_layers SHARED target_compile_definitions(cabot_layers PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(cabot_layers - ${dependencies} + nav2_costmap_2d + nav2_core + people_msgs + pluginlib ) ### planner plugin @@ -126,7 +130,13 @@ add_library(cabot_planners SHARED target_compile_definitions(cabot_planners PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") ament_target_dependencies(cabot_planners - ${dependencies} + angles + nav2_core + people_msgs + pluginlib + queue_msgs + rclcpp + tf2 ) target_link_libraries(cabot_planners @@ -140,7 +150,9 @@ add_executable(cabot_scan ) ament_target_dependencies(cabot_scan - ${dependencies} + diagnostic_updater + rclcpp + sensor_msgs ) ### cabot lifecycle manager @@ -148,7 +160,17 @@ add_executable(cabot_lifecycle_manager src/cabot_lifecycle_manager.cpp ) ament_target_dependencies(cabot_lifecycle_manager - ${dependencies} + nav2_lifecycle_manager +) + +### controller plugin +add_library(cabot_controllers SHARED + plugins/cabot_rotation_shim_controller.cpp +) +ament_target_dependencies(cabot_controllers + nav2_core + nav2_rotation_shim_controller + pluginlib ) install(TARGETS @@ -156,6 +178,7 @@ install(TARGETS cabot_goals cabot_layers cabot_planners + cabot_controllers cabot_scan cabot_lifecycle_manager ARCHIVE DESTINATION lib @@ -168,6 +191,7 @@ install(FILES goal_plugins.xml layers_plugins.xml planners_plugins.xml + controllers_plugins.xml DESTINATION share/${PROJECT_NAME} ) @@ -187,13 +211,16 @@ ament_export_libraries( cabot_critics cabot_goals cabot_layers - cabot_planners) + cabot_planners + cabot_controllers +) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(dwb_core critics_plugins.xml) pluginlib_export_plugin_description_file(nav2_core goal_plugins.xml) pluginlib_export_plugin_description_file(nav2_costmap_2d layers_plugins.xml) pluginlib_export_plugin_description_file(nav2_core planners_plugins.xml) +pluginlib_export_plugin_description_file(nav2_core controllers_plugins.xml) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/cabot_navigation2/controllers_plugins.xml b/cabot_navigation2/controllers_plugins.xml new file mode 100644 index 00000000..cbd80076 --- /dev/null +++ b/cabot_navigation2/controllers_plugins.xml @@ -0,0 +1,30 @@ + + + + + + Custmozied Rotation Shim Controller + + + diff --git a/cabot_navigation2/include/cabot_navigation2/cabot_rotation_shim_controller.hpp b/cabot_navigation2/include/cabot_navigation2/cabot_rotation_shim_controller.hpp new file mode 100644 index 00000000..170533ff --- /dev/null +++ b/cabot_navigation2/include/cabot_navigation2/cabot_rotation_shim_controller.hpp @@ -0,0 +1,41 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + +#ifndef CABOT_NAVIGATION2__CABOT_ROTATION_SHIM_CONTROLLER_HPP_ +#define CABOT_NAVIGATION2__CABOT_ROTATION_SHIM_CONTROLLER_HPP_ +#include + +namespace cabot_navigation2 +{ +class CaBotRotationShimController : public nav2_rotation_shim_controller::RotationShimController +{ +public: + CaBotRotationShimController(); + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * goal_checker) override; + +protected: + geometry_msgs::msg::PoseStamped getNearestPathPt(const geometry_msgs::msg::PoseStamped & pose); + geometry_msgs::msg::Pose transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt); +}; +} // namespace cabot_navigation2 +#endif // CABOT_NAVIGATION2__CABOT_ROTATION_SHIM_CONTROLLER_HPP_ diff --git a/cabot_navigation2/params/nav2_params.yaml b/cabot_navigation2/params/nav2_params.yaml index 652ce88a..caa90133 100644 --- a/cabot_navigation2/params/nav2_params.yaml +++ b/cabot_navigation2/params/nav2_params.yaml @@ -184,7 +184,13 @@ controller_server: stateful: True # DWB parameters FollowPath: - plugin: "dwb_core::DWBLocalPlanner" + plugin: "cabot_navigation2::CaBotRotationShimController" + primary_controller: "dwb_core::DWBLocalPlanner" + angular_dist_threshold: 0.5 + forward_sampling_distance: 1.0 + rotate_to_heading_angular_vel: 0.8 + max_angular_accel: 1.6 + simulate_ahead_time: 0.5 #trajectory_generator_name: "dwb_plugins::LimitedAccelGenerator" # default: dwb_plugins::StandardTrajectoryGenerator trajectory_generator_name: "dwb_plugins::StandardTrajectoryGenerator" debug_trajectory_details: True diff --git a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp new file mode 100644 index 00000000..b66cac43 --- /dev/null +++ b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp @@ -0,0 +1,134 @@ +// Copyright (c) 2024 Carnegie Mellon University +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include +#include +PLUGINLIB_EXPORT_CLASS(cabot_navigation2::CaBotRotationShimController, nav2_core::Controller) + +namespace cabot_navigation2 +{ + +CaBotRotationShimController::CaBotRotationShimController() +: nav2_rotation_shim_controller::RotationShimController() +{ +} + +geometry_msgs::msg::TwistStamped CaBotRotationShimController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity, + nav2_core::GoalChecker * goal_checker) +{ + auto node = node_.lock(); + + if (path_updated_) { + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + + std::lock_guard lock_reinit(mutex_); + try { + geometry_msgs::msg::Pose sampled_pt_base = transformPoseToBaseFrame(getNearestPathPt(pose)); + + double angular_distance_to_heading = + std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); + if (fabs(angular_distance_to_heading) > angular_dist_threshold_) { + RCLCPP_DEBUG( + logger_, + "Robot is not within the new path's rough heading, rotating to heading..."); + return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + } else { + RCLCPP_DEBUG( + logger_, + "Robot is at the new path's rough heading, passing to controller"); + path_updated_ = false; + } + } catch (const std::runtime_error & e) { + RCLCPP_DEBUG( + logger_, + "Rotation Shim Controller was unable to find a sampling point," + " a rotational collision was detected, or TF failed to transform" + " into base frame! what(): %s", e.what()); + path_updated_ = false; + } + } + + // If at this point, use the primary controller to path track + return primary_controller_->computeVelocityCommands(pose, velocity, goal_checker); +} + +geometry_msgs::msg::PoseStamped CaBotRotationShimController::getNearestPathPt(const geometry_msgs::msg::PoseStamped & pose) +{ + if (current_path_.poses.size() < 2) { + throw nav2_core::PlannerException( + "Path is too short to find a valid sampled path point for rotation."); + } + + // find the nearest pose in the path from the current robot pose + double dx, dy, dist; + double mind = 10000; + unsigned int mini; + for (unsigned int i = 1; i != current_path_.poses.size(); i++) { + dx = current_path_.poses[i].pose.position.x - pose.pose.position.x; + dy = current_path_.poses[i].pose.position.y - pose.pose.position.y; + dist = hypot(dx, dy); + if (dist < mind) { + mind = dist; + mini = i; + } + } + + RCLCPP_INFO( + node_.lock()->get_logger(), "minimum distance %ld %.2f (%.2f, %.2f) - (%.2f, %.2f)", + mini, mind, current_path_.poses[mini].pose.position.x, current_path_.poses[mini].pose.position.y, + pose.pose.position.x, pose.pose.position.y); + + geometry_msgs::msg::Pose start = current_path_.poses.front().pose; + // Find the first point at least sampling distance away from the nearest point + for (unsigned int i = mini; i != current_path_.poses.size(); i++) { + dx = current_path_.poses[i].pose.position.x - start.position.x; + dy = current_path_.poses[i].pose.position.y - start.position.y; + if (hypot(dx, dy) >= forward_sampling_distance_) { + current_path_.poses[i].header.frame_id = current_path_.header.frame_id; + current_path_.poses[i].header.stamp = clock_->now(); // Get current time transformation + RCLCPP_INFO( + node_.lock()->get_logger(), "return %ld %.2f (%.2f, %.2f) - (%.2f, %.2f)", + i, hypot(dx, dy), current_path_.poses[i].pose.position.x, current_path_.poses[i].pose.position.y, + pose.pose.position.x, pose.pose.position.y); + return current_path_.poses[i]; + } + } + + throw nav2_core::PlannerException( + std::string( + "Unable to find a sampling point at least %0.2f from the robot," + "passing off to primary controller plugin.", forward_sampling_distance_)); +} + +geometry_msgs::msg::Pose +CaBotRotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt) +{ + auto node = node_.lock(); + geometry_msgs::msg::PoseStamped pt_base; + if (!nav2_util::transformPoseInTargetFrame(pt, pt_base, *tf_, costmap_ros_->getBaseFrameID(), 1.0)) { + throw nav2_core::PlannerException("Failed to transform pose to base frame!"); + } + return pt_base.pose; +} +} // namespace cabot_navigation2 From 4d2350bd3b69d5b6c16d0c54f73580528d33bf21 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sun, 31 Mar 2024 20:54:51 -0400 Subject: [PATCH 09/35] separate Narrow/Tight by corners Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navgoal.py | 20 +++++++++++++++++++- cabot_ui/cabot_ui/navigation.py | 5 ++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 1625770d..a1d75254 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -18,6 +18,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. +from typing import List import math import inspect from itertools import groupby @@ -734,7 +735,8 @@ def __init__(self, delegate, navcog_route, anchor, target_poi=None, set_back=(0, self.separated_route = [navcog_route] self.navcog_routes = [create_ros_path(navcog_route, self.anchor, self.global_map_name, target_poi=target_poi, set_back=set_back)] else: - self.separated_route = [list(group) for _, group in groupby(navcog_route, key=lambda x: x.navigation_mode)] + self.separated_route = self.separate_route(navcog_route) + # self.separated_route = [list(group) for _, group in groupby(navcog_route, key=lambda x: x.navigation_mode)] self.navcog_routes = [ create_ros_path( route, @@ -752,6 +754,22 @@ def __init__(self, delegate, navcog_route, anchor, target_poi=None, set_back=(0, self.route_index = 0 super(NavGoal, self).__init__(delegate, angle=180, floor=navcog_route[-1].floor, pose_msg=last_pose, **kwargs) + def separate_route(self, route: List[geojson.RouteLink]) -> List[List[geojson.RouteLink]]: + separated_routes = [] + current_group = [route[0]] + for i in range(1, len(route)): + current_link = route[i] + previous_link = route[i - 1] + orientation_diff = math.fabs(geoutil.diff_angle(current_link.pose.orientation, previous_link.pose.orientation)) + if current_link.navigation_mode != previous_link.navigation_mode or \ + (previous_link.navigation_mode != geojson.NavigationMode.Standard and orientation_diff > 80.0 / 180.0 * math.pi): + separated_routes.append(current_group) + current_group = [current_link] + else: + current_group.append(current_link) + separated_routes.append(current_group) # Add the last group + return separated_routes + @property def is_social_navigation_enabled(self): return self.navcog_routes[self.route_index][2] == geojson.NavigationMode.Standard diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 35da59e2..44cfe4be 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -742,7 +742,10 @@ def _stop_loop(self): def _process_queue_func(self): if len(self._process_queue) > 0: process = self._process_queue.pop(0) - process[0](*process[1:]) + try: + process[0](*process[1:]) + except: + self._logger.error(traceback.format_exc()) # Main loop of navigation GOAL_POSITION_TORELANCE = 1 From 5831712b6dd1ace89c228ae4d7359ac5ee7b71dd Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Mon, 1 Apr 2024 01:55:46 -0400 Subject: [PATCH 10/35] fix rotation shim controller logic Signed-off-by: Daisuke Sato --- cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp index b66cac43..43eede11 100644 --- a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp +++ b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp @@ -48,7 +48,7 @@ geometry_msgs::msg::TwistStamped CaBotRotationShimController::computeVelocityCom double angular_distance_to_heading = std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); - if (fabs(angular_distance_to_heading) > angular_dist_threshold_) { + if (fabs(angular_distance_to_heading) > angular_dist_threshold_ && velocity.linear.x < 0.1) { RCLCPP_DEBUG( logger_, "Robot is not within the new path's rough heading, rotating to heading..."); From f9771511207a08201fd55b6faaf2e5830ce5bf8a Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Mon, 1 Apr 2024 01:57:45 -0400 Subject: [PATCH 11/35] add custom rosbridge_suite Signed-off-by: Daisuke Sato --- dependency.repos | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/dependency.repos b/dependency.repos index 14d689aa..7c4f4d56 100644 --- a/dependency.repos +++ b/dependency.repos @@ -37,3 +37,7 @@ repositories: url: https://github.com/CMU-cabot/cabot-description version: main + docker/home/ros2_ws/src/rosbridge_suite: + type: git + url: https://github.com/CMU-cabot/rosbridge_suite + version: ros2-dev From 7b003ebaea85ec53a1c7b06a9e021f187b056a93 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Mon, 1 Apr 2024 13:16:01 -0400 Subject: [PATCH 12/35] exclude rosbridge_suite from unittest Signed-off-by: Daisuke Sato --- script/unittest.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/script/unittest.sh b/script/unittest.sh index b67d7a38..3c743efb 100755 --- a/script/unittest.sh +++ b/script/unittest.sh @@ -12,6 +12,7 @@ function help() package= all= build= +exclude="(rosbridge.*|rosapi)" while getopts "hp:ab" arg; do case $arg in @@ -38,7 +39,7 @@ fi if [[ $all -eq 1 ]]; then ([[ -z $build ]] || colcon build) && \ - colcon test && \ + colcon test --packages-ignore-regex "$exclude" && \ colcon test-result --verbose else ([[ -z $build ]] || colcon build --packages-up-to $package) && \ From 901228182682e533334da564d919b124cafbf4de Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Mon, 1 Apr 2024 16:36:17 -0400 Subject: [PATCH 13/35] adjust reading facility entrance Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/geojson.py | 22 ++++++++++++++++++++-- cabot_ui/cabot_ui/geoutil.py | 2 +- cabot_ui/cabot_ui/navgoal.py | 3 +-- cabot_ui/cabot_ui/navigation.py | 2 +- cabot_ui/i18n/en.yaml | 7 ++++--- cabot_ui/i18n/ja.yaml | 1 + 6 files changed, 28 insertions(+), 9 deletions(-) diff --git a/cabot_ui/cabot_ui/geojson.py b/cabot_ui/cabot_ui/geojson.py index 51689f33..00c92894 100644 --- a/cabot_ui/cabot_ui/geojson.py +++ b/cabot_ui/cabot_ui/geojson.py @@ -600,7 +600,7 @@ def __init__(self, facility, i, node): self.node_id = node._id self._id = f"{facility._id}_ent{i}" self.name = i18n.localized_attr(facility.properties, f"ent{i}_n") - super(Entrance, self).__init__(r=0, x=0, y=0, angle=45, floor=self.floor) + super(Entrance, self).__init__(r=0, x=0, y=0, angle=60, floor=self.floor) Object._register(self) @property @@ -626,13 +626,31 @@ def approached_statement(self): diff = geoutil.q_diff(self.quaternion, p1) _, _, angle = euler_from_quaternion(diff) CaBotRclpyUtil.debug(f"Entrance.approacehd_statement {diff} {angle}") - direction = "RIGHT_SIDE" if angle < 0 else "LEFT_SIDE" + if math.fabs(angle) < math.pi / 4: + direction = "IN_FRONT" + else: + direction = "RIGHT_SIDE" if angle < 0 else "LEFT_SIDE" i18n_direction = i18n.localized_string(direction) return i18n.localized_string("APPROACEHD_TO_FACILITY").format(self.facility.name, i18n_direction) def passed_statement(self): return None + # distance is adjusted to by the TargetPoint orientation + # <- T Target (orientation) + # | + # | + # robot R ---distance--- o + # + # adjusted = True + def distance_to(self, robot): + dist_TR = robot.distance_to(self) + pose_TR = geoutil.Pose.pose_from_points(self, robot) + yaw = geoutil.diff_angle(self.orientation, pose_TR.orientation) + adjusted = dist_TR * math.cos(yaw) + CaBotRclpyUtil.info(f"Entrance.distance_to dist={dist_TR}, yaw={yaw}, adjusted={adjusted}") + return adjusted + class Facility(Object): """Facility class""" diff --git a/cabot_ui/cabot_ui/geoutil.py b/cabot_ui/cabot_ui/geoutil.py index 977cbf0d..9a7f8e16 100644 --- a/cabot_ui/cabot_ui/geoutil.py +++ b/cabot_ui/cabot_ui/geoutil.py @@ -539,7 +539,7 @@ def is_passed(self, pose): if self._was_approached and \ self._pose_approached is not None and \ self._pose_approached.distance_to(pose) > TargetPlace.PASSED_THRETHOLD and \ - self.distance_to(pose) > TargetPlace.PASSED_THRETHOLD: + math.fabs(self.distance_to(pose)) > TargetPlace.PASSED_THRETHOLD: self._was_passed = True self._pose_passed = pose return True diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index a1d75254..7eafd663 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -21,7 +21,6 @@ from typing import List import math import inspect -from itertools import groupby import numpy import time import traceback @@ -762,7 +761,7 @@ def separate_route(self, route: List[geojson.RouteLink]) -> List[List[geojson.Ro previous_link = route[i - 1] orientation_diff = math.fabs(geoutil.diff_angle(current_link.pose.orientation, previous_link.pose.orientation)) if current_link.navigation_mode != previous_link.navigation_mode or \ - (previous_link.navigation_mode != geojson.NavigationMode.Standard and orientation_diff > 80.0 / 180.0 * math.pi): + (previous_link.navigation_mode != geojson.NavigationMode.Standard and orientation_diff > 80.0 / 180.0 * math.pi): separated_routes.append(current_group) current_group = [current_link] else: diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 44cfe4be..262cb65c 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -744,7 +744,7 @@ def _process_queue_func(self): process = self._process_queue.pop(0) try: process[0](*process[1:]) - except: + except: # noqa: 722 self._logger.error(traceback.format_exc()) # Main loop of navigation diff --git a/cabot_ui/i18n/en.yaml b/cabot_ui/i18n/en.yaml index 0ecbe671..3562b605 100644 --- a/cabot_ui/i18n/en.yaml +++ b/cabot_ui/i18n/en.yaml @@ -110,6 +110,7 @@ RETURN_TO_POSITION_PLEASE: "Passed through a narrow road. return to first positi NOT_DETECT_TOUCH: "Touch on the handle is not detected" ## Facility -RIGHT_SIDE: "right" -LEFT_SIDE: "left" -APPROACEHD_TO_FACILITY: "{0} is on your {1}" +IN_FRONT: "in front of you" +RIGHT_SIDE: "on your right" +LEFT_SIDE: "on your left" +APPROACEHD_TO_FACILITY: "{0} is {1}" diff --git a/cabot_ui/i18n/ja.yaml b/cabot_ui/i18n/ja.yaml index 44d0c097..4084377b 100644 --- a/cabot_ui/i18n/ja.yaml +++ b/cabot_ui/i18n/ja.yaml @@ -110,6 +110,7 @@ RETURN_TO_POSITION_PLEASE: "細い道を抜けました。元の位置に戻っ NOT_DETECT_TOUCH: "ハンドルのタッチを検出できません。" ## Facility +IN_FRONT: "正面" RIGHT_SIDE: "右側" LEFT_SIDE: "左側" APPROACEHD_TO_FACILITY: "{1}に{0}があります" From 706c20d62d426b1e328b5c77f5c10cc0d896e3e1 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Mon, 1 Apr 2024 21:08:18 -0400 Subject: [PATCH 14/35] change log output of cabot_ext_ble.launch.xml from screen to log Signed-off-by: Daisuke Sato --- cabot_ui/launch/cabot_ext_ble.launch.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cabot_ui/launch/cabot_ext_ble.launch.xml b/cabot_ui/launch/cabot_ext_ble.launch.xml index e01b251e..46888486 100644 --- a/cabot_ui/launch/cabot_ext_ble.launch.xml +++ b/cabot_ui/launch/cabot_ext_ble.launch.xml @@ -23,13 +23,13 @@ + name="rosbridge_server" output="log"> + name="tf2_web_republisher" output="log"> From 6ef4950a485865da1616ee8f9e94127d53ba1624 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 2 Apr 2024 15:54:09 -0400 Subject: [PATCH 15/35] update cabot rotation shim controller logic to avoid over turning in last step positions Signed-off-by: Daisuke Sato --- cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp index 43eede11..d70453fe 100644 --- a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp +++ b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp @@ -48,7 +48,9 @@ geometry_msgs::msg::TwistStamped CaBotRotationShimController::computeVelocityCom double angular_distance_to_heading = std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); - if (fabs(angular_distance_to_heading) > angular_dist_threshold_ && velocity.linear.x < 0.1) { + double distance_to_heading = hypot(sampled_pt_base.position.x, sampled_pt_base.position.y); + if (distance_to_heading >= forward_sampling_distance_ && + fabs(angular_distance_to_heading) > angular_dist_threshold_ && velocity.linear.x < 0.1) { RCLCPP_DEBUG( logger_, "Robot is not within the new path's rough heading, rotating to heading..."); From e673abc32e2be227d8577da2c61e39d44d838ec6 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 3 Apr 2024 09:10:16 -0400 Subject: [PATCH 16/35] add origin_x, origin_y setting for run_test Signed-off-by: Daisuke Sato --- cabot_navigation2/test/run_test.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py index 7c537372..0e561d91 100755 --- a/cabot_navigation2/test/run_test.py +++ b/cabot_navigation2/test/run_test.py @@ -585,13 +585,15 @@ def done_stop_localization_callback(future): # change gazebo model position request = SetEntityState.Request() request.state.name = 'mobile_base' + origin_x = test_action['origin_x'] if 'origin_x' in test_action else self.config['origin_x'] if 'origin_x' in self.config else 0 + origin_y = test_action['origin_y'] if 'origin_y' in test_action else self.config['origin_y'] if 'origin_y' in self.config else 0 init_x = test_action['x'] if 'x' in test_action else self.config['init_x'] init_y = test_action['y'] if 'y' in test_action else self.config['init_y'] init_z = test_action['z'] if 'z' in test_action else self.config['init_z'] init_a = test_action['a'] if 'a' in test_action else self.config['init_a'] init_yaw = init_a / 180.0 * math.pi - request.state.pose.position.x = float(init_x) - request.state.pose.position.y = float(init_y) + request.state.pose.position.x = float(init_x + origin_x) + request.state.pose.position.y = float(init_y + origin_y) request.state.pose.position.z = float(init_z) q = quaternion_from_euler(0, 0, init_yaw) request.state.pose.orientation.x = q[0] From ac2bf6ec36e63e15b2176edf6c4cc92b1be6ec9e Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 6 Apr 2024 08:52:15 -0400 Subject: [PATCH 17/35] implement velocity obstacle for people Signed-off-by: Daisuke Sato --- .../src/safety/people_speed_control_node.cpp | 54 +++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/cabot/src/safety/people_speed_control_node.cpp b/cabot/src/safety/people_speed_control_node.cpp index 01a8a3f3..72bcb049 100644 --- a/cabot/src/safety/people_speed_control_node.cpp +++ b/cabot/src/safety/people_speed_control_node.cpp @@ -261,7 +261,7 @@ class PeopleSpeedControlNode : public rclcpp::Node geometry_msgs::msg::TransformStamped transform_msg; try { transform_msg = tfBuffer->lookupTransform( - "base_footprint", "map", + "base_footprint", input->header.frame_id, rclcpp::Time(0), rclcpp::Duration(std::chrono::duration(1.0))); } catch (tf2::TransformException & ex) { RCLCPP_WARN(get_logger(), "%s", ex.what()); @@ -286,14 +286,51 @@ class PeopleSpeedControlNode : public rclcpp::Node double y = p_local.y(); double vx = v_local.x(); double vy = v_local.y(); - double dist = sqrt(x * x + y * y); + double vr = last_odom_.twist.twist.linear.x; + + // velocity obstacle + double RPy = atan2(y, x); + double dist = hypot(x, y); + if (vx > 0 || vy > 0) { + double pr = 1.0; + double s = atan2(pr, dist) + M_PI_2; + double Px1 = x + cos(RPy+s) * pr; + double Py1 = y + sin(RPy+s) * pr; + double Px2 = x + cos(RPy-s) * pr; + double Py2 = y + sin(RPy-s) * pr; + double v1 = vy / -(Py1 / Px1); + double t1 = -Py1 / vy; + double v2 = vy / -(Py2 / Px2); + double t2 = -Py2 / vy; + bool swapped = false; + if ((0 < t1 && t1 < 5) || (0 < t2 && t2 < 5)) { + v1 -= vx; + v2 -= vx; + if (t1 < t2) { + double temp = v1; + v1 = v2; + v2 = temp; + temp = t1; + t1 = t2; + t2 = temp; + swapped = true; + } + speed_limit = std::max(0.0, v1); + if (v1 < v2 && v2 < 1.0) { + speed_limit = max_speed_; + } + RCLCPP_INFO(get_logger(), "PeopleSpeedControl collision cone (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) - %.2f - %.2f, (%d), %.2f, %.2f", + x, y, vx, vy, Px1, Py1, Px2, Py2, t1, t2, v1, v2, vr, speed_limit, swapped, RPy, s); + } + } - double pt = atan2(y, x); - double sdx = abs(social_distance_x_ * cos(pt)); - double sdy = abs(social_distance_y_ * sin(pt)); + // social distance + double sdx = abs(social_distance_x_ * cos(RPy)); + double sdy = abs(social_distance_y_ * sin(RPy)); double min_path_dist = 100; - if (abs(pt) > M_PI_2) { + if (abs(RPy) > M_PI_2) { + RCLCPP_INFO(get_logger(), "PeopleSpeedControl person is back %.2f", RPy); continue; } auto max_v = [](double D, double A, double d) @@ -322,9 +359,8 @@ class PeopleSpeedControlNode : public rclcpp::Node } RCLCPP_INFO( - get_logger(), "PeopleSpeedControl people_limit %s dist from path=%.2f x=%.2f y=%.2f vx=%.2f" - " vy=%.2f pt=%.2f sdx=%.2f sdy=%.2f dist=%.2f limit=%.2f", - it->name.c_str(), min_path_dist, x, y, vx, vy, pt, sdx, sdy, dist, speed_limit); + get_logger(), "PeopleSpeedControl people_limit %s, %.2f %.2f (%.2f %.2f) - %.2f (%.2f)", + it->name.c_str(), min_path_dist, dist, social_distance_x_, social_distance_y_, speed_limit, max_v(std::max(0.0, dist - social_distance_y_), max_acc_, delay_)); if (speed_limit < max_speed_) { std_msgs::msg::String msg; From 22dc87531e9fa5fd1de6c6e6ebf32d522f5a591f Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 6 Apr 2024 08:53:09 -0400 Subject: [PATCH 18/35] bugfix cabot planner Signed-off-by: Daisuke Sato --- cabot_navigation2/plugins/cabot_planner.cpp | 18 ++++++++++++++++++ .../plugins/cabot_planner_param.cpp | 10 ++++++++-- 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/cabot_navigation2/plugins/cabot_planner.cpp b/cabot_navigation2/plugins/cabot_planner.cpp index 65bce285..e59929f5 100644 --- a/cabot_navigation2/plugins/cabot_planner.cpp +++ b/cabot_navigation2/plugins/cabot_planner.cpp @@ -663,6 +663,24 @@ void CaBotPlanner::peopleCallback(people_msgs::msg::People::SharedPtr people) std::unique_lock lock(mutex_); RCLCPP_DEBUG_THROTTLE(logger_, *clock_, 200, "received people %ld", people->people.size()); last_people_ = people; + + geometry_msgs::msg::TransformStamped transform; + try { + RCLCPP_INFO(logger_, "lookupTransform %s->%s", last_people_->header.frame_id.c_str(), "map"); + transform = tf_->lookupTransform("map", last_people_->header.frame_id, rclcpp::Time(0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "%s", ex.what()); + return; + } + tf2::Transform tf2_transform; + tf2::convert(transform.transform, tf2_transform); + + for (auto it = last_people_->people.begin(); it != last_people_->people.end(); it++) { + tf2::Vector3 p(it->position.x, it->position.y, 0); + p = tf2_transform * p; + it->position.x = p.x(); + it->position.y = p.y(); + } } void CaBotPlanner::obstaclesCallback(people_msgs::msg::People::SharedPtr obstacles) diff --git a/cabot_navigation2/plugins/cabot_planner_param.cpp b/cabot_navigation2/plugins/cabot_planner_param.cpp index e4873e79..7cbe1361 100644 --- a/cabot_navigation2/plugins/cabot_planner_param.cpp +++ b/cabot_navigation2/plugins/cabot_planner_param.cpp @@ -377,6 +377,9 @@ void CaBotPlannerParam::setCost() bool stationary = (std::find(it->tags.begin(), it->tags.end(), "stationary") != it->tags.end()); if (!stationary || options.ignore_people) { clearCostAround(*it); + RCLCPP_INFO(logger, "setCost: ignore a person %s in frame %s", it->name.c_str(), people_msg.header.frame_id.c_str()); + } else { + RCLCPP_INFO(logger, "setCost: consider a person %s in frame %s", it->name.c_str(), people_msg.header.frame_id.c_str()); } } for (auto it = obstacles_msg.people.begin(); it != obstacles_msg.people.end(); it++) { @@ -421,6 +424,7 @@ void CaBotPlannerParam::clearCostAround(people_msgs::msg::Person & person) float mx, my; if (!worldToMap(person.position.x, person.position.y, mx, my)) { + RCLCPP_INFO(logger, "person position is out of map (%.2f, %.2f)", person.position.x, person.position.y); return; } RCLCPP_INFO(logger, "clearCostAround %.2f %.2f \n %s", mx, my, rosidl_generator_traits::to_yaml(person).c_str()); @@ -428,6 +432,7 @@ void CaBotPlannerParam::clearCostAround(people_msgs::msg::Person & person) ObstacleGroup group; scanObstacleAt(group, mx, my, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, max_obstacle_scan_distance_cell); + RCLCPP_INFO(logger, "found obstacles %ld", group.obstacles_.size()); for (auto obstacle = group.obstacles_.begin(); obstacle != group.obstacles_.end(); obstacle++) { cost[obstacle->index] = 0; } @@ -445,8 +450,8 @@ void CaBotPlannerParam::scanObstacleAt(ObstacleGroup & group, float mx, float my auto entry = queue.front(); auto temp = entry.first; queue.pop(); - // RCLCPP_INFO(logger, "queue.size=%ld, cost=%d, mark=%d, (%.2f %.2f) [%d], %d", - // queue.size(), cost_[temp.index], mark_[temp.index], temp.x, temp.y, temp.index, entry.second); + // RCLCPP_INFO(logger, "queue.size=%ld, cost=%d, static_cost=%d, mark=%d, (%.2f %.2f) [%d], %d", + // queue.size(), cost[temp.index], static_cost[temp.index], mark[temp.index], temp.x, temp.y, temp.index, entry.second); if (entry.second > max_dist) { continue; } @@ -508,6 +513,7 @@ bool CaBotPlannerParam::worldToMap(float wx, float wy, float & mx, float & my) c if (0 <= mx && mx < width && 0 <= my && my < height) { return true; } + RCLCPP_ERROR(logger, "worldToMap out of bound origin (%.2f, %.2f) resolution=%.2f (%d, %d) - (%.2f, %.2f)", origin_x, origin_y, resolution, width, height, mx, my); return false; } From 834c5bae779cbd23add10f510de18e274ef9a9ee Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 6 Apr 2024 08:53:46 -0400 Subject: [PATCH 19/35] add option to list test modules Signed-off-by: Daisuke Sato --- cabot_navigation2/test/run_test.py | 9 +++++++++ launch.sh | 12 +++++++++++- script/run_test.sh | 11 ++++++++++- 3 files changed, 30 insertions(+), 2 deletions(-) diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py index 0e561d91..ccc0bb3e 100755 --- a/cabot_navigation2/test/run_test.py +++ b/cabot_navigation2/test/run_test.py @@ -23,6 +23,7 @@ ############################################################################### import importlib +import pkgutil import inspect import sys import math @@ -980,6 +981,7 @@ def main(): parser.add_option('-m', '--module', type=str, help='test module name') parser.add_option('-d', '--debug', action='store_true', help='debug print') parser.add_option('-f', '--func', type=str, help='test func name') + parser.add_option('-L', '--list-modules', action='store_true', help='list test modules') parser.add_option('-l', '--list-functions', action='store_true', help='list test function') parser.add_option('-w', '--wait-ready', action='store_true', help='wait ready') @@ -995,6 +997,13 @@ def main(): handler.setFormatter(ColorFormatter()) logger.addHandler(handler) + if options.list_modules: + module = __import__(options.module) + modules = [name for _, name, _ in pkgutil.iter_modules(module.__path__)] + for m in modules: + logger.info(m) + sys.exit(0) + if options.list_functions: module = importlib.import_module(options.module) functions = [func for func in dir(module) if inspect.isfunction(getattr(module, func))] diff --git a/launch.sh b/launch.sh index d62ca383..ff4cb1e1 100755 --- a/launch.sh +++ b/launch.sh @@ -118,6 +118,7 @@ function help() echo "run test (-t) options" echo " -D debug" echo " -f run test CABOT_SITE.." + echo " -L list test modules" echo " -l list test functions" echo " -T run test CABOT_SITE." } @@ -134,6 +135,7 @@ module=tests test_func= unittest= retryoption= +list_modules=0 list_functions=0 pwd=`pwd` @@ -146,7 +148,7 @@ if [ -n "$CABOT_LAUNCH_LOG_PREFIX" ]; then log_prefix=$CABOT_LAUNCH_LOG_PREFIX fi -while getopts "hDf:HlMn:rsS:tT:uvy" arg; do +while getopts "hDf:HlLMn:rsS:tT:uvy" arg; do case $arg in h) help @@ -164,6 +166,9 @@ while getopts "hDf:HlMn:rsS:tT:uvy" arg; do l) list_functions=1 ;; + L) + list_modules=1 + ;; M) log_dmesg=1 ;; @@ -230,6 +235,11 @@ if [ $error -eq 1 ]; then exit 1 fi +if [[ $list_modules -eq 1 ]]; then + docker compose run --rm navigation /home/developer/ros2_ws/script/run_test.sh -L + exit +fi + if [[ $list_functions -eq 1 ]]; then docker compose run --rm navigation /home/developer/ros2_ws/script/run_test.sh -l $module exit diff --git a/script/run_test.sh b/script/run_test.sh index 5181a9dc..8788bbfd 100755 --- a/script/run_test.sh +++ b/script/run_test.sh @@ -46,10 +46,11 @@ scriptdir=`pwd` wait_ready_option= debug= +list_modules=0 list_functions=0 retry=0 -while getopts "hdlrw" arg; do +while getopts "hdLlrw" arg; do case $arg in h) help @@ -58,6 +59,9 @@ while getopts "hdlrw" arg; do d) debug="-d" ;; + L) + list_modules=1 + ;; l) list_functions=1 ;; @@ -82,6 +86,11 @@ blue "testing with $CABOT_SITE" source $scriptdir/../install/setup.bash while [[ 1 -eq 1 ]]; do + if [[ $list_modules -eq 1 ]]; then + ros2 run cabot_navigation2 run_test.py -m ${CABOT_SITE} -L + exit + fi + if [[ $list_functions -eq 1 ]]; then ros2 run cabot_navigation2 run_test.py -m ${CABOT_SITE}.$module -l exit From 366c634e00acd6aa6010140c43125807e0810e91 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Sat, 6 Apr 2024 09:03:13 -0400 Subject: [PATCH 20/35] fix lint error Signed-off-by: Daisuke Sato --- .../src/safety/people_speed_control_node.cpp | 20 +++++++++++-------- .../cabot_rotation_shim_controller.cpp | 3 ++- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/cabot/src/safety/people_speed_control_node.cpp b/cabot/src/safety/people_speed_control_node.cpp index 72bcb049..ce1d18f7 100644 --- a/cabot/src/safety/people_speed_control_node.cpp +++ b/cabot/src/safety/people_speed_control_node.cpp @@ -294,10 +294,10 @@ class PeopleSpeedControlNode : public rclcpp::Node if (vx > 0 || vy > 0) { double pr = 1.0; double s = atan2(pr, dist) + M_PI_2; - double Px1 = x + cos(RPy+s) * pr; - double Py1 = y + sin(RPy+s) * pr; - double Px2 = x + cos(RPy-s) * pr; - double Py2 = y + sin(RPy-s) * pr; + double Px1 = x + cos(RPy + s) * pr; + double Py1 = y + sin(RPy + s) * pr; + double Px2 = x + cos(RPy - s) * pr; + double Py2 = y + sin(RPy - s) * pr; double v1 = vy / -(Py1 / Px1); double t1 = -Py1 / vy; double v2 = vy / -(Py2 / Px2); @@ -319,8 +319,11 @@ class PeopleSpeedControlNode : public rclcpp::Node if (v1 < v2 && v2 < 1.0) { speed_limit = max_speed_; } - RCLCPP_INFO(get_logger(), "PeopleSpeedControl collision cone (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) - %.2f - %.2f, (%d), %.2f, %.2f", - x, y, vx, vy, Px1, Py1, Px2, Py2, t1, t2, v1, v2, vr, speed_limit, swapped, RPy, s); + RCLCPP_INFO( + get_logger(), + "PeopleSpeedControl collision cone (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f) (%.2f, %.2f)" + " - %.2f - %.2f, (%d), %.2f, %.2f", + x, y, vx, vy, Px1, Py1, Px2, Py2, t1, t2, v1, v2, vr, speed_limit, swapped, RPy, s); } } @@ -359,8 +362,9 @@ class PeopleSpeedControlNode : public rclcpp::Node } RCLCPP_INFO( - get_logger(), "PeopleSpeedControl people_limit %s, %.2f %.2f (%.2f %.2f) - %.2f (%.2f)", - it->name.c_str(), min_path_dist, dist, social_distance_x_, social_distance_y_, speed_limit, max_v(std::max(0.0, dist - social_distance_y_), max_acc_, delay_)); + get_logger(), "PeopleSpeedControl people_limit %s, %.2f %.2f (%.2f %.2f) - %.2f (%.2f)", + it->name.c_str(), min_path_dist, dist, social_distance_x_, social_distance_y_, speed_limit, + max_v(std::max(0.0, dist - social_distance_y_), max_acc_, delay_)); if (speed_limit < max_speed_) { std_msgs::msg::String msg; diff --git a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp index d70453fe..ee9cc9a9 100644 --- a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp +++ b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp @@ -50,7 +50,8 @@ geometry_msgs::msg::TwistStamped CaBotRotationShimController::computeVelocityCom std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); double distance_to_heading = hypot(sampled_pt_base.position.x, sampled_pt_base.position.y); if (distance_to_heading >= forward_sampling_distance_ && - fabs(angular_distance_to_heading) > angular_dist_threshold_ && velocity.linear.x < 0.1) { + fabs(angular_distance_to_heading) > angular_dist_threshold_ && velocity.linear.x < 0.1) + { RCLCPP_DEBUG( logger_, "Robot is not within the new path's rough heading, rotating to heading..."); From ccd0a3ca0f1cce31287cc2fd56fe521c98eecfae Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 9 Apr 2024 00:02:53 -0400 Subject: [PATCH 21/35] fix possible bug Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navgoal.py | 2 +- cabot_ui/cabot_ui/navigation.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 7eafd663..931b003a 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -401,7 +401,7 @@ def estimate_next_goal(goals, current_pose, current_floor): continue if goal.match(pose=current_pose, floor=current_floor): return (goal, i-1) - return (None, -1) # might be reached the goal + return (None, 0) # might be reached the goal class Goal(geoutil.TargetPlace): diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 262cb65c..3f1fc53d 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -653,7 +653,8 @@ def _resume_navigation(self, callback): current_pose = self.current_local_pose() goal, index = navgoal.estimate_next_goal(self._sub_goals, current_pose, self.current_floor) - goal.estimate_inner_goal(current_pose, self.current_floor) + if goal: + goal.estimate_inner_goal(current_pose, self.current_floor) self._goal_index = index-1 self._last_estimated_goal = None From e0222e4ce248143eb8d5497c1feb30dc67516b96 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 9 Apr 2024 15:11:42 -0400 Subject: [PATCH 22/35] adjust cabot rotation shim controller Signed-off-by: Daisuke Sato --- cabot_navigation2/params/nav2_params.yaml | 4 ++-- .../plugins/cabot_rotation_shim_controller.cpp | 7 ++++++- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/cabot_navigation2/params/nav2_params.yaml b/cabot_navigation2/params/nav2_params.yaml index caa90133..050e5155 100644 --- a/cabot_navigation2/params/nav2_params.yaml +++ b/cabot_navigation2/params/nav2_params.yaml @@ -186,9 +186,9 @@ controller_server: FollowPath: plugin: "cabot_navigation2::CaBotRotationShimController" primary_controller: "dwb_core::DWBLocalPlanner" - angular_dist_threshold: 0.5 + angular_dist_threshold: 0.3 forward_sampling_distance: 1.0 - rotate_to_heading_angular_vel: 0.8 + rotate_to_heading_angular_vel: 0.5 max_angular_accel: 1.6 simulate_ahead_time: 0.5 #trajectory_generator_name: "dwb_plugins::LimitedAccelGenerator" # default: dwb_plugins::StandardTrajectoryGenerator diff --git a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp index ee9cc9a9..d5a50bd4 100644 --- a/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp +++ b/cabot_navigation2/plugins/cabot_rotation_shim_controller.cpp @@ -49,7 +49,12 @@ geometry_msgs::msg::TwistStamped CaBotRotationShimController::computeVelocityCom double angular_distance_to_heading = std::atan2(sampled_pt_base.position.y, sampled_pt_base.position.x); double distance_to_heading = hypot(sampled_pt_base.position.x, sampled_pt_base.position.y); - if (distance_to_heading >= forward_sampling_distance_ && + + RCLCPP_DEBUG( + logger_, + "angular_distance_to_heading: abs(%.2f) <> %.2f, distance_to_heading: %.2f <> %.2f", + angular_distance_to_heading, angular_dist_threshold_, distance_to_heading, forward_sampling_distance_); + if (distance_to_heading >= forward_sampling_distance_ * 0.9 && fabs(angular_distance_to_heading) > angular_dist_threshold_ && velocity.linear.x < 0.1) { RCLCPP_DEBUG( From aeea9deb95a34c02e42981efc9ea8ce7851dcecd Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Thu, 11 Apr 2024 11:38:40 -0400 Subject: [PATCH 23/35] add logging for debug Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navgoal.py | 3 +++ cabot_ui/cabot_ui/navigation.py | 1 + 2 files changed, 4 insertions(+) diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 931b003a..4fb4e28a 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -395,7 +395,9 @@ def convert(g, a=anchor): def estimate_next_goal(goals, current_pose, current_floor): + CaBotRclpyUtil.info(F"estimate_next_goal is called: len(goals)={len(goals)}, {current_pose}, {current_floor}") for i in range(len(goals), 0, -1): + CaBotRclpyUtil.info(F"checking goal[{i-1}]") goal = goals[i-1] if goal.completed(pose=current_pose, floor=current_floor): continue @@ -874,6 +876,7 @@ def estimate_inner_goal(self, current_pose, current_floor): self.route_index = min_index except: # noqa: #722 CaBotRclpyUtil.error(traceback.format_exc()) + self.route_index = 0 def match(self, pose, floor): # work around, Link.distance_to is not implemented for local geometry diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 3f1fc53d..fb1c6f2f 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -653,6 +653,7 @@ def _resume_navigation(self, callback): current_pose = self.current_local_pose() goal, index = navgoal.estimate_next_goal(self._sub_goals, current_pose, self.current_floor) + self._logger.info(F"navigation.{util.callee_name()} estimated next goal index={index}: {goal}") if goal: goal.estimate_inner_goal(current_pose, self.current_floor) self._goal_index = index-1 From 9308d650415bf8939b79dd282f1b169381ab28a8 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Thu, 11 Apr 2024 11:42:13 -0400 Subject: [PATCH 24/35] add lock for changing process queue Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navigation.py | 29 ++++++++++++++++++----------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index fb1c6f2f..4dd5515b 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -423,6 +423,7 @@ def __init__(self, node: Node, tf_node: Node, srv_node: Node, act_node: Node, so self.current_queue_interval = self.initial_queue_interval self._process_queue = [] + self._process_queue_lock = threading.Lock() self._process_timer = act_node.create_timer(0.1, self._process_queue_func, callback_group=MutuallyExclusiveCallbackGroup()) self._start_loop() @@ -535,7 +536,8 @@ def _goal_updated_callback(self, msg): # wrap execution by a queue def set_destination(self, destination): self.social_navigation.set_active(True) - self._process_queue.append((self._set_destination, destination)) + with self._process_queue_lock: + self._process_queue.append((self._set_destination, destination)) def _set_destination(self, destination): """ @@ -613,7 +615,8 @@ def _set_destination(self, destination): # wrap execution by a queue def retry_navigation(self): - self._process_queue.append((self._retry_navigation,)) + with self._process_queue_lock: + self._process_queue.append((self._retry_navigation,)) def _retry_navigation(self): self._logger.info(F"navigation.{util.callee_name()} called") @@ -626,7 +629,8 @@ def _retry_navigation(self): # wrap execution by a queue def pause_navigation(self, callback): - self._process_queue.append((self._pause_navigation, callback)) + with self._process_queue_lock: + self._process_queue.append((self._pause_navigation, callback)) def _pause_navigation(self, callback): self._logger.info(F"navigation.{util.callee_name()} called") @@ -645,7 +649,8 @@ def _pause_navigation(self, callback): # wrap execution by a queue def resume_navigation(self, callback=None): - self._process_queue.append((self._resume_navigation, callback)) + with self._process_queue_lock: + self._process_queue.append((self._resume_navigation, callback)) def _resume_navigation(self, callback): self._logger.info(F"navigation.{util.callee_name()} called") @@ -663,7 +668,8 @@ def _resume_navigation(self, callback): # wrap execution by a queue def cancel_navigation(self, callback=None): - self._process_queue.append((self._cancel_navigation, callback)) + with self._process_queue_lock: + self._process_queue.append((self._cancel_navigation, callback)) self.social_navigation.set_active(False) def _cancel_navigation(self, callback): @@ -742,12 +748,13 @@ def _stop_loop(self): self.lock.release() def _process_queue_func(self): - if len(self._process_queue) > 0: - process = self._process_queue.pop(0) - try: - process[0](*process[1:]) - except: # noqa: 722 - self._logger.error(traceback.format_exc()) + with self._process_queue_lock: + if len(self._process_queue) > 0: + process = self._process_queue.pop(0) + try: + process[0](*process[1:]) + except: # noqa: 722 + self._logger.error(traceback.format_exc()) # Main loop of navigation GOAL_POSITION_TORELANCE = 1 From daf5225ed05c3387fb1f858ae1bf0a43a9554b47 Mon Sep 17 00:00:00 2001 From: Fuki Miyazaki Date: Fri, 12 Apr 2024 13:31:03 +0900 Subject: [PATCH 25/35] change nav2-param in Narrow and Tight mode Signed-off-by: Fuki Miyazaki --- cabot_ui/cabot_ui/navgoal.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 4fb4e28a..232d7788 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -634,10 +634,10 @@ def get_parameters_for(cls, mode): /local_costmap/local_costmap: inflation_layer.inflation_radius: 0.45 /cabot/lidar_speed_control_node: - min_distance: 0.25 + min_distance: 0.60 /cabot/people_speed_control_node: social_distance_x: 1.0 - social_distance_y: 1.0 + social_distance_y: 0.50 /cabot/speed_control_node_touch_true: complete_stop: [false,false,true,false,true,false,true] /cabot/speed_control_node_touch_false: @@ -664,10 +664,10 @@ def get_parameters_for(cls, mode): /local_costmap/local_costmap: inflation_layer.inflation_radius: 0.25 /cabot/lidar_speed_control_node: - min_distance: 0.25 + min_distance: 0.60 /cabot/people_speed_control_node: social_distance_x: 1.0 - social_distance_y: 1.0 + social_distance_y: 0.50 /cabot/speed_control_node_touch_true: complete_stop: [false,false,true,false,true,false,true] /cabot/speed_control_node_touch_false: From 309c87a9289cf9ef22e01d2fe30a9f710686bb95 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Fri, 12 Apr 2024 13:36:24 -0400 Subject: [PATCH 26/35] re enable social navigation for tight/narrow NavGoal Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navgoal.py | 12 ------------ cabot_ui/cabot_ui/navigation.py | 4 ---- 2 files changed, 16 deletions(-) diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 232d7788..a0effa29 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -526,10 +526,6 @@ def current_statement(self): def is_social_navigation_enabled(self): return True - @property - def is_stop_reason_enabled(self): - return True - def __str__(self): ret = F"{type(self)}, ({hex(id(self))})\n" for key in self.__dict__: @@ -771,10 +767,6 @@ def separate_route(self, route: List[geojson.RouteLink]) -> List[List[geojson.Ro separated_routes.append(current_group) # Add the last group return separated_routes - @property - def is_social_navigation_enabled(self): - return self.navcog_routes[self.route_index][2] == geojson.NavigationMode.Standard - def _extract_pois(self): """extract pois along the route""" temp = [] @@ -1281,10 +1273,6 @@ def nav_params(self): def is_social_navigation_enabled(self): return False - @property - def is_stop_reason_enabled(self): - return False - def _enter(self): # change queue_interval setting if self.queue_interval is not None: diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 4dd5515b..7ca2c3c6 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -989,13 +989,9 @@ def _check_social(self, current_pose): self._logger.info(F"navigation.{util.callee_name()} called", throttle_duration_sec=1) # do not provide social navigation messages while queue navigation - # do not provide social navigation messages while narrow/tight if self._current_goal and not self._current_goal.is_social_navigation_enabled: self._logger.info("social navigation is disabled") return - if self._current_goal and not self._current_goal.is_stop_reason_enabled: - self._logger.info("stop reason is disabled") - return self.social_navigation.current_pose = current_pose message = self.social_navigation.get_message() From db7c412ca8ac690a71baa63622d55e90be7845e3 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Fri, 12 Apr 2024 15:02:46 -0400 Subject: [PATCH 27/35] bugfix Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navigation.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 7ca2c3c6..33cc3096 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -748,11 +748,13 @@ def _stop_loop(self): self.lock.release() def _process_queue_func(self): + process = None with self._process_queue_lock: if len(self._process_queue) > 0: process = self._process_queue.pop(0) try: - process[0](*process[1:]) + if process: + process[0](*process[1:]) except: # noqa: 722 self._logger.error(traceback.format_exc()) From 6af72dddc2b4fef2207bb6c170f4bf46c94c58e9 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Fri, 12 Apr 2024 15:03:05 -0400 Subject: [PATCH 28/35] social navigation bugfix Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/social_navigation.py | 51 ++++++++++++++------------ cabot_ui/cabot_ui/turn_detector.py | 2 +- 2 files changed, 28 insertions(+), 25 deletions(-) diff --git a/cabot_ui/cabot_ui/social_navigation.py b/cabot_ui/cabot_ui/social_navigation.py index 3e242ab7..48d3412f 100644 --- a/cabot_ui/cabot_ui/social_navigation.py +++ b/cabot_ui/cabot_ui/social_navigation.py @@ -18,6 +18,7 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. +import traceback import rclpy.node import rclpy.time import rclpy.clock @@ -170,9 +171,13 @@ def _obstacles_callback(self, msg): self._update() def _stop_reason_callback(self, msg: StopReason): - if msg.summary: - self._stop_reason = StopReason[msg.reason] - self._update() + try: + if msg.summary: + self._stop_reason = msg.reason + self._logger.info("_stop_reason_callback summary is True and calling _update()") + self._update() + except: # noqa: 722 + self._logger.error(traceback.format_exc()) def _update(self): ''' @@ -204,19 +209,19 @@ def _update(self): if self._stop_reason is not None: self._logger.info(F"social navigation stop_reason {self._stop_reason}") code = self._stop_reason - if code == StopReason.AVOIDING_PEOPLE: - self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.STOP, 7) + if code == "AVOIDING_PEOPLE": + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.Category.AVOID, 7) # self._set_message(SNMessage.Code.TRYING_TO_AVOID_PEOPLE, SNMessage.STOP, 7) - elif code == StopReason.THERE_ARE_PEOPLE_IN_THE_PATH: - self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.STOP, 7) + elif code == "THERE_ARE_PEOPLE_IN_THE_PATH": + self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.Category.STOP, 7) # self._set_message(SNMessage.Code.PEOPLE_ARE_IN_MY_WAY, SNMessage.STOP, 7) - elif code == StopReason.AVOIDING_OBSTACLE: + elif code == "AVOIDING_OBSTACLE": self._set_sound(SNMessage.Code.OBSTACLE_AHEAD, SNMessage.Category.AVOID) # self._set_sound(SNMessage.Code.TRYING_TO_AVOID_OBSTACLE, SNMessage.STOP, 7) - elif code == StopReason.UNKNOWN: - self._set_message(SNMessage.Code.PLEASE_WAIT_FOR_A_SECOND, SNMessage.STOP, 7) - elif code == StopReason.NO_TOUCH: - self._set_message(SNMessage.Code.NOT_DETECT_TOUCH, SNMessage.STOP, 7) + elif code == "UNKNOWN": + self._set_message(SNMessage.Code.PLEASE_WAIT_FOR_A_SECOND, SNMessage.Category.STOP, 7) + elif code == "NO_TOUCH": + self._set_message(SNMessage.Code.NOT_DETECT_TOUCH, SNMessage.Category.STOP, 7) self._stop_reason = None # check event @@ -236,41 +241,39 @@ def _update(self): # following people if param == "people_speed_following": if self._people_count == 1: - self._set_message(SNMessage.Code.FOLLOWING_A_PERSON, SNMessage.Category.FOLLWING, 1) + self._set_message(SNMessage.Code.FOLLOWING_A_PERSON, SNMessage.Category.FOLLOWING, 1) elif self._people_count > 1: - self._set_message(SNMessage.Code.FOLLOWING_PEOPLE, SNMessage.Category.FOLLWING, 1) + self._set_message(SNMessage.Code.FOLLOWING_PEOPLE, SNMessage.Category.FOLLOWING, 1) # delete event after check self._event = None def _set_message(self, code, category, priority): - self._logger.info(F"set_message {code} {category} {priority}") now = self._node.get_clock().now() - if self._last_message.priority < priority and \ - (self._last_message.category != category or - (now - self._last_message.time) > Duration(seconds=15.0)): - self._priority = priority + self._logger.info(F"set_message {code} {category} {priority} {self._last_message} {now - self._last_message.time}") + if (self._last_message.priority < priority and self._last_message.category != category) or \ + (now - self._last_message.time) > Duration(seconds=25.0): self._message.code = code self._message.category = category self._message.priority = priority + self._message.time = now def _set_sound(self, code, category, priority): self._logger.info(F"set_sound {code} {category} {priority}") now = self._node.get_clock().now() - if self._last_sound.priority < priority and \ - (self._last_sound.category != category or - (now - self._last_sound.time) > Duration(seconds=15.0)): - self._priority = priority + if (self._last_sound.priority < priority and self._last_sound.category != category) or \ + (now - self._last_sound.time) > Duration(seconds=25.0): self._sound.code = code self._sound.category = category self._sound.priority = priority + self._sound.time = now def get_message(self) -> SNMessage: if not self._is_active: return now = self._node.get_clock().now() - if self._message.code is not None and (now - self._last_message.time) > Duration(seconds=5.0): + self._logger.info(f"get_message {self._message}") self._last_message = self._message self._message = SNMessage.empty_message(self._node.get_clock()) return self._last_message diff --git a/cabot_ui/cabot_ui/turn_detector.py b/cabot_ui/cabot_ui/turn_detector.py index 16a28d06..3c208748 100644 --- a/cabot_ui/cabot_ui/turn_detector.py +++ b/cabot_ui/cabot_ui/turn_detector.py @@ -191,7 +191,7 @@ def detects(path, current_pose=None, visualize=False): # smaller than minimum turn if abs(yaw[TurnEnds[-1]]-yaw[TurnStarts[-1]]) < thtMinimumTurn: # smaller than minimum deviation - if max(dyaw2[TurnStarts[-1]:TurnEnds[-1]]) < thtMinimumDev / 180 * math.pi: + if dyaw2[TurnStarts[-1]:TurnEnds[-1]] and max(dyaw2[TurnStarts[-1]:TurnEnds[-1]]) < thtMinimumDev / 180 * math.pi: del TurnStarts[-1] del TurnEnds[-1] else: From cd1406ba5ad9e6c17ccaedd1b4b616b789280a3f Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Fri, 12 Apr 2024 17:02:04 -0400 Subject: [PATCH 29/35] update lidar speed control deal with points too close Signed-off-by: Daisuke Sato --- cabot/src/safety/lidar_speed_control_node.cpp | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/cabot/src/safety/lidar_speed_control_node.cpp b/cabot/src/safety/lidar_speed_control_node.cpp index eb5102d9..cabbcccb 100644 --- a/cabot/src/safety/lidar_speed_control_node.cpp +++ b/cabot/src/safety/lidar_speed_control_node.cpp @@ -58,6 +58,8 @@ class LiDARSpeedControlNode : public rclcpp::Node double limit_factor_; double min_distance_; double front_angle_in_degree_; + double front_min_range_; + double blind_spot_min_range_; rclcpp::Subscription::SharedPtr scan_sub_; rclcpp::Publisher::SharedPtr vis_pub_; @@ -82,7 +84,9 @@ class LiDARSpeedControlNode : public rclcpp::Node max_acc_(0.6), limit_factor_(3.0), min_distance_(0.5), - front_angle_in_degree_(60) + front_angle_in_degree_(60), + front_min_range_(0.25), + blind_spot_min_range_(0.25) { RCLCPP_INFO(get_logger(), "LiDARSpeedControlNodeClass Constructor"); tfBuffer = new tf2_ros::Buffer(get_clock()); @@ -149,6 +153,8 @@ class LiDARSpeedControlNode : public rclcpp::Node limit_factor_ = declare_parameter("limit_factor", limit_factor_); min_distance_ = declare_parameter("min_distance", min_distance_); front_angle_in_degree_ = declare_parameter("front_angle_in_degree", front_angle_in_degree_); + front_min_range_ = declare_parameter("front_min_range", front_min_range_); + blind_spot_min_range_ = declare_parameter("blind_spot_min_range", blind_spot_min_range_); RCLCPP_INFO( get_logger(), "LiDARSpeedControl with check_blind_space=%s, check_front_obstacle=%s, max_speed=%.2f", @@ -230,9 +236,12 @@ class LiDARSpeedControlNode : public rclcpp::Node curr.transform(map_to_robot_tf2 * robot_to_lidar_tf2); CaBotSafety::Line line(robotp, curr); - double dot = robot.dot(line) / robot.length() / line.length(); - RCLCPP_DEBUG(get_logger(), "%.2f,%.2f,%.2f,%.2f,%.2f", line.s.x, line.s.y, line.e.x, line.e.y, dot); + if (line.length() < front_min_range_) { + continue; + } + double dot = robot.dot(line) / robot.length() / line.length(); + RCLCPP_DEBUG(get_logger(), "%.2f,%.2f,%.2f,%.2f,%.2f,%.2f", line.s.x, line.s.y, line.e.x, line.e.y, dot, line.length()); if (dot < cos(front_angle_in_degree_ / 180.0 * M_PI_2)) { continue; } @@ -243,6 +252,9 @@ class LiDARSpeedControlNode : public rclcpp::Node } // calculate the speed speed_limit = std::min(max_speed_, std::max(min_speed_, (min_range - min_distance_) / limit_factor_)); + if (speed_limit < max_speed_) { + RCLCPP_INFO(get_logger(), "limit by front obstacle (%.2f), min_range=%.2f", speed_limit, min_range); + } } if (check_blind_space_) { // Check blind space @@ -250,6 +262,9 @@ class LiDARSpeedControlNode : public rclcpp::Node for (uint64_t i = 0; i < input->ranges.size(); i++) { double angle = input->angle_min + input->angle_increment * i; double range = input->ranges[i]; + if (range < blind_spot_min_range_) { + continue; + } if (range == inf) { range = input->range_max; } @@ -332,6 +347,9 @@ class LiDARSpeedControlNode : public rclcpp::Node // update speed limit if (limit < speed_limit) { speed_limit = limit; + if (speed_limit < max_speed_) { + RCLCPP_INFO(get_logger(), "limit by blind spot (%.2f), critical_distance=%.2f", speed_limit, critical_distance); + } } } From eb91b8c5a79cba04d8310a0556d98efa47430548 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 16 Apr 2024 13:32:11 -0400 Subject: [PATCH 30/35] suppress Entrance.distance_to log Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/geojson.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cabot_ui/cabot_ui/geojson.py b/cabot_ui/cabot_ui/geojson.py index 00c92894..111b0732 100644 --- a/cabot_ui/cabot_ui/geojson.py +++ b/cabot_ui/cabot_ui/geojson.py @@ -648,7 +648,7 @@ def distance_to(self, robot): pose_TR = geoutil.Pose.pose_from_points(self, robot) yaw = geoutil.diff_angle(self.orientation, pose_TR.orientation) adjusted = dist_TR * math.cos(yaw) - CaBotRclpyUtil.info(f"Entrance.distance_to dist={dist_TR}, yaw={yaw}, adjusted={adjusted}") + CaBotRclpyUtil.debug(f"Entrance.distance_to dist={dist_TR}, yaw={yaw}, adjusted={adjusted}") return adjusted From 9a0518a7854b09349d3360e81720ab0f89785f5f Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 16 Apr 2024 13:32:34 -0400 Subject: [PATCH 31/35] bugfix: social navigation set_sound Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/social_navigation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cabot_ui/cabot_ui/social_navigation.py b/cabot_ui/cabot_ui/social_navigation.py index 48d3412f..6d02a052 100644 --- a/cabot_ui/cabot_ui/social_navigation.py +++ b/cabot_ui/cabot_ui/social_navigation.py @@ -216,7 +216,7 @@ def _update(self): self._set_message(SNMessage.Code.PERSON_AHEAD, SNMessage.Category.STOP, 7) # self._set_message(SNMessage.Code.PEOPLE_ARE_IN_MY_WAY, SNMessage.STOP, 7) elif code == "AVOIDING_OBSTACLE": - self._set_sound(SNMessage.Code.OBSTACLE_AHEAD, SNMessage.Category.AVOID) + self._set_sound(SNMessage.Code.OBSTACLE_AHEAD, SNMessage.Category.AVOID, 7) # self._set_sound(SNMessage.Code.TRYING_TO_AVOID_OBSTACLE, SNMessage.STOP, 7) elif code == "UNKNOWN": self._set_message(SNMessage.Code.PLEASE_WAIT_FOR_A_SECOND, SNMessage.Category.STOP, 7) From 3be595a310d2049aa636ccc9aec70dde5b9c56a6 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 16 Apr 2024 14:06:32 -0400 Subject: [PATCH 32/35] update Exception handling in the _check_loop to isolate isseus Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navigation.py | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 33cc3096..323ab9b5 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -804,16 +804,34 @@ def _check_loop(self): # cabot is active now self._logger.debug("cabot is active", throttle_duration_sec=1) + # isolate error handling try: self._check_info_poi(self.current_pose) + except Exception: + self._logger.error(traceback.format_exc(), throttle_duration_sec=3) + try: self._check_nearby_facility(self.current_pose) + except Exception: + self._logger.error(traceback.format_exc(), throttle_duration_sec=3) + try: self._check_speed_limit(self.current_pose) + except Exception: + self._logger.error(traceback.format_exc(), throttle_duration_sec=3) + try: self._check_turn(self.current_pose) + except Exception: + self._logger.error(traceback.format_exc(), throttle_duration_sec=3) + try: self._check_queue_wait(self.current_pose) + except Exception: + self._logger.error(traceback.format_exc(), throttle_duration_sec=3) + try: self._check_social(self.current_pose) + except Exception: + self._logger.error(traceback.format_exc(), throttle_duration_sec=3) + try: self._check_goal(self.current_pose) except Exception: - import traceback self._logger.error(traceback.format_exc(), throttle_duration_sec=3) def _check_info_poi(self, current_pose): From 2e256fcb3b567babd2ddedfd1e959bde3bf5c293 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 16 Apr 2024 15:25:36 -0400 Subject: [PATCH 33/35] bugfix: turn_detector error, added test case Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/turn_detector.py | 2 +- cabot_ui/test/data/plan_msg1.yaml | 17332 ++++++++++++++++++++++++++ cabot_ui/test/test_turn_detector.py | 57 +- 3 files changed, 17387 insertions(+), 4 deletions(-) create mode 100644 cabot_ui/test/data/plan_msg1.yaml diff --git a/cabot_ui/cabot_ui/turn_detector.py b/cabot_ui/cabot_ui/turn_detector.py index 3c208748..304a62ab 100644 --- a/cabot_ui/cabot_ui/turn_detector.py +++ b/cabot_ui/cabot_ui/turn_detector.py @@ -191,7 +191,7 @@ def detects(path, current_pose=None, visualize=False): # smaller than minimum turn if abs(yaw[TurnEnds[-1]]-yaw[TurnStarts[-1]]) < thtMinimumTurn: # smaller than minimum deviation - if dyaw2[TurnStarts[-1]:TurnEnds[-1]] and max(dyaw2[TurnStarts[-1]:TurnEnds[-1]]) < thtMinimumDev / 180 * math.pi: + if dyaw2[TurnStarts[-1]:TurnEnds[-1]].size > 0 and max(dyaw2[TurnStarts[-1]:TurnEnds[-1]]) < thtMinimumDev / 180 * math.pi: del TurnStarts[-1] del TurnEnds[-1] else: diff --git a/cabot_ui/test/data/plan_msg1.yaml b/cabot_ui/test/data/plan_msg1.yaml new file mode 100644 index 00000000..4210d062 --- /dev/null +++ b/cabot_ui/test/data/plan_msg1.yaml @@ -0,0 +1,17332 @@ +header: + stamp: + sec: 0 + nanosec: 0 + frame_id: map +poses: +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.54720687866211 + y: -15.431489944458008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673346900249873 + w: 0.49772536149894875 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.53711700439453 + y: -15.44875717163086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673355800259017 + w: 0.49772381058287 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.527027130126953 + y: -15.466024398803711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673366480233383 + w: 0.4977219494814748 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.516939163208008 + y: -15.483290672302246 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673378940153004 + w: 0.49771977819361735 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.50684928894043 + y: -15.500557899475098 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673394366644842 + w: 0.4977170899281389 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.496761322021484 + y: -15.517824172973633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673412759660831 + w: 0.49771388468228095 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.486671447753906 + y: -15.535091400146484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673435899093346 + w: 0.49770985226654707 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.476581573486328 + y: -15.552358627319336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673465564758219 + w: 0.4977046824870485 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.466493606567383 + y: -15.569624900817871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673504722964024 + w: 0.49769785835103575 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.456403732299805 + y: -15.586892127990723 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673560492815088 + w: 0.49768813907383086 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.446308135986328 + y: -15.604158401489258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673480990782176 + w: 0.49770199419472133 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.436220169067383 + y: -15.62142562866211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673564052555838 + w: 0.49768751869231304 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.426124572753906 + y: -15.638692855834961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.867340979305336 + w: 0.49771440165787906 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.416034698486328 + y: -15.655959129333496 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8673583037764897 + w: 0.4976842099865844 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.405941009521484 + y: -15.673226356506348 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.8668794179793188 + w: 0.4985178779982093 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.405311584472656 + y: -15.674306869506836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7633229457848412 + w: 0.6460170898965076 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.40200424194336 + y: -15.694029808044434 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7633190952093656 + w: 0.6460216396443352 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.39869499206543 + y: -15.713753700256348 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.763314050914461 + w: 0.646027599779263 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.395387649536133 + y: -15.733476638793945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7633071967597834 + w: 0.6460356982200915 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.392078399658203 + y: -15.75320053100586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7632973774558983 + w: 0.64604729979232 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.388776779174805 + y: -15.772927284240723 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7633114324834904 + w: 0.6460306935742309 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.385469436645508 + y: -15.79265022277832 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7632956446192611 + w: 0.646049347113103 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.38216781616211 + y: -15.812376976013184 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.763326334268827 + w: 0.6460130860994341 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37885856628418 + y: -15.832100868225098 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7632821283050291 + w: 0.646065316055695 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37691879272461 + y: -15.843688011169434 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264824415330975 + w: 0.6871850275901751 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37580680847168 + y: -15.863656044006348 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264827692083706 + w: 0.687184681176274 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.374696731567383 + y: -15.883623123168945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264832197616015 + w: 0.687184204856905 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.373584747314453 + y: -15.90359115600586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264837931925344 + w: 0.6871835986318262 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.372474670410156 + y: -15.923559188842773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264846533379855 + w: 0.6871826892933109 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37136459350586 + y: -15.943526268005371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264859230744282 + w: 0.6871813469344872 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37025260925293 + y: -15.963494300842285 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264881348674693 + w: 0.6871790086264172 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369142532348633 + y: -15.983461380004883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7264930089774185 + w: 0.687173855663133 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368030548095703 + y: -16.003429412841797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7265116859006986 + w: 0.6871541095341893 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367250442504883 + y: -16.01744842529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7131668698631779 + w: 0.7009943050621432 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36690902709961 + y: -16.037446975708008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7131724686976472 + w: 0.7009886089600197 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366565704345703 + y: -16.057445526123047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7131797805398299 + w: 0.7009811699533448 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36621856689453 + y: -16.077444076538086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7131710898844257 + w: 0.7009900117355885 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365877151489258 + y: -16.097442626953125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.71318115933566 + w: 0.700979767160682 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365528106689453 + y: -16.11743927001953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7131682486848442 + w: 0.700992902294875 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36518669128418 + y: -16.13743782043457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7131845436409994 + w: 0.7009763239308296 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364839553833008 + y: -16.15743637084961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7131589311393021 + w: 0.7010023815482 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364498138427734 + y: -16.17743492126465 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7132053087061256 + w: 0.7009551965949037 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364229202270508 + y: -16.192934036254883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7084325021382867 + w: 0.7057785700303505 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364154815673828 + y: -16.212936401367188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7084365826914989 + w: 0.7057744741093935 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36408233642578 + y: -16.23293685913086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7084418831685594 + w: 0.7057691535995217 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364009857177734 + y: -16.25293731689453 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.708448992475845 + w: 0.7057620172975875 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363935470581055 + y: -16.272939682006836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7084590042778127 + w: 0.705751967236146 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36385726928711 + y: -16.292940139770508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.708446342269107 + w: 0.7057646775841955 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36378288269043 + y: -16.312942504882812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7084624536891659 + w: 0.7057485045770387 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363704681396484 + y: -16.332942962646484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7084380129828961 + w: 0.7057730384201751 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363630294799805 + y: -16.352943420410156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7084818457964999 + w: 0.70572903736263 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363563537597656 + y: -16.370189666748047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066008958152383 + w: 0.7076123048909642 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36359405517578 + y: -16.39019012451172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066051556774214 + w: 0.707608051091907 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363624572753906 + y: -16.410192489624023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066105964542557 + w: 0.7076026179845302 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36365509033203 + y: -16.430192947387695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066178929343537 + w: 0.7075953316585788 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363685607910156 + y: -16.4501953125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066281416205531 + w: 0.7075850969811924 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363710403442383 + y: -16.470195770263672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066162902457425 + w: 0.7075969321296868 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363740921020508 + y: -16.490196228027344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066328652449432 + w: 0.7075803797136563 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363765716552734 + y: -16.51019859313965 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066113556290974 + w: 0.7076018598731982 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36379623413086 + y: -16.53019905090332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066574107146815 + w: 0.7075558662621786 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363819122314453 + y: -16.549413681030273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058084118332677 + w: 0.7084027708764277 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.363893508911133 + y: -16.569414138793945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058080740404233 + w: 0.7084031074320244 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36396598815918 + y: -16.58941650390625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058076940232816 + w: 0.7084034860568783 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36404037475586 + y: -16.609416961669922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058071451092738 + w: 0.7084040329590852 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364112854003906 + y: -16.629417419433594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058063850737942 + w: 0.7084047902075924 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364185333251953 + y: -16.6494197845459 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058052872433338 + w: 0.7084058840095521 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364259719848633 + y: -16.66942024230957 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058034293725394 + w: 0.7084077350551466 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36433219909668 + y: -16.689422607421875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7057997980656058 + w: 0.7084113529938308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36440658569336 + y: -16.709423065185547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7057893685111747 + w: 0.7084217439467801 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364479064941406 + y: -16.72942352294922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054604424171457 + w: 0.7087492957207118 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364484786987305 + y: -16.730735778808594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.70544709295101 + w: 0.7087625829902203 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36457633972168 + y: -16.7507381439209 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054422346971088 + w: 0.7087674184847589 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364667892456055 + y: -16.77073860168457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054360667791871 + w: 0.7087735574124575 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36475944519043 + y: -16.790740966796875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054278709699789 + w: 0.7087817145347097 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364856719970703 + y: -16.810741424560547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054374186561797 + w: 0.7087722118986508 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.364948272705078 + y: -16.83074188232422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054256318922961 + w: 0.7087839430103893 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365047454833984 + y: -16.850744247436523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054405026159638 + w: 0.7087691424356286 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36513900756836 + y: -16.870744705200195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.70541929484183 + w: 0.7087902499786911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365236282348633 + y: -16.8907470703125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.705454021186192 + w: 0.7087556870969232 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365327835083008 + y: -16.910747528076172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7052693860349235 + w: 0.7089394142815887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365345001220703 + y: -16.914276123046875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.705338513631752 + w: 0.7088706378372226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36544418334961 + y: -16.934276580810547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.705333696889457 + w: 0.7088754305463347 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365541458129883 + y: -16.95427703857422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7053275280315886 + w: 0.7088815685295031 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365638732910156 + y: -16.974279403686523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7053194577327719 + w: 0.708889598275746 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365741729736328 + y: -16.994279861450195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7053287533569232 + w: 0.7088803493453382 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365840911865234 + y: -17.0142822265625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7053171338077839 + w: 0.708891910489302 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.365943908691406 + y: -17.034282684326172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7053313730109144 + w: 0.7088777428065705 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36604118347168 + y: -17.054283142089844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7053107957920176 + w: 0.7088982164875511 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366146087646484 + y: -17.07428550720215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7053422740482753 + w: 0.7088668961380604 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366243362426758 + y: -17.09428596496582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7052156342717009 + w: 0.7089928837292816 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366273880004883 + y: -17.099998474121094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054850284371726 + w: 0.7087248229397654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366365432739258 + y: -17.1200008392334 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054848594639873 + w: 0.7087249911404833 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366456985473633 + y: -17.14000129699707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054846060041342 + w: 0.7087252434414846 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366548538208008 + y: -17.160003662109375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054843103008581 + w: 0.7087255377925382 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366640090942383 + y: -17.180004119873047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054838456240333 + w: 0.7087260003439447 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366731643676758 + y: -17.20000457763672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054832542167268 + w: 0.7087265890452942 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366823196411133 + y: -17.220006942749023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054822403744869 + w: 0.7087275982464593 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.366914749145508 + y: -17.240007400512695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054804239035302 + w: 0.7087294063949198 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367006301879883 + y: -17.260009765625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054759883154402 + w: 0.7087338216215964 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367097854614258 + y: -17.280010223388672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054488672615631 + w: 0.7087608169752173 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36713409423828 + y: -17.28782844543457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056152948276636 + w: 0.7085951281975268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367219924926758 + y: -17.307830810546875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056177444859796 + w: 0.7085926888322507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367305755615234 + y: -17.327831268310547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056207854293984 + w: 0.708589660642885 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367389678955078 + y: -17.34783172607422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056248400004081 + w: 0.7085856230367636 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367475509643555 + y: -17.367834091186523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056303305284909 + w: 0.7085801554081603 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36756134033203 + y: -17.387834548950195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056382283676604 + w: 0.7085722903600941 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367647171020508 + y: -17.4078369140625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056507295809369 + w: 0.7085598406922957 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367725372314453 + y: -17.427837371826172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056297814775891 + w: 0.7085807021729351 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36781120300293 + y: -17.447837829589844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7056533480504902 + w: 0.7085572329636709 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.367891311645508 + y: -17.46784019470215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7055514740335014 + w: 0.7086586748845691 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36793327331543 + y: -17.477561950683594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058882953337479 + w: 0.7083231709543432 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368000030517578 + y: -17.4975643157959 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058847066798541 + w: 0.7083267472540454 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36806869506836 + y: -17.51756477355957 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058801891721337 + w: 0.7083312491584095 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368135452270508 + y: -17.537567138671875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058742783710089 + w: 0.7083371394570579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368202209472656 + y: -17.557567596435547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058662564908639 + w: 0.708345133354902 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368268966674805 + y: -17.57756805419922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058546879358746 + w: 0.7083566612370841 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368343353271484 + y: -17.597570419311523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058669742417224 + w: 0.7083444181151113 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368410110473633 + y: -17.617570877075195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058467080564713 + w: 0.7083646128413266 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36848258972168 + y: -17.6375732421875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7058695074742537 + w: 0.708341893733566 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368549346923828 + y: -17.657573699951172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7057827813453457 + w: 0.7084283065747924 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368593215942383 + y: -17.66899299621582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062447905096283 + w: 0.7079677223419237 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368640899658203 + y: -17.688995361328125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062449593022667 + w: 0.7079675539600241 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368690490722656 + y: -17.708995819091797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062452124911489 + w: 0.7079673013870993 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36874008178711 + y: -17.72899627685547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062455078780641 + w: 0.7079670067185726 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36878776550293 + y: -17.748998641967773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.706245929859158 + w: 0.7079665857633206 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368837356567383 + y: -17.768999099731445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062465628303286 + w: 0.707965954329971 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368885040283203 + y: -17.78900146484375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062474911870243 + w: 0.7079650282267014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.368934631347656 + y: -17.809001922607422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062490947093655 + w: 0.7079634285909137 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36898422241211 + y: -17.829002380371094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062527659179245 + w: 0.707959766253197 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36903190612793 + y: -17.8490047454834 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062677037431029 + w: 0.7079448641309891 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369062423706055 + y: -17.86186981201172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7064391708245308 + w: 0.707773761822766 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369098663330078 + y: -17.881872177124023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7064368505575337 + w: 0.7077760777070335 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369136810302734 + y: -17.901872634887695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7064339818532961 + w: 0.7077789409716123 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369173049926758 + y: -17.921875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7064301850210195 + w: 0.7077827305686175 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36920928955078 + y: -17.941875457763672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7064251225463614 + w: 0.7077877833329411 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369245529174805 + y: -17.961875915527344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7064178662696604 + w: 0.7077950255653257 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369281768798828 + y: -17.98187828063965 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7064067285842397 + w: 0.7078061414051959 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369319915771484 + y: -18.00187873840332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.706387321593251 + w: 0.7078255094953225 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369361877441406 + y: -18.021881103515625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.70640858487729 + w: 0.7078042887773883 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36939811706543 + y: -18.041881561279297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7063364390044784 + w: 0.7078762850487879 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369428634643555 + y: -18.05589485168457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066524763844182 + w: 0.7075607942924547 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36945343017578 + y: -18.075897216796875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066498615971852 + w: 0.707563405713353 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369478225708008 + y: -18.095897674560547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066466141867029 + w: 0.7075666489161774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369503021240234 + y: -18.11589813232422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.706642396748274 + w: 0.7075708608456505 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369525909423828 + y: -18.135900497436523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066367031665722 + w: 0.707576546910564 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369550704956055 + y: -18.155900955200195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066286055493431 + w: 0.7075846336795273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36957550048828 + y: -18.1759033203125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066161637175399 + w: 0.7075970584825144 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369606018066406 + y: -18.195903778076172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066340461461202 + w: 0.7075792003918451 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369630813598633 + y: -18.215904235839844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066104699250363 + w: 0.7076027443363397 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369661331176758 + y: -18.23590660095215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066690927100155 + w: 0.7075441989080283 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369678497314453 + y: -18.250701904296875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066865942558854 + w: 0.7075267185760673 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36970329284668 + y: -18.270702362060547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066879015829196 + w: 0.7075254127989535 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369728088378906 + y: -18.29070281982422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066895884529472 + w: 0.707523727921689 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369752883911133 + y: -18.310705184936523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066916970348324 + w: 0.7075216218194527 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36977767944336 + y: -18.330705642700195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066946068675266 + w: 0.7075187153880469 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369800567626953 + y: -18.3507080078125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066986974816802 + w: 0.7075146295149639 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36982536315918 + y: -18.370708465576172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7067049809563768 + w: 0.7075083532308627 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369850158691406 + y: -18.390708923339844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.706715776594827 + w: 0.7074975696862644 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369874954223633 + y: -18.41071128845215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7067389276240352 + w: 0.7074744434824686 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369892120361328 + y: -18.43071174621582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066808588571597 + w: 0.707532447118086 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369911193847656 + y: -18.44591522216797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066092889845661 + w: 0.7076039236188038 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.36994171142578 + y: -18.465917587280273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066140970870317 + w: 0.7075991222421626 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.369972229003906 + y: -18.485918045043945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066201704329865 + w: 0.7075930572986545 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37000274658203 + y: -18.50592041015625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066280150944667 + w: 0.7075852233361393 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370027542114258 + y: -18.525920867919922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066198330262436 + w: 0.7075933942413282 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370058059692383 + y: -18.545921325683594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066308830135428 + w: 0.7075823592851229 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37008285522461 + y: -18.5659236907959 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066191582122757 + w: 0.7075940681261925 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370113372802734 + y: -18.58592414855957 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066379684109044 + w: 0.7075752833445426 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370136260986328 + y: -18.605926513671875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066169228864169 + w: 0.7075963003652094 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370166778564453 + y: -18.625926971435547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7066822083669199 + w: 0.707531099230029 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37018585205078 + y: -18.64118003845215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062536942665057 + w: 0.7079588401417931 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370235443115234 + y: -18.66118049621582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.706254917996871 + w: 0.7079576193567191 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370283126831055 + y: -18.681182861328125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062564371074921 + w: 0.7079561038964429 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370332717895508 + y: -18.701183319091797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062583781885535 + w: 0.7079541674702354 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370380401611328 + y: -18.72118377685547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062610366170443 + w: 0.7079515153996196 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37042999267578 + y: -18.741186141967773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062648343547557 + w: 0.7079477267099948 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370479583740234 + y: -18.761186599731445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.706270615316478 + w: 0.7079419594433456 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370527267456055 + y: -18.78118896484375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.706280615847085 + w: 0.7079319823815438 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370576858520508 + y: -18.801189422607422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7063020933035136 + w: 0.7079105543746858 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37061882019043 + y: -18.821189880371094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7062361398331629 + w: 0.7079763518603945 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370655059814453 + y: -18.836071014404297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054035787133474 + w: 0.7088058910155886 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37075424194336 + y: -18.85607147216797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.705406451579941 + w: 0.7088030319273447 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370851516723633 + y: -18.876073837280273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054101271423618 + w: 0.7087993739592304 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.370948791503906 + y: -18.896074295043945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.705414816625497 + w: 0.7087947068687919 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37104606628418 + y: -18.91607666015625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054211959628907 + w: 0.7087883578941496 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.371143341064453 + y: -18.936077117919922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054302790267998 + w: 0.7087793178642923 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37124252319336 + y: -18.956077575683594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054443047397801 + w: 0.7087653581476794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.371334075927734 + y: -18.9760799407959 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054286314098547 + w: 0.7087809576923039 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.371431350708008 + y: -18.99608039855957 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054580767072155 + w: 0.7087516504450317 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.371522903442383 + y: -19.016082763671875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7054175627048896 + w: 0.7087919738735006 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37158966064453 + y: -19.0301513671875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7036961932271022 + w: 0.7105009976331383 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.371784210205078 + y: -19.050148010253906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7037020797361316 + w: 0.7104951674536871 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37197494506836 + y: -19.070146560668945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7036953038941998 + w: 0.7105018784473759 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.372169494628906 + y: -19.090145111083984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7037027996655756 + w: 0.7104944544068101 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.372358322143555 + y: -19.110143661499023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7036937369716417 + w: 0.710503430355467 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.372554779052734 + y: -19.130142211914062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.703704112476231 + w: 0.7104931541429798 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.372743606567383 + y: -19.15013885498047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7036904337182796 + w: 0.7105067019341758 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37293815612793 + y: -19.170137405395508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7037074580151029 + w: 0.7104898405564446 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.373126983642578 + y: -19.190135955810547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7036791686629466 + w: 0.7105178587409499 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.373323440551758 + y: -19.210134506225586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7037317232920598 + w: 0.7104658060965269 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.373445510864258 + y: -19.222951889038086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7007403486736657 + w: 0.7134164027695952 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37380599975586 + y: -19.242950439453125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7007436229319929 + w: 0.7134131866738552 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.374164581298828 + y: -19.26294708251953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7007477476260191 + w: 0.7134091352071832 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37452507019043 + y: -19.28294563293457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7007531479631987 + w: 0.7134038306735305 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37488555908203 + y: -19.30294418334961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.700760461741661 + w: 0.713396646515537 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.375246047973633 + y: -19.32294273376465 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7007710495811529 + w: 0.7133862460609465 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.375598907470703 + y: -19.342941284179688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.700756889905567 + w: 0.7134001550671805 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.375959396362305 + y: -19.362937927246094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7007742811801304 + w: 0.7133830715937066 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.376314163208008 + y: -19.382936477661133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7007439631138298 + w: 0.7134128525331763 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37667465209961 + y: -19.402935028076172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.7008012389586171 + w: 0.7133565892834153 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.376869201660156 + y: -19.41393280029297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6960056002223067 + w: 0.7180363531599124 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.377490997314453 + y: -19.433921813964844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6960046158607122 + w: 0.7180373073180687 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.378114700317383 + y: -19.45391082763672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6960034175057009 + w: 0.7180384689000896 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37873649597168 + y: -19.473899841308594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6960018339621771 + w: 0.7180400038446926 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.37936019897461 + y: -19.49388885498047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6959996512344737 + w: 0.7180421195734209 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.379981994628906 + y: -19.513877868652344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6959964841279387 + w: 0.7180451894425225 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.380603790283203 + y: -19.53386688232422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6959914766480859 + w: 0.7180500431259766 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.381227493286133 + y: -19.553855895996094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6959823603799141 + w: 0.7180588792292756 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.38184928894043 + y: -19.57384490966797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.695960532239696 + w: 0.7180800356260011 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.38247299194336 + y: -19.593833923339844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6958381115384908 + w: 0.718198665085467 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.382747650146484 + y: -19.602516174316406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885616402601161 + w: 0.7251778178917903 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.383785247802734 + y: -19.62248992919922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885665245526824 + w: 0.7251731801890086 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.384822845458984 + y: -19.64246368408203 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885727919619876 + w: 0.7251672291062754 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.385860443115234 + y: -19.662437438964844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885810907972352 + w: 0.725159348968549 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.386890411376953 + y: -19.682411193847656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885715817076461 + w: 0.7251683782850922 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.387928009033203 + y: -19.70238494873047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885833816056656 + w: 0.7251571737054708 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.388959884643555 + y: -19.72235870361328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885688154048063 + w: 0.7251710049719458 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.389997482299805 + y: -19.742332458496094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885897353213886 + w: 0.7251511403907602 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.391029357910156 + y: -19.762306213378906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6885575339722851 + w: 0.7251817168200019 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.392066955566406 + y: -19.78227996826172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.688683694985744 + w: 0.7250619065023226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.392366409301758 + y: -19.788087844848633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773440973947338 + w: 0.7356663467391406 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.394014358520508 + y: -19.808019638061523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773437027524548 + w: 0.7356667100947236 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.395662307739258 + y: -19.82794952392578 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773432204116271 + w: 0.7356671541957042 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.397310256958008 + y: -19.847881317138672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773425626736653 + w: 0.7356677597874408 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.398958206176758 + y: -19.867813110351562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773415979903421 + w: 0.7356686479875908 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.400606155395508 + y: -19.88774299621582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773402386618643 + w: 0.7356698995402005 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.402254104614258 + y: -19.90767478942871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773380023422401 + w: 0.7356719585406416 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.403902053833008 + y: -19.92760467529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6773336612333374 + w: 0.7356759554044447 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.405550003051758 + y: -19.94753646850586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.677321514773013 + w: 0.7356871384125124 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.407197952270508 + y: -19.96746826171875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6771094701076823 + w: 0.7358823041020172 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.407398223876953 + y: -19.969884872436523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6605876533882892 + w: 0.7507489275323365 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.409944534301758 + y: -19.989721298217773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6605884141060435 + w: 0.7507482581723799 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.41248893737793 + y: -20.009557723999023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6605894433113374 + w: 0.7507473525665058 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.415035247802734 + y: -20.029394149780273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6605907857511593 + w: 0.7507461713393323 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.417579650878906 + y: -20.049230575561523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6605926651633613 + w: 0.7507445176172565 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.420124053955078 + y: -20.069067001342773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6605955737693223 + w: 0.7507419582761975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.422670364379883 + y: -20.088903427124023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6606004065080712 + w: 0.750737705807675 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.425214767456055 + y: -20.108739852905273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6606102061442766 + w: 0.750729082651003 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.42776107788086 + y: -20.128576278686523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6606412599556417 + w: 0.7507017554556684 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.43012237548828 + y: -20.147008895874023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367060123675299 + w: 0.771106642310283 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.43390655517578 + y: -20.16664695739746 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367062421751891 + w: 0.7711064525570703 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.43769073486328 + y: -20.1862850189209 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367065179443056 + w: 0.7711062248531247 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.44147491455078 + y: -20.205923080444336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367068856363342 + w: 0.7711059212477104 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.44525909423828 + y: -20.225561141967773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367074371741055 + w: 0.7711054658392602 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.44904327392578 + y: -20.24519920349121 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367083104415773 + w: 0.771104744775074 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.45282745361328 + y: -20.26483726501465 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367097352446467 + w: 0.7711035682998049 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.45661163330078 + y: -20.284475326538086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367129065727067 + w: 0.7711009496841095 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.46039581298828 + y: -20.304113388061523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6367249483289942 + w: 0.7710910064158703 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.463111877441406 + y: -20.318212509155273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034562997000334 + w: 0.7973960711919412 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.468544006347656 + y: -20.337459564208984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034527350557861 + w: 0.7973987688438522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.473976135253906 + y: -20.356706619262695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034480297069142 + w: 0.7974023297199747 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.479408264160156 + y: -20.375953674316406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034416132882264 + w: 0.7974071854153953 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.484840393066406 + y: -20.39520263671875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034323925885101 + w: 0.797414163139147 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.490272521972656 + y: -20.41444969177246 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034178959644768 + w: 0.7974251330562662 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.495710372924805 + y: -20.433696746826172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.603438571416826 + w: 0.7974094873566656 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.501142501831055 + y: -20.452943801879883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034088651798417 + w: 0.7974319666419045 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.506580352783203 + y: -20.472190856933594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.6034939891340104 + w: 0.7973675470440962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.509309768676758 + y: -20.48185920715332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590700830746294 + w: 0.8291204027226245 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.516803741455078 + y: -20.500402450561523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590623736198367 + w: 0.8291256011018802 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.52429962158203 + y: -20.518943786621094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590522425320319 + w: 0.829132432196393 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.531801223754883 + y: -20.537487030029297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590603968287706 + w: 0.8291269340080911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.539295196533203 + y: -20.5560302734375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590450765902591 + w: 0.829137263871424 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.546796798706055 + y: -20.57457160949707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590560478772391 + w: 0.8291298663851654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.554298400878906 + y: -20.593114852905273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590765570025341 + w: 0.8291160373615941 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.56179428100586 + y: -20.611656188964844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5590382565517198 + w: 0.8291418622356571 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.569293975830078 + y: -20.63019561767578 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5592156146733832 + w: 0.8290222532028136 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.571535110473633 + y: -20.6357421875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5023655499034547 + w: 0.8646553384269362 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.581439971923828 + y: -20.653114318847656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5023696728956875 + w: 0.8646529429514943 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.591346740722656 + y: -20.670488357543945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5023751873799429 + w: 0.8646497389723582 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.601253509521484 + y: -20.687862396240234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5023828148571967 + w: 0.8646453072423164 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.61115837097168 + y: -20.705238342285156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5023862162871514 + w: 0.8646433309085774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.621065139770508 + y: -20.72261619567871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5023918337832355 + w: 0.8646400669341653 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.630970001220703 + y: -20.739992141723633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5024027079585176 + w: 0.8646337484946724 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.64087677001953 + y: -20.757369995117188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5024327532524563 + w: 0.8646162897257699 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.650775909423828 + y: -20.77474594116211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.5020505195312498 + w: 0.8648382946183651 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.652088165283203 + y: -20.77704429626465 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4343545687839906 + w: 0.9007419766928116 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.664539337158203 + y: -20.792694091796875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.43434936100139204 + w: 0.9007444879640853 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.676990509033203 + y: -20.80834197998047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4343424083205495 + w: 0.9007478405937508 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.689441680908203 + y: -20.823991775512695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4343327174751222 + w: 0.9007525134745258 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.701892852783203 + y: -20.839641571044922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.434318194535082 + w: 0.9007595161283541 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.714351654052734 + y: -20.85529136657715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.43432960350857824 + w: 0.9007540149874889 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.726808547973633 + y: -20.870941162109375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.43435223333688894 + w: 0.9007431028851993 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.739259719848633 + y: -20.88658905029297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4343143288906115 + w: 0.900761380011542 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.75171661376953 + y: -20.902238845825195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.43533686168033814 + w: 0.9002676362406427 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.75208854675293 + y: -20.9027099609375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.3593512426564057 + w: 0.9332023812664094 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.766925811767578 + y: -20.916122436523438 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.359361393868495 + w: 0.9331984722431196 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.781757354736328 + y: -20.929536819458008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.3593502692502725 + w: 0.9332027560984573 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.79659652709961 + y: -20.942949295043945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.35936428626086797 + w: 0.9331973584190093 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.81142807006836 + y: -20.95636558532715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.35934832243683323 + w: 0.9332035057595067 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.826265335083008 + y: -20.969778060913086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.35937098881069374 + w: 0.9331947773113737 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.841096878051758 + y: -20.983192443847656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.359342481987132 + w: 0.933205754718287 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.855934143066406 + y: -20.996604919433594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.35940419536086066 + w: 0.9331819888730238 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.870765686035156 + y: -21.010021209716797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.3573727544300552 + w: 0.9339618377594855 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.870943069458008 + y: -21.01017951965332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.2845617184367725 + w: 0.9586577222347458 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.88770294189453 + y: -21.021093368530273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.2845569757756917 + w: 0.9586591300026264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.904462814331055 + y: -21.032005310058594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.28455066174019183 + w: 0.9586610041632125 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.921222686767578 + y: -21.04291534423828 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.28455397589611103 + w: 0.9586600204460993 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.937984466552734 + y: -21.053829193115234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.2845438619958491 + w: 0.9586630224434899 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.954744338989258 + y: -21.064739227294922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.28454706187732304 + w: 0.9586620726705437 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.97150421142578 + y: -21.07564926147461 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.28455334734956006 + w: 0.9586602070140184 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 27.988264083862305 + y: -21.086563110351562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.28451469150159814 + w: 0.9586716801490229 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.005023956298828 + y: -21.09747314453125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.2842873469018505 + w: 0.958739122176365 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.006446838378906 + y: -21.09839630126953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.21662019494333112 + w: 0.9762559557527489 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.024568557739258 + y: -21.106857299804688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.21661654355762786 + w: 0.9762567659469236 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.04269027709961 + y: -21.11531639099121 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.2166117283790197 + w: 0.9762578343494375 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.060810089111328 + y: -21.123775482177734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.21660506567606197 + w: 0.9762593126436586 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.07893180847168 + y: -21.132234573364258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.2165952461718706 + w: 0.9762614912694993 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.09705352783203 + y: -21.14069366455078 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.21657936034980138 + w: 0.9762650155928312 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.115175247192383 + y: -21.149150848388672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.21657950582462263 + w: 0.9762649833199807 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.133296966552734 + y: -21.157608032226562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.21657986951165473 + w: 0.9762649026377598 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.151416778564453 + y: -21.16606330871582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.21658353547525963 + w: 0.9762640893534069 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.155536651611328 + y: -21.167985916137695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.15921126910495745 + w: 0.9872445349506822 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.17452621459961 + y: -21.174272537231445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.15921521167579528 + w: 0.9872438991308236 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.193513870239258 + y: -21.180559158325195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.15922038997393353 + w: 0.9872430640002231 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.212501525878906 + y: -21.186845779418945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.15922746599356816 + w: 0.9872419227693215 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.231489181518555 + y: -21.193132400512695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.1592377637206258 + w: 0.9872402618437186 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.250476837158203 + y: -21.19942283630371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.1592331444566312 + w: 0.9872410069007738 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.269466400146484 + y: -21.20570945739746 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.15925434302478667 + w: 0.9872375875278168 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.288454055786133 + y: -21.211997985839844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.15925493146421552 + w: 0.9872374926046559 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.30744171142578 + y: -21.21828842163086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.15925835912276387 + w: 0.9872369396702723 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.31527328491211 + y: -21.220882415771484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.11372962334963067 + w: 0.9935117376119675 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.334754943847656 + y: -21.22540283203125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.1137295197182802 + w: 0.9935117494748863 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.354236602783203 + y: -21.229921340942383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.11372938647797064 + w: 0.9935117647271946 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.373720169067383 + y: -21.23444175720215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.113729216226461 + w: 0.9935117842162291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.39320182800293 + y: -21.23896026611328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.11372895714807035 + w: 0.9935118138733995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.41268539428711 + y: -21.243480682373047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.11372856482934979 + w: 0.9935118587827004 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.432167053222656 + y: -21.248001098632812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.11372787642098595 + w: 0.9935119375854389 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.451648712158203 + y: -21.252519607543945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.11372633675477027 + w: 0.9935121138306974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.471132278442383 + y: -21.25704002380371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.11371987459503043 + w: 0.9935128535263601 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.483314514160156 + y: -21.259864807128906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07948442333679027 + w: 0.9968361081174868 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.50305938720703 + y: -21.263036727905273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07947863769640225 + w: 0.9968365694284715 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.522804260253906 + y: -21.266204833984375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07948212096417097 + w: 0.9968362916984097 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.54254913330078 + y: -21.269371032714844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07948680740605565 + w: 0.996835918016798 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.562294006347656 + y: -21.27254295349121 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07947791727629484 + w: 0.9968366268679248 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.58203887939453 + y: -21.275711059570312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07948394800828425 + w: 0.9968361460185 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.601783752441406 + y: -21.278881072998047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07946777197429161 + w: 0.9968374357022523 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.62152862548828 + y: -21.28204917907715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0794748499201152 + w: 0.9968368714238931 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.641273498535156 + y: -21.28521728515625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.07949880201555488 + w: 0.9968349615047075 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.657833099365234 + y: -21.287874221801758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.054733605075661954 + w: 0.9985009927263074 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.677711486816406 + y: -21.29006004333496 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.05473635765793927 + w: 0.998500841837573 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.697589874267578 + y: -21.29224395751953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.054739891377820635 + w: 0.998500648117939 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.717470169067383 + y: -21.294429779052734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.05474459680375626 + w: 0.9985003901455393 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.737348556518555 + y: -21.296615600585938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.05475116951921377 + w: 0.9985000297627828 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.75722885131836 + y: -21.298803329467773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.05474224967049685 + w: 0.9985005188286148 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.77710723876953 + y: -21.300987243652344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0547523709830732 + w: 0.9984999638816878 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.796985626220703 + y: -21.303176879882812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.054735290102429744 + w: 0.9985009003588343 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.816865921020508 + y: -21.305360794067383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.05475819975553228 + w: 0.9984996442460724 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.83674430847168 + y: -21.30754852294922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.05402570155893514 + w: 0.9985395453215986 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.83753204345703 + y: -21.307634353637695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03750113023892021 + w: 0.9992965852192248 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.857479095458984 + y: -21.309133529663086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.037503453184820394 + w: 0.9992964980421046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.877424240112305 + y: -21.310630798339844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03750642759762426 + w: 0.9992963864083889 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.897371292114258 + y: -21.312129974365234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03751035129060192 + w: 0.9992962391333491 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.917316436767578 + y: -21.313629150390625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.037515782663513256 + w: 0.9992960352423821 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.93726348876953 + y: -21.315126419067383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.037523778952801005 + w: 0.999295735011964 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.957210540771484 + y: -21.316625595092773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03753674127511733 + w: 0.9992952481896654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.977155685424805 + y: -21.318126678466797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.037527170301954925 + w: 0.9992956076602799 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 28.997102737426758 + y: -21.31962776184082 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.037501964116958976 + w: 0.9992965539254953 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.017047882080078 + y: -21.32112693786621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03759133782239299 + w: 0.9992931958743253 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.021503448486328 + y: -21.321462631225586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025874515101399605 + w: 0.9996651986881745 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.041475296020508 + y: -21.32249641418457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025874392207978218 + w: 0.9996652018690396 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.061445236206055 + y: -21.323532104492188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025874232074125522 + w: 0.9996652060137805 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.081417083740234 + y: -21.324565887451172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025874021665690156 + w: 0.9996652114597383 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.10138702392578 + y: -21.325599670410156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025873736776390887 + w: 0.9996652188333983 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.121356964111328 + y: -21.326635360717773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025873315959513615 + w: 0.9996652297250611 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.141328811645508 + y: -21.327669143676758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025872651217799907 + w: 0.999665246929672 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.161298751831055 + y: -21.328704833984375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.02587142786957419 + w: 0.9996652785907838 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.181270599365234 + y: -21.32973861694336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025868435600615285 + w: 0.9996653560264939 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.20124053955078 + y: -21.330772399902344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.025849852618320786 + w: 0.9996658367272592 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.208906173706055 + y: -21.33116912841797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.018339732604347233 + w: 0.9998318129605605 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.22889518737793 + y: -21.33190155029297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0183430587289771 + w: 0.9998317519445286 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.248884201049805 + y: -21.33263397216797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.018347267598598505 + w: 0.9998316747191326 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.26887321472168 + y: -21.33336639404297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.018352767063591735 + w: 0.9998315737868602 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.288862228393555 + y: -21.334102630615234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.018346455622006586 + w: 0.9998316896188627 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.30885124206543 + y: -21.334835052490234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.018354208508100225 + w: 0.9998315473268691 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.328840255737305 + y: -21.3355712890625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01834472551587932 + w: 0.9998317213640239 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.34882926940918 + y: -21.3363037109375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01835793689539872 + w: 0.9998314788767878 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.368818283081055 + y: -21.337038040161133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.018338447595337332 + w: 0.9998318365304203 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.38880729675293 + y: -21.337770462036133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01839019805015272 + w: 0.9998308860080669 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.39925765991211 + y: -21.33815574645996 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013621199592440006 + w: 0.9999072271574313 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.419252395629883 + y: -21.338701248168945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013617157096100955 + w: 0.9999072822180165 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.439247131347656 + y: -21.339244842529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013622036773754913 + w: 0.999907215752609 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.45924186706543 + y: -21.33979034423828 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013616901006138785 + w: 0.9999072857055243 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.479236602783203 + y: -21.340333938598633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01362346622125051 + w: 0.9999071962777938 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.49923324584961 + y: -21.34088134765625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013616425144424888 + w: 0.9999072921857737 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.519227981567383 + y: -21.34142303466797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013626466664046473 + w: 0.9999071553930663 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.539222717285156 + y: -21.341970443725586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013615227574618539 + w: 0.999907308493188 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.55921745300293 + y: -21.342514038085938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013636790346794863 + w: 0.9999070146513812 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.579212188720703 + y: -21.343059539794922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.013606535414956768 + w: 0.9999074268121032 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.59199333190918 + y: -21.343406677246094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010809043410322822 + w: 0.9999415805838627 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.611988067626953 + y: -21.343841552734375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01080613133471786 + w: 0.9999416120582125 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.63198471069336 + y: -21.344274520874023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010802465863023419 + w: 0.9999416516633749 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.651979446411133 + y: -21.344707489013672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010797711738569367 + w: 0.9999417030113359 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.671974182128906 + y: -21.34514045715332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010791299956230587 + w: 0.9999417722273906 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.69196891784668 + y: -21.345571517944336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010798291918706808 + w: 0.9999416967461845 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.711963653564453 + y: -21.346004486083984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010788601140525225 + w: 0.9999418013491739 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.73196029663086 + y: -21.346435546875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010799717690425589 + w: 0.999941681348371 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.751955032348633 + y: -21.34686851501465 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010779673070204337 + w: 0.9999418976363075 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.771949768066406 + y: -21.34729766845703 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.010808916757851983 + w: 0.9999415819529268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.78662872314453 + y: -21.34761619567871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009201333469189756 + w: 0.999957666835146 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.806623458862305 + y: -21.347984313964844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009197779692635898 + w: 0.9999576995296979 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.826618194580078 + y: -21.34835433959961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009193315120982997 + w: 0.9999577405856142 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.846614837646484 + y: -21.34872055053711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00919874263943896 + w: 0.9999576906718871 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.866609573364258 + y: -21.349090576171875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009192909081499423 + w: 0.9999577443185383 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.88660430908203 + y: -21.349456787109375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009200507421033646 + w: 0.999957674435871 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.906599044799805 + y: -21.349824905395508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0091920727891638 + w: 0.9999577520064729 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.926593780517578 + y: -21.350191116333008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009204782010634975 + w: 0.9999576350966758 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.946590423583984 + y: -21.350561141967773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009189392556000296 + w: 0.9999577766408199 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.966585159301758 + y: -21.350927352905273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.009230168785495874 + w: 0.9999574010847618 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 29.982784271240234 + y: -21.351226806640625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008347405035194858 + w: 0.9999651598076696 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.002779006958008 + y: -21.351558685302734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008350808900561252 + w: 0.999965131387443 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.02277374267578 + y: -21.351890563964844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008355077003016631 + w: 0.9999650957349829 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.042768478393555 + y: -21.35222625732422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008349487399901082 + w: 0.9999651424225541 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.062763214111328 + y: -21.35256004333496 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008354982942719778 + w: 0.999965096520887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.082759857177734 + y: -21.352895736694336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008347079083649908 + w: 0.9999651625285609 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.102754592895508 + y: -21.353227615356445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008354792959545712 + w: 0.9999650981082305 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.12274932861328 + y: -21.35356330871582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008341318122769142 + w: 0.9999652106008362 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.142744064331055 + y: -21.353897094726562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008354195071319473 + w: 0.9999651031034584 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.162738800048828 + y: -21.354228973388672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008396592044432587 + w: 0.999964747999668 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.18018341064453 + y: -21.354522705078125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007937467194491432 + w: 0.9999684978110742 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.200178146362305 + y: -21.354839324951172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007937659040897884 + w: 0.9999684962882333 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.220172882080078 + y: -21.35515594482422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007937898383258923 + w: 0.9999684943883267 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.240169525146484 + y: -21.3554744720459 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00793820757261181 + w: 0.9999684919338879 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.260164260864258 + y: -21.355791091918945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007938621998098675 + w: 0.9999684886439029 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.28015899658203 + y: -21.356109619140625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007939203125071218 + w: 0.9999684840302413 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.300153732299805 + y: -21.356426239013672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007940082265870738 + w: 0.9999684770499574 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.320148468017578 + y: -21.35674285888672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00794156209078319 + w: 0.9999684652985604 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.340145111083984 + y: -21.3570613861084 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007944584137199208 + w: 0.9999684412934675 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.360139846801758 + y: -21.357378005981445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007954167143690193 + w: 0.999968365112142 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.37855339050293 + y: -21.3576717376709 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0077397171661520705 + w: 0.999970047940531 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.398548126220703 + y: -21.35797882080078 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007743382276311587 + w: 0.9999700195660483 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.41854476928711 + y: -21.358287811279297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007747969368044242 + w: 0.9999699840348568 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.438539505004883 + y: -21.358598709106445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0077429091786269146 + w: 0.9999700232294224 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.458534240722656 + y: -21.35890769958496 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00774895654033774 + w: 0.9999699763855593 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.47852897644043 + y: -21.35921859741211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007742053784482021 + w: 0.9999700298524942 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.498523712158203 + y: -21.359527587890625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007750940663495366 + w: 0.999969961008245 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.51852035522461 + y: -21.359838485717773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00774004125670016 + w: 0.9999700454320343 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.538515090942383 + y: -21.360145568847656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0077569791775314876 + w: 0.9999699141844415 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.558509826660156 + y: -21.360458374023438 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007729622397078487 + w: 0.9999701260225721 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.577655792236328 + y: -21.360754013061523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0077795514273305425 + w: 0.9999697388319256 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.597652435302734 + y: -21.361064910888672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007779147711211104 + w: 0.9999697419726695 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.617647171020508 + y: -21.36137580871582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007778643415295421 + w: 0.9999697458956535 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.63764190673828 + y: -21.36168670654297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00777799476874538 + w: 0.9999697509411859 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.657636642456055 + y: -21.36199951171875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007777128664947687 + w: 0.9999697576775654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.677631378173828 + y: -21.3623104095459 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007775916119621116 + w: 0.9999697671072364 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.697628021240234 + y: -21.362621307373047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007774094507726529 + w: 0.9999697812707067 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.717622756958008 + y: -21.362932205200195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007771051968783718 + w: 0.9999698049197778 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.73761749267578 + y: -21.363243103027344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007764944073967116 + w: 0.9999698523673242 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.757612228393555 + y: -21.363555908203125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007746436457090505 + w: 0.9999699959109855 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.77730941772461 + y: -21.363861083984375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007810592862594421 + w: 0.9999694968543464 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.797304153442383 + y: -21.364171981811523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007813638660129225 + w: 0.9999694730594975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.817298889160156 + y: -21.364482879638672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007817443927945788 + w: 0.9999694433183614 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.83729362487793 + y: -21.36479377746582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00782233694714021 + w: 0.9999694050543173 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.857288360595703 + y: -21.365108489990234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007816152222999697 + w: 0.9999694534156666 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.87728500366211 + y: -21.365419387817383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007822743922652893 + w: 0.9999694018706375 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.897279739379883 + y: -21.365734100341797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007813571606950825 + w: 0.9999694735834405 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.917274475097656 + y: -21.366044998168945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007823689186138454 + w: 0.9999693944754103 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.93726921081543 + y: -21.36635971069336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0078058362775514575 + w: 0.9999695339959154 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.957263946533203 + y: -21.366670608520508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007828408053109078 + w: 0.9999693575441969 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.97726058959961 + y: -21.366985321044922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.977338790893555 + y: -21.366985321044922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007806074223219576 + w: 0.9999695321384655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 30.997333526611328 + y: -21.36729621887207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007808612931181458 + w: 0.9999695123172961 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.017330169677734 + y: -21.36760711669922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007811784919121796 + w: 0.9999694875426837 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.037324905395508 + y: -21.367919921875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007815859796643832 + w: 0.9999694557013425 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.05731964111328 + y: -21.36823081970215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007821290172589045 + w: 0.9999694132422432 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.077314376831055 + y: -21.368545532226562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007813685224836425 + w: 0.9999694726956454 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.097309112548828 + y: -21.36885643005371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00782127527188363 + w: 0.9999694133587894 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.117305755615234 + y: -21.369171142578125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007808626900594169 + w: 0.999969512208211 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.137300491333008 + y: -21.369482040405273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007821228707179191 + w: 0.9999694137229949 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.15729522705078 + y: -21.369796752929688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007783724091784842 + w: 0.9999697063607792 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.177289962768555 + y: -21.370107650756836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007811784919121796 + w: 0.9999694875426837 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.17768096923828 + y: -21.370113372802734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007792051260047904 + w: 0.9999696415077614 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.197675704956055 + y: -21.370424270629883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007793034706826061 + w: 0.9999696338439774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.217670440673828 + y: -21.37073516845703 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007794262618346731 + w: 0.9999696242737758 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.237667083740234 + y: -21.371047973632812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0077958402308558475 + w: 0.9999696119758315 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.257661819458008 + y: -21.37135887145996 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007797941696366925 + w: 0.999969595590436 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.27765655517578 + y: -21.37166976928711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007800878532776847 + w: 0.9999695726841477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.297651290893555 + y: -21.371980667114258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007805273775852856 + w: 0.999969538386687 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.317646026611328 + y: -21.372291564941406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007812572328329688 + w: 0.9999694813911145 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.337642669677734 + y: -21.372604370117188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00782706885227742 + w: 0.999969368027432 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.357637405395508 + y: -21.37291717529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0077955468731593395 + w: 0.9999696142628277 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.37763214111328 + y: -21.37322998046875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008240919412346979 + w: 0.9999660430470823 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.37818717956543 + y: -21.37323760986328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007805366905273324 + w: 0.9999695376597599 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.398181915283203 + y: -21.373550415039062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007807824590655028 + w: 0.9999695184730191 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.41817855834961 + y: -21.37386131286621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00781089413625688 + w: 0.9999694945011034 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.438173294067383 + y: -21.37417221069336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00781483723568718 + w: 0.9999694636932569 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.458168029785156 + y: -21.374483108520508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007820086940621071 + w: 0.9999694226526334 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.47816276550293 + y: -21.374794006347656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007827425537896681 + w: 0.99996936523548 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.498157501220703 + y: -21.37510871887207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007819475080397719 + w: 0.9999694274373927 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.51815414428711 + y: -21.37541961669922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007831454315866482 + w: 0.999969333691438 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.538148880004883 + y: -21.375734329223633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00781765626300417 + w: 0.9999694416583707 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.558143615722656 + y: -21.37604522705078 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007851126037815785 + w: 0.9999691794350155 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.57813835144043 + y: -21.376359939575195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007575105605290327 + w: 0.9999713084759325 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.57874298095703 + y: -21.37636947631836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007853748561328752 + w: 0.9999691588411792 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.598737716674805 + y: -21.376684188842773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007853118075389692 + w: 0.9999691637928112 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.618732452392578 + y: -21.376998901367188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007852330200784944 + w: 0.9999691699799639 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.638729095458984 + y: -21.37731170654297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007851317884353011 + w: 0.9999691779287394 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.658723831176758 + y: -21.377626419067383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007849972164704727 + w: 0.9999691884938322 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.67871856689453 + y: -21.377941131591797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007848094676230497 + w: 0.999969203230756 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.698713302612305 + y: -21.37825584411621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007845290550288155 + w: 0.9999692252345477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.718708038330078 + y: -21.378570556640625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007840651775221204 + w: 0.9999692616174458 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.738704681396484 + y: -21.378883361816406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007831499017979164 + w: 0.9999693333413437 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.758699417114258 + y: -21.37919807434082 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007805010685239666 + w: 0.9999695404402092 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.77869415283203 + y: -21.37950897216797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008195895211752654 + w: 0.9999664130867986 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.77981185913086 + y: -21.379528045654297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007846162241388099 + w: 0.9999692183952873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.799806594848633 + y: -21.37984275817871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007844693590888279 + w: 0.9999692299178335 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.819801330566406 + y: -21.380157470703125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007842858941857123 + w: 0.9999692443088528 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.83979606628418 + y: -21.380470275878906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0078405055620714 + w: 0.9999692627638768 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.859790802001953 + y: -21.38078498840332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007837374551778672 + w: 0.9999692873084328 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.87978744506836 + y: -21.381099700927734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007833004920391025 + w: 0.9999693215463749 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.899782180786133 + y: -21.38141441345215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.00782648027443876 + w: 0.9999693726342391 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.919776916503906 + y: -21.381729125976562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007815685644641773 + w: 0.9999694570625166 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.93977165222168 + y: -21.38204002380371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007831499017979164 + w: 0.9999693333413437 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.959766387939453 + y: -21.382354736328125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.007805010685239666 + w: 0.9999695404402092 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.97976303100586 + y: -21.382665634155273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.008195895211752654 + w: 0.9999664130867986 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 31.980878829956055 + y: -21.38268280029297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341525333373549 + w: 0.999416632074252 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.000831604003906 + y: -21.384048461914062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415553416957032 + w: 0.9994165295238974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.020782470703125 + y: -21.385412216186523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.04073715209961 + y: -21.386775970458984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.06068801879883 + y: -21.38814353942871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.08064270019531 + y: -21.389507293701172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.10059356689453 + y: -21.3908748626709 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.12054443359375 + y: -21.39223861694336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.140499114990234 + y: -21.393606185913086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.16044998168945 + y: -21.394969940185547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.18040466308594 + y: -21.396337509155273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.184078216552734 + y: -21.396587371826172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414506848664971 + w: 0.9994168871387165 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.20402908325195 + y: -21.397951126098633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414725023373614 + w: 0.9994168125969638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.22398376464844 + y: -21.399314880371094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341499643864544 + w: 0.9994167198583401 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.243934631347656 + y: -21.400680541992188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415343433167525 + w: 0.9994166012846455 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.263885498046875 + y: -21.40204429626465 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.28384017944336 + y: -21.40340805053711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.30379104614258 + y: -21.404775619506836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.32374572753906 + y: -21.406139373779297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.34369659423828 + y: -21.407506942749023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.3636474609375 + y: -21.408870697021484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.383602142333984 + y: -21.41023826599121 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.38727569580078 + y: -21.41048812866211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415151692637342 + w: 0.9994166668070068 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.4072265625 + y: -21.41185188293457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415439861895429 + w: 0.9994165683312327 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.427181243896484 + y: -21.41321563720703 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034157987703373714 + w: 0.9994164456702002 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.4471321105957 + y: -21.414581298828125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416257830552122 + w: 0.9994162887623551 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.46708679199219 + y: -21.41594696044922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415634953211659 + w: 0.9994165016581624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.487037658691406 + y: -21.417312622070312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034162388426614565 + w: 0.9994162952528787 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.506988525390625 + y: -21.418678283691406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415314020543323 + w: 0.9994166113358873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.52694320678711 + y: -21.4200439453125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034161963991407 + w: 0.9994163097609774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.54689407348633 + y: -21.421409606933594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414405207540883 + w: 0.9994169218638794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.56684875488281 + y: -21.422775268554688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416011360275466 + w: 0.9994163730090911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.58679962158203 + y: -21.42413902282715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034350341877765735 + w: 0.9994098528696226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.59048080444336 + y: -21.424392700195312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341525333373549 + w: 0.999416632074252 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.61043167114258 + y: -21.425756454467773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415553416957032 + w: 0.9994165295238974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.63038635253906 + y: -21.427120208740234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.65033721923828 + y: -21.428483963012695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.6702880859375 + y: -21.429851531982422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.690242767333984 + y: -21.431215286254883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.7101936340332 + y: -21.43258285522461 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.73014831542969 + y: -21.43394660949707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.750099182128906 + y: -21.435314178466797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.770050048828125 + y: -21.436677932739258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.79000473022461 + y: -21.438045501708984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.793678283691406 + y: -21.438295364379883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414506848664971 + w: 0.9994168871387165 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.813629150390625 + y: -21.439659118652344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414725023373614 + w: 0.9994168125969638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.83358383178711 + y: -21.441024780273438 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341499643864544 + w: 0.9994167198583401 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.85353469848633 + y: -21.4423885345459 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415343433167525 + w: 0.9994166012846455 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.87348937988281 + y: -21.44375228881836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.89344024658203 + y: -21.44511604309082 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.91339111328125 + y: -21.446483612060547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.933345794677734 + y: -21.447847366333008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.95329666137695 + y: -21.449214935302734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.97325134277344 + y: -21.450578689575195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.993202209472656 + y: -21.451946258544922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 32.99687576293945 + y: -21.45219612121582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415151692637342 + w: 0.9994166668070068 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.01683044433594 + y: -21.45355987548828 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415439861895429 + w: 0.9994165683312327 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.036781311035156 + y: -21.454925537109375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034157987703373714 + w: 0.9994164456702002 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.056732177734375 + y: -21.456289291381836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416257830552122 + w: 0.9994162887623551 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.07668685913086 + y: -21.457656860351562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415634953211659 + w: 0.9994165016581624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.09663772583008 + y: -21.459020614624023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034162388426614565 + w: 0.9994162952528787 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.11659240722656 + y: -21.46038818359375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415314020543323 + w: 0.9994166113358873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.13654327392578 + y: -21.46175193786621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034161963991407 + w: 0.9994163097609774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.156494140625 + y: -21.463119506835938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414405207540883 + w: 0.9994169218638794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.176448822021484 + y: -21.4644832611084 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416011360275466 + w: 0.9994163730090911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.1963996887207 + y: -21.46584701538086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034350341877765735 + w: 0.9994098528696226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.20008087158203 + y: -21.466100692749023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341525333373549 + w: 0.999416632074252 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.22003173828125 + y: -21.467464447021484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415553416957032 + w: 0.9994165295238974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.239986419677734 + y: -21.468828201293945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.25993728637695 + y: -21.470191955566406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.27989196777344 + y: -21.471559524536133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.299842834472656 + y: -21.472923278808594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.319793701171875 + y: -21.47429084777832 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.33974838256836 + y: -21.47565460205078 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.35969924926758 + y: -21.477022171020508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.37965393066406 + y: -21.47838592529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.39960479736328 + y: -21.479753494262695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.40327835083008 + y: -21.480003356933594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034144044629172504 + w: 0.9994169221182725 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.42323303222656 + y: -21.481369018554688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414611468279849 + w: 0.9994168513948868 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.44318389892578 + y: -21.48273277282715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148691080272256 + w: 0.9994167633662666 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.463134765625 + y: -21.48409652709961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415198603913849 + w: 0.999416650776633 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.483089447021484 + y: -21.48546028137207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415634953211659 + w: 0.9994165016581624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.5030403137207 + y: -21.48682403564453 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034162388426614565 + w: 0.9994162952528787 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.52299499511719 + y: -21.488191604614258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415314020543323 + w: 0.9994166113358873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.542945861816406 + y: -21.48955535888672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034161963991407 + w: 0.9994163097609774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.562896728515625 + y: -21.490922927856445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414405207540883 + w: 0.9994169218638794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.58285140991211 + y: -21.492286682128906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416011360275466 + w: 0.9994163730090911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.60280227661133 + y: -21.49365234375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034350341877765735 + w: 0.9994098528696226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.606483459472656 + y: -21.49390411376953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341525333373549 + w: 0.999416632074252 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.626434326171875 + y: -21.495269775390625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415553416957032 + w: 0.9994165295238974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.64638900756836 + y: -21.496633529663086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.66633987426758 + y: -21.497997283935547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.68629455566406 + y: -21.499364852905273 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.70624542236328 + y: -21.500728607177734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.7261962890625 + y: -21.50209617614746 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.746150970458984 + y: -21.503459930419922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.7661018371582 + y: -21.50482749938965 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.78605651855469 + y: -21.50619125366211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.806007385253906 + y: -21.507558822631836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.8096809387207 + y: -21.507808685302734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414506848664971 + w: 0.9994168871387165 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.82963562011719 + y: -21.509172439575195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414725023373614 + w: 0.9994168125969638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.849586486816406 + y: -21.510536193847656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341499643864544 + w: 0.9994167198583401 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.869537353515625 + y: -21.51190185546875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415343433167525 + w: 0.9994166012846455 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.88949203491211 + y: -21.51326560974121 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.90944290161133 + y: -21.514629364013672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.92939758300781 + y: -21.5159969329834 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.94934844970703 + y: -21.51736068725586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.96929931640625 + y: -21.518728256225586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 33.989253997802734 + y: -21.520092010498047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.00920486450195 + y: -21.521459579467773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.01287841796875 + y: -21.521709442138672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415151692637342 + w: 0.9994166668070068 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.032833099365234 + y: -21.523073196411133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415439861895429 + w: 0.9994165683312327 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.05278396606445 + y: -21.524436950683594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034157987703373714 + w: 0.9994164456702002 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.07273864746094 + y: -21.525802612304688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416257830552122 + w: 0.9994162887623551 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.092689514160156 + y: -21.52716827392578 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415634953211659 + w: 0.9994165016581624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.112640380859375 + y: -21.528533935546875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034162388426614565 + w: 0.9994162952528787 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.13259506225586 + y: -21.52989959716797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415314020543323 + w: 0.9994166113358873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.15254592895508 + y: -21.531265258789062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034161963991407 + w: 0.9994163097609774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.17250061035156 + y: -21.532630920410156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414405207540883 + w: 0.9994169218638794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.19245147705078 + y: -21.53399658203125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416011360275466 + w: 0.9994163730090911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.21240234375 + y: -21.53536033630371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034350341877765735 + w: 0.9994098528696226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.21608352661133 + y: -21.535614013671875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341525333373549 + w: 0.999416632074252 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.23603820800781 + y: -21.536977767944336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415553416957032 + w: 0.9994165295238974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.25598907470703 + y: -21.538341522216797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.27593994140625 + y: -21.539705276489258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.295894622802734 + y: -21.541072845458984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.31584548950195 + y: -21.542436599731445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.33580017089844 + y: -21.543804168701172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.355751037597656 + y: -21.545167922973633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.375701904296875 + y: -21.54653549194336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.39565658569336 + y: -21.54789924621582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.41560745239258 + y: -21.549266815185547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.419281005859375 + y: -21.549516677856445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414506848664971 + w: 0.9994168871387165 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.43923568725586 + y: -21.550880432128906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414725023373614 + w: 0.9994168125969638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.45918655395508 + y: -21.55224609375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341499643864544 + w: 0.9994167198583401 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.47914123535156 + y: -21.55360984802246 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415343433167525 + w: 0.9994166012846455 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.49909210205078 + y: -21.554973602294922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.51904296875 + y: -21.556337356567383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.538997650146484 + y: -21.55770492553711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.5589485168457 + y: -21.55906867980957 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.57890319824219 + y: -21.560436248779297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.598854064941406 + y: -21.561800003051758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.618804931640625 + y: -21.563167572021484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.62248229980469 + y: -21.563417434692383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341525333373549 + w: 0.999416632074252 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.642433166503906 + y: -21.564781188964844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415553416957032 + w: 0.9994165295238974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.662384033203125 + y: -21.566146850585938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.68233871459961 + y: -21.5675106048584 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.70228958129883 + y: -21.568878173828125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.72224426269531 + y: -21.570241928100586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.74219512939453 + y: -21.571609497070312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.76214599609375 + y: -21.572973251342773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.782100677490234 + y: -21.5743408203125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.80205154418945 + y: -21.57570457458496 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.82200622558594 + y: -21.577072143554688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.825679779052734 + y: -21.577322006225586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415151692637342 + w: 0.9994166668070068 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.84563064575195 + y: -21.578685760498047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415439861895429 + w: 0.9994165683312327 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.86558532714844 + y: -21.580049514770508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034157987703373714 + w: 0.9994164456702002 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.885536193847656 + y: -21.58141326904297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416257830552122 + w: 0.9994162887623551 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.905487060546875 + y: -21.582780838012695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415634953211659 + w: 0.9994165016581624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.92544174194336 + y: -21.584144592285156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034162388426614565 + w: 0.9994162952528787 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.94539260864258 + y: -21.585512161254883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415314020543323 + w: 0.9994166113358873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.96534729003906 + y: -21.586875915527344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034161963991407 + w: 0.9994163097609774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 34.98529815673828 + y: -21.58824348449707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414405207540883 + w: 0.9994169218638794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.0052490234375 + y: -21.58960723876953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416011360275466 + w: 0.9994163730090911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.025203704833984 + y: -21.590972900390625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034350341877765735 + w: 0.9994098528696226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.02888488769531 + y: -21.591224670410156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414506848664971 + w: 0.9994168871387165 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.04883575439453 + y: -21.59259033203125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414725023373614 + w: 0.9994168125969638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.06878662109375 + y: -21.59395408630371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341499643864544 + w: 0.9994167198583401 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.088741302490234 + y: -21.595317840576172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415343433167525 + w: 0.9994166012846455 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.10869216918945 + y: -21.596681594848633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.12864685058594 + y: -21.598045349121094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.148597717285156 + y: -21.59941291809082 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.168548583984375 + y: -21.60077667236328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.18850326538086 + y: -21.602144241333008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.20845413208008 + y: -21.60350799560547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.22840881347656 + y: -21.604875564575195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.23208236694336 + y: -21.605125427246094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341525333373549 + w: 0.999416632074252 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.25203323364258 + y: -21.606491088867188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415553416957032 + w: 0.9994165295238974 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.27198791503906 + y: -21.60785484313965 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.29193878173828 + y: -21.60921859741211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.3118896484375 + y: -21.610586166381836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.331844329833984 + y: -21.611949920654297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.3517951965332 + y: -21.613317489624023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.37174987792969 + y: -21.614681243896484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.391700744628906 + y: -21.61604881286621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.411651611328125 + y: -21.617412567138672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.43160629272461 + y: -21.6187801361084 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.435279846191406 + y: -21.619029998779297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034144044629172504 + w: 0.9994169221182725 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.455230712890625 + y: -21.620393753051758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414611468279849 + w: 0.9994168513948868 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.47518539428711 + y: -21.62175750732422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148691080272256 + w: 0.9994167633662666 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.49513626098633 + y: -21.623123168945312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415198603913849 + w: 0.999416650776633 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.51509094238281 + y: -21.624486923217773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415634953211659 + w: 0.9994165016581624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.53504180908203 + y: -21.625850677490234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034162388426614565 + w: 0.9994162952528787 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.55499267578125 + y: -21.62721824645996 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415314020543323 + w: 0.9994166113358873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.574947357177734 + y: -21.628582000732422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034161963991407 + w: 0.9994163097609774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.59489822387695 + y: -21.62994956970215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414405207540883 + w: 0.9994169218638794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.61485290527344 + y: -21.63131332397461 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416011360275466 + w: 0.9994163730090911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.634803771972656 + y: -21.63267707824707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034350341877765735 + w: 0.9994098528696226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.638484954833984 + y: -21.632930755615234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034160005632387025 + w: 0.9994163766995192 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.6584358215332 + y: -21.634294509887695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416381437994224 + w: 0.9994162465094375 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.67839050292969 + y: -21.635662078857422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415926100915109 + w: 0.9994164021504294 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.698341369628906 + y: -21.637025833129883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164026597533356 + w: 0.9994162392550179 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.718292236328125 + y: -21.63839340209961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.73824691772461 + y: -21.63975715637207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.75819778442383 + y: -21.641124725341797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.77815246582031 + y: -21.642488479614258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.79810333251953 + y: -21.643856048583984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.81805419921875 + y: -21.645219802856445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.838008880615234 + y: -21.646587371826172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.84168243408203 + y: -21.64683723449707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414506848664971 + w: 0.9994168871387165 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.86163330078125 + y: -21.64820098876953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414725023373614 + w: 0.9994168125969638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.881587982177734 + y: -21.649566650390625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0341499643864544 + w: 0.9994167198583401 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.90153884887695 + y: -21.650930404663086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415343433167525 + w: 0.9994166012846455 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.92149353027344 + y: -21.652294158935547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415802865765397 + w: 0.9994164442704667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.941444396972656 + y: -21.653657913208008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034164395185977435 + w: 0.9994162266551291 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.961395263671875 + y: -21.655025482177734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415562352437102 + w: 0.9994165264701507 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 35.98134994506836 + y: -21.656389236450195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034165232886969425 + w: 0.9994161980185128 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.00130081176758 + y: -21.657756805419922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034148817666269454 + w: 0.9994167590409898 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.02125549316406 + y: -21.659120559692383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416890760170401 + w: 0.9994160723909268 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.04120635986328 + y: -21.66048812866211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03399409938450849 + w: 0.9994220335809273 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.04487991333008 + y: -21.660737991333008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415151692637342 + w: 0.9994166668070068 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.06483459472656 + y: -21.66210174560547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415439861895429 + w: 0.9994165683312327 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.08478546142578 + y: -21.663467407226562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034157987703373714 + w: 0.9994164456702002 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.104736328125 + y: -21.664831161499023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416257830552122 + w: 0.9994162887623551 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.124691009521484 + y: -21.66619873046875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415634953211659 + w: 0.9994165016581624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.1446418762207 + y: -21.66756248474121 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034162388426614565 + w: 0.9994162952528787 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.16459655761719 + y: -21.668930053710938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03415314020543323 + w: 0.9994166113358873 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.184547424316406 + y: -21.6702938079834 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034161963991407 + w: 0.9994163097609774 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.204498291015625 + y: -21.671661376953125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03414405207540883 + w: 0.9994169218638794 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.22445297241211 + y: -21.673025131225586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.03416011360275466 + w: 0.9994163730090911 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.24440383911133 + y: -21.674388885498047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.034350341877765735 + w: 0.9994098528696226 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.248085021972656 + y: -21.67464256286621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015983118886047053 + w: 0.999872261796813 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.26807403564453 + y: -21.67527961730957 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986555027270404 + w: 0.9998722068636372 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.288063049316406 + y: -21.675920486450195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015982439107408684 + w: 0.9998722726629526 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.30805206298828 + y: -21.676559448242188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986633248369225 + w: 0.9998722056129884 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.328041076660156 + y: -21.677200317382812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015981383122495526 + w: 0.9998722895417654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.34803009033203 + y: -21.677837371826172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986761754459934 + w: 0.9998722035583378 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.368019104003906 + y: -21.678478240966797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01597951140316121 + w: 0.9998723194564976 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.38800811767578 + y: -21.679115295410156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598701131701215 + w: 0.9998721995680997 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.407997131347656 + y: -21.67975616455078 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015975289325611434 + w: 0.9998723869229328 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.42798614501953 + y: -21.680395126342773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598772089411413 + w: 0.9998721882223807 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.447975158691406 + y: -21.6810359954834 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01595682727782397 + w: 0.9998726817266415 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.46796417236328 + y: -21.681673049926758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01600370034482882 + w: 0.9998719325870054 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.4716796875 + y: -21.681793212890625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015989933433687504 + w: 0.9998721528419452 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.491668701171875 + y: -21.68243408203125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986555027270404 + w: 0.9998722068636372 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.51165771484375 + y: -21.683074951171875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015982439107408684 + w: 0.9998722726629526 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.531646728515625 + y: -21.683712005615234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986633248369225 + w: 0.9998722056129884 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.5516357421875 + y: -21.68435287475586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015981383122495526 + w: 0.9998722895417654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.571624755859375 + y: -21.68498992919922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986761754459934 + w: 0.9998722035583378 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.59161376953125 + y: -21.685630798339844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01597951140316121 + w: 0.9998723194564976 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.611602783203125 + y: -21.686269760131836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598701131701215 + w: 0.9998721995680997 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.631591796875 + y: -21.68691062927246 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015975289325611434 + w: 0.9998723869229328 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.651580810546875 + y: -21.68754768371582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598772089411413 + w: 0.9998721882223807 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.67156982421875 + y: -21.688188552856445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01595682727782397 + w: 0.9998726817266415 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.691558837890625 + y: -21.688827514648438 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01600370034482882 + w: 0.9998719325870054 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.69527816772461 + y: -21.688945770263672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015989497630444393 + w: 0.999872159811206 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.715267181396484 + y: -21.689586639404297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598607452623264 + w: 0.9998722145460598 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.73525619506836 + y: -21.690227508544922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0159819064589362 + w: 0.9998722811769201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.755245208740234 + y: -21.69086456298828 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598603541568289 + w: 0.999872215171363 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.77523422241211 + y: -21.691505432128906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015980703343838297 + w: 0.999872300406725 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.795223236083984 + y: -21.6921443939209 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598597209384039 + w: 0.9998722161837557 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.81521224975586 + y: -21.692785263061523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015978569025047316 + w: 0.9998723345167181 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.835201263427734 + y: -21.693422317504883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015985845450155196 + w: 0.9998722182085288 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.85519027709961 + y: -21.694063186645508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0159737584266677 + w: 0.9998724113814355 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.875179290771484 + y: -21.6947021484375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015985489730391 + w: 0.9998722238956733 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.89516830444336 + y: -21.69533920288086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.016016999788979653 + w: 0.9998717196309533 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.915157318115234 + y: -21.695980072021484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015977485103958054 + w: 0.9998723518378497 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.918880462646484 + y: -21.69609832763672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015983118886047053 + w: 0.999872261796813 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.93886947631836 + y: -21.69673728942871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986555027270404 + w: 0.9998722068636372 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.958858489990234 + y: -21.697378158569336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015982439107408684 + w: 0.9998722726629526 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.97884750366211 + y: -21.698015213012695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986633248369225 + w: 0.9998722056129884 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 36.998836517333984 + y: -21.69865608215332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015981383122495526 + w: 0.9998722895417654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.01882553100586 + y: -21.699295043945312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015986761754459934 + w: 0.9998722035583378 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.038814544677734 + y: -21.699935913085938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01597951140316121 + w: 0.9998723194564976 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.05880355834961 + y: -21.700572967529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598701131701215 + w: 0.9998721995680997 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.078792572021484 + y: -21.701213836669922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.015975289325611434 + w: 0.9998723869229328 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.09878158569336 + y: -21.70185089111328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01598772089411413 + w: 0.9998721882223807 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.118770599365234 + y: -21.702491760253906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01595682727782397 + w: 0.9998726817266415 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.13875961303711 + y: -21.7031307220459 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.01600370034482882 + w: 0.9998719325870054 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.14247512817383 + y: -21.703248977661133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4208418113210927 + w: 0.9071340418284289 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.155391693115234 + y: -21.718521118164062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4208399188910955 + w: 0.9071349197708884 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.168304443359375 + y: -21.73379135131836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42083764797268125 + w: 0.9071359732966285 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.18122100830078 + y: -21.749061584472656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42083480932095463 + w: 0.9071372901958092 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.19413757324219 + y: -21.764333724975586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4208312136895184 + w: 0.9071389582553528 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.20705032348633 + y: -21.779603958129883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42082653665527575 + w: 0.9071411279656136 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.219966888427734 + y: -21.794876098632812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42082012937055246 + w: 0.9071441003041091 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.232879638671875 + y: -21.81014633178711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42081091041350493 + w: 0.9071483768805173 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.24579620361328 + y: -21.825416564941406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42079647360166067 + w: 0.9071550737356909 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.25871276855469 + y: -21.84068489074707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42078830891557256 + w: 0.9071588609940228 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.27162551879883 + y: -21.855953216552734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4207693569797237 + w: 0.9071676516647129 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.2845458984375 + y: -21.8712215423584 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42088041649323665 + w: 0.9071161309405096 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.29128646850586 + y: -21.879188537597656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4208471101153411 + w: 0.9071315835685394 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.30419921875 + y: -21.894460678100586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.420845731348832 + w: 0.9071322232207754 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.317115783691406 + y: -21.909730911254883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4208440552000018 + w: 0.9071330008343969 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.33003234863281 + y: -21.925003051757812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4208419464945939 + w: 0.9071339791181021 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.34294509887695 + y: -21.94027328491211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42083932412700387 + w: 0.907135195694846 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.35586166381836 + y: -21.955543518066406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42083586367779124 + w: 0.9071368010628648 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.3687744140625 + y: -21.970815658569336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42083118665467056 + w: 0.9071389707971 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.381690979003906 + y: -21.986085891723633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.420824400896034 + w: 0.9071421187501405 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.39460754394531 + y: -22.001358032226562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4208137761350136 + w: 0.9071470475148947 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.40752029418945 + y: -22.01662826538086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.42079474333990563 + w: 0.9071558763396195 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.42043685913086 + y: -22.031896591186523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4207800360531805 + w: 0.9071626983397654 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.433349609375 + y: -22.047164916992188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.4207090393967234 + w: 0.9071956261853814 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.44009017944336 + y: -22.055126190185547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.459999084472656 + y: -22.057003021240234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.47990798950195 + y: -22.058879852294922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.49981689453125 + y: -22.06075668334961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.51972961425781 + y: -22.062633514404297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.53963851928711 + y: -22.064510345458984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.559547424316406 + y: -22.066387176513672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.5794563293457 + y: -22.06826400756836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.599365234375 + y: -22.070140838623047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.61927795410156 + y: -22.072017669677734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.638427734375 + y: -22.073823928833008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.65834045410156 + y: -22.075700759887695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.67824935913086 + y: -22.077577590942383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.698158264160156 + y: -22.07945442199707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.71806716918945 + y: -22.081331253051758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.73797607421875 + y: -22.083208084106445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.75788879394531 + y: -22.085084915161133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.77779769897461 + y: -22.08696174621582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.797706604003906 + y: -22.088838577270508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.8176155090332 + y: -22.090715408325195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.836761474609375 + y: -22.09252166748047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.85667419433594 + y: -22.094398498535156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.876583099365234 + y: -22.096275329589844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.89649200439453 + y: -22.09815216064453 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.91640090942383 + y: -22.10002899169922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.936309814453125 + y: -22.101905822753906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.95622253417969 + y: -22.103782653808594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.976131439208984 + y: -22.10565948486328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 37.99604034423828 + y: -22.10753631591797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.01594924926758 + y: -22.109413146972656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.03510284423828 + y: -22.111221313476562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.05501174926758 + y: -22.11309814453125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.074920654296875 + y: -22.114974975585938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.09483337402344 + y: -22.116851806640625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.114742279052734 + y: -22.118728637695312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.13465118408203 + y: -22.12060546875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.15456008911133 + y: -22.122482299804688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.174468994140625 + y: -22.124359130859375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.19438171386719 + y: -22.126235961914062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.214290618896484 + y: -22.12811279296875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.233436584472656 + y: -22.129919052124023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.25334548950195 + y: -22.13179588317871 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.27325439453125 + y: -22.1336727142334 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.29316711425781 + y: -22.135549545288086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.31307601928711 + y: -22.137426376342773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.332984924316406 + y: -22.13930320739746 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.3528938293457 + y: -22.14118003845215 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.372802734375 + y: -22.143056869506836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.39271545410156 + y: -22.144933700561523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.41262435913086 + y: -22.14681053161621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.43177795410156 + y: -22.148616790771484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.45168685913086 + y: -22.150493621826172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.471595764160156 + y: -22.15237045288086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.49150466918945 + y: -22.154247283935547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.51141357421875 + y: -22.156124114990234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.53132629394531 + y: -22.158000946044922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.55123519897461 + y: -22.15987777709961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.571144104003906 + y: -22.161754608154297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.5910530090332 + y: -22.163631439208984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.6109619140625 + y: -22.165508270263672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.63011169433594 + y: -22.167314529418945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.650020599365234 + y: -22.169191360473633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.66992950439453 + y: -22.17106819152832 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.68983840942383 + y: -22.172945022583008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.709747314453125 + y: -22.174821853637695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.72966003417969 + y: -22.176698684692383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.749568939208984 + y: -22.17857551574707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.76947784423828 + y: -22.180452346801758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.78938674926758 + y: -22.182329177856445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.809295654296875 + y: -22.184206008911133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.82844924926758 + y: -22.186012268066406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697418985910552 + w: 0.9988961034497436 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.848358154296875 + y: -22.187889099121094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697382146248396 + w: 0.9988961207739324 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.86827087402344 + y: -22.18976593017578 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697336003640349 + w: 0.998896142472725 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.888179779052734 + y: -22.19164276123047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697276836907596 + w: 0.9988961702958647 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.90808868408203 + y: -22.193519592285156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697197575810194 + w: 0.9988962075678235 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.92799758911133 + y: -22.195396423339844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697086684692433 + w: 0.9988962597125132 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.947906494140625 + y: -22.19727325439453 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046969192316512864 + w: 0.9988963384521611 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.96781921386719 + y: -22.19915008544922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04696639026866246 + w: 0.99889647020356 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 38.987728118896484 + y: -22.201026916503906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04696073407532852 + w: 0.9988967361319719 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.00763702392578 + y: -22.202903747558594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04694330778074337 + w: 0.9988975552350713 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.02678298950195 + y: -22.204708099365234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.04669189453125 + y: -22.206584930419922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.06660461425781 + y: -22.20846176147461 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.08651351928711 + y: -22.210338592529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.106422424316406 + y: -22.212215423583984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.1263313293457 + y: -22.214092254638672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.146240234375 + y: -22.21596908569336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.16615295410156 + y: -22.217845916748047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.18606185913086 + y: -22.219722747802734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.205970764160156 + y: -22.221599578857422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.22512435913086 + y: -22.223405838012695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.245033264160156 + y: -22.225282669067383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.26494216918945 + y: -22.22715950012207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.28485107421875 + y: -22.229036331176758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.30476379394531 + y: -22.230913162231445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.32467269897461 + y: -22.232789993286133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.344581604003906 + y: -22.23466682434082 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.3644905090332 + y: -22.236543655395508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.3843994140625 + y: -22.238420486450195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.40431213378906 + y: -22.240297317504883 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.423458099365234 + y: -22.242103576660156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.44336700439453 + y: -22.243980407714844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.46327590942383 + y: -22.24585723876953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.483184814453125 + y: -22.24773406982422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.50309753417969 + y: -22.249610900878906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.523006439208984 + y: -22.251487731933594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.54291534423828 + y: -22.25336456298828 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.56282424926758 + y: -22.25524139404297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.582733154296875 + y: -22.257118225097656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.60264587402344 + y: -22.258995056152344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.62179183959961 + y: -22.26080322265625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.641700744628906 + y: -22.262680053710938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.6616096496582 + y: -22.264556884765625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.6815185546875 + y: -22.266433715820312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.70143127441406 + y: -22.268310546875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.72134017944336 + y: -22.270187377929688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.741249084472656 + y: -22.272064208984375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.76115798950195 + y: -22.273941040039062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.78106689453125 + y: -22.27581787109375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.80097961425781 + y: -22.277694702148438 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.82012939453125 + y: -22.27950096130371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.84004211425781 + y: -22.2813777923584 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.85995101928711 + y: -22.283254623413086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.879859924316406 + y: -22.285131454467773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.8997688293457 + y: -22.28700828552246 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.919677734375 + y: -22.28888511657715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.93959045410156 + y: -22.290761947631836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.95949935913086 + y: -22.292638778686523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.979408264160156 + y: -22.29451560974121 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 39.99931716918945 + y: -22.2963924407959 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.018470764160156 + y: -22.298198699951172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.03837966918945 + y: -22.30007553100586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.05828857421875 + y: -22.301952362060547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.07820129394531 + y: -22.303829193115234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.09811019897461 + y: -22.305706024169922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.118019104003906 + y: -22.30758285522461 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.1379280090332 + y: -22.309459686279297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.1578369140625 + y: -22.311336517333984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.17774963378906 + y: -22.313213348388672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.19765853881836 + y: -22.31509017944336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.21680450439453 + y: -22.316896438598633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.23671340942383 + y: -22.31877326965332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.256622314453125 + y: -22.320650100708008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.27653503417969 + y: -22.322526931762695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.296443939208984 + y: -22.324403762817383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.31635284423828 + y: -22.32628059387207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.33626174926758 + y: -22.328157424926758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.356170654296875 + y: -22.330034255981445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.37608337402344 + y: -22.331911087036133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.395992279052734 + y: -22.33378791809082 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.415138244628906 + y: -22.335594177246094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046972753484363104 + w: 0.9988961709958134 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.4350471496582 + y: -22.33747100830078 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046972225077050005 + w: 0.9988961958438479 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.4549560546875 + y: -22.33934783935547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046971562707300935 + w: 0.9988962269909893 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.47486877441406 + y: -22.341224670410156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697071055742485 + w: 0.9988962670616657 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.49477767944336 + y: -22.343101501464844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469695755979298 + w: 0.9988963204297783 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.514686584472656 + y: -22.34497833251953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04696798293335583 + w: 0.9988963953179388 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.53459548950195 + y: -22.34685516357422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04696557905155311 + w: 0.9988965083452601 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.55450439453125 + y: -22.348731994628906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04696156017734356 + w: 0.9988966972944248 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.57441711425781 + y: -22.350608825683594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04695344428183063 + w: 0.998897078817469 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.59432601928711 + y: -22.35248565673828 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046928449090347296 + w: 0.9988982534102133 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.61347961425781 + y: -22.354290008544922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698798798251164 + w: 0.9988954544822773 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.63338851928711 + y: -22.35616683959961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469891601527762 + w: 0.9988953993427624 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.653297424316406 + y: -22.358043670654297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699062629580968 + w: 0.998895330372671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.6732063293457 + y: -22.359920501708984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699251293149316 + w: 0.9988952416186512 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.693115234375 + y: -22.361797332763672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046995032166651345 + w: 0.9988951230993449 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.71302795410156 + y: -22.36367416381836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699856728195274 + w: 0.9988949567764589 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.73293685913086 + y: -22.365550994873047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047003888559664905 + w: 0.9988947063931566 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.752845764160156 + y: -22.367427825927734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04701280076655155 + w: 0.9988942869814025 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.77275466918945 + y: -22.369308471679688 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.79266357421875 + y: -22.371185302734375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.81181716918945 + y: -22.37299156188965 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.83172607421875 + y: -22.374868392944336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.85163879394531 + y: -22.376745223999023 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.87154769897461 + y: -22.37862205505371 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.891456604003906 + y: -22.3804988861084 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.9113655090332 + y: -22.382375717163086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.9312744140625 + y: -22.384252548217773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.95118713378906 + y: -22.38612937927246 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.97109603881836 + y: -22.38800621032715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 40.991004943847656 + y: -22.389883041381836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.01015090942383 + y: -22.39168930053711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.030059814453125 + y: -22.393566131591797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.04997253417969 + y: -22.395442962646484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.069881439208984 + y: -22.397319793701172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.08979034423828 + y: -22.39919662475586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.10969924926758 + y: -22.401073455810547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.129608154296875 + y: -22.402950286865234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.14952087402344 + y: -22.404827117919922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.169429779052734 + y: -22.40670394897461 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.18933868408203 + y: -22.408580780029297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.208492279052734 + y: -22.41038703918457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.22840118408203 + y: -22.412263870239258 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.24831008911133 + y: -22.414140701293945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.268218994140625 + y: -22.416017532348633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.28813171386719 + y: -22.41789436340332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.308040618896484 + y: -22.419771194458008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.32794952392578 + y: -22.421648025512695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.34785842895508 + y: -22.423524856567383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.367767333984375 + y: -22.42540168762207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.38768005371094 + y: -22.427278518676758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.40682601928711 + y: -22.42908477783203 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697418985910552 + w: 0.9988961034497436 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.426734924316406 + y: -22.43096160888672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697382146248396 + w: 0.9988961207739324 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.4466438293457 + y: -22.432838439941406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697336003640349 + w: 0.998896142472725 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.466552734375 + y: -22.434715270996094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697276836907596 + w: 0.9988961702958647 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.48646545410156 + y: -22.43659210205078 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697197575810194 + w: 0.9988962075678235 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.50637435913086 + y: -22.43846893310547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04697086684692433 + w: 0.9988962597125132 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.526283264160156 + y: -22.440345764160156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046969192316512864 + w: 0.9988963384521611 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.54619216918945 + y: -22.442222595214844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04696639026866246 + w: 0.99889647020356 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.56610107421875 + y: -22.44409942626953 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04696073407532852 + w: 0.9988967361319719 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.58601379394531 + y: -22.44597625732422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04694330778074337 + w: 0.9988975552350713 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.605159759521484 + y: -22.44778060913086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.62506866455078 + y: -22.449657440185547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.64497756958008 + y: -22.451534271240234 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.664886474609375 + y: -22.453411102294922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.68479919433594 + y: -22.45528793334961 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.704708099365234 + y: -22.457164764404297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.72461700439453 + y: -22.459041595458984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.74452590942383 + y: -22.460918426513672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.764434814453125 + y: -22.46279525756836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.78434753417969 + y: -22.464672088623047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.803497314453125 + y: -22.46647834777832 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698037073480335 + w: 0.9988958127680887 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.82341003417969 + y: -22.468355178833008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698069075601266 + w: 0.9988957977167028 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.843318939208984 + y: -22.470232009887695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698109264310589 + w: 0.9988957788148171 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.86322784423828 + y: -22.472108840942383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469816098866684 + w: 0.9988957544871522 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.88313674926758 + y: -22.47398567199707 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698230202551651 + w: 0.9988957219331671 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.903045654296875 + y: -22.475862503051758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698327325257178 + w: 0.9988956762517667 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.92295837402344 + y: -22.477739334106445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469847356748356 + w: 0.9988956074653477 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.942867279052734 + y: -22.479616165161133 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698718048740286 + w: 0.9988954924664764 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.96277618408203 + y: -22.48149299621582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.0469921184869657 + w: 0.9988952601750131 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 41.98268508911133 + y: -22.483369827270508 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04700733064418783 + w: 0.9988945444168308 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.00183868408203 + y: -22.48517608642578 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698180710903064 + w: 0.9988957452110655 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.02174758911133 + y: -22.48705291748047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698228714081033 + w: 0.9988957226332579 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.041656494140625 + y: -22.488929748535156 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698288997140202 + w: 0.9988956942794053 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.06156921386719 + y: -22.490806579589844 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046983667697263545 + w: 0.9988956576988975 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.081478118896484 + y: -22.49268341064453 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.046984705905426645 + w: 0.9988956088656014 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.10138702392578 + y: -22.49456024169922 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698615716406421 + w: 0.9988955406021962 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.12129592895508 + y: -22.496437072753906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04698834521536103 + w: 0.9988954376779995 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.141204833984375 + y: -22.498313903808594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699201057289559 + w: 0.9988952652517264 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.16111755371094 + y: -22.50019073486328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.04699941570953748 + w: 0.9988949168571046 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.181026458740234 + y: -22.50206756591797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: -0.047022215327590426 + w: 0.9988938438421201 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.200172424316406 + y: -22.503875732421875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998617757536885 + w: 0.9927755623457157 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.2195930480957 + y: -22.49911117553711 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998697642479893 + w: 0.9927754657969922 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.239013671875 + y: -22.494346618652344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998796758972455 + w: 0.9927753460041638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.25843811035156 + y: -22.48958396911621 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998921764084082 + w: 0.992775194920753 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.27785873413086 + y: -22.484819412231445 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1199908819097832 + w: 0.9927749937717572 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.297279357910156 + y: -22.480056762695312 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999316750524787 + w: 0.9927747175225896 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.31669998168945 + y: -22.475292205810547 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999652562817496 + w: 0.9927743116323905 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.33612060546875 + y: -22.47052764892578 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.12000191785352697 + w: 0.9927736598598269 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.35554504394531 + y: -22.465761184692383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11997983855445195 + w: 0.9927763284549282 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.37496566772461 + y: -22.46099853515625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998064480125782 + w: 0.9927762310173801 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.394386291503906 + y: -22.456233978271484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998716874039086 + w: 0.9927754425536849 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.39990997314453 + y: -22.454879760742188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998267151306255 + w: 0.9927759860797342 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.41933059692383 + y: -22.450115203857422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998308573147205 + w: 0.9927759360189762 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.438751220703125 + y: -22.445350646972656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998360350445456 + w: 0.9927758734427856 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.45817565917969 + y: -22.440587997436523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998426921252696 + w: 0.9927757929872867 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.477596282958984 + y: -22.435823440551758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998513463294043 + w: 0.9927756883944707 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.49701690673828 + y: -22.431060791015625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998634030396275 + w: 0.9927755426784353 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.51643753051758 + y: -22.42629623413086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998809333452 + w: 0.9927753308064955 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.535858154296875 + y: -22.421531677246094 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999092629028371 + w: 0.9927749884077457 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.55528259277344 + y: -22.41676902770996 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999623715511398 + w: 0.9927743465000562 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.574703216552734 + y: -22.412004470825195 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1200098249649595 + w: 0.9927727040525841 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.59412384033203 + y: -22.407238006591797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11986038740823486 + w: 0.9927907571741126 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.59965515136719 + y: -22.4058837890625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998617757536885 + w: 0.9927755623457157 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.619075775146484 + y: -22.401119232177734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998697642479893 + w: 0.9927754657969922 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.63849639892578 + y: -22.39635467529297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998796758972455 + w: 0.9927753460041638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.65791702270508 + y: -22.391592025756836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998921764084082 + w: 0.992775194920753 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.677337646484375 + y: -22.38682746887207 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1199908819097832 + w: 0.9927749937717572 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.69676208496094 + y: -22.382064819335938 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999316750524787 + w: 0.9927747175225896 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.716182708740234 + y: -22.377300262451172 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999652562817496 + w: 0.9927743116323905 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.73560333251953 + y: -22.372535705566406 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.12000191785352697 + w: 0.9927736598598269 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.75502395629883 + y: -22.367769241333008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11997983855445195 + w: 0.9927763284549282 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.774444580078125 + y: -22.363006591796875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998064480125782 + w: 0.9927762310173801 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.79386901855469 + y: -22.35824203491211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998716874039086 + w: 0.9927754425536849 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.79939270019531 + y: -22.356887817382812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998267151306255 + w: 0.9927759860797342 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.81881332397461 + y: -22.352123260498047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998308573147205 + w: 0.9927759360189762 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.838233947753906 + y: -22.34735870361328 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998360350445456 + w: 0.9927758734427856 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.8576545715332 + y: -22.34259605407715 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998426921252696 + w: 0.9927757929872867 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.8770751953125 + y: -22.337831497192383 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998513463294043 + w: 0.9927756883944707 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.89649963378906 + y: -22.33306884765625 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998634030396275 + w: 0.9927755426784353 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.91592025756836 + y: -22.328304290771484 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998809333452 + w: 0.9927753308064955 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.935340881347656 + y: -22.32353973388672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999092629028371 + w: 0.9927749884077457 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.95476150512695 + y: -22.318777084350586 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999623715511398 + w: 0.9927743465000562 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.97418212890625 + y: -22.31401252746582 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1200098249649595 + w: 0.9927727040525841 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.99360656738281 + y: -22.309246063232422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11986038740823486 + w: 0.9927907571741126 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 42.9991340637207 + y: -22.307891845703125 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998617757536885 + w: 0.9927755623457157 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.0185546875 + y: -22.30312728881836 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998697642479893 + w: 0.9927754657969922 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.03797912597656 + y: -22.298362731933594 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998796758972455 + w: 0.9927753460041638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.05739974975586 + y: -22.29360008239746 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998921764084082 + w: 0.992775194920753 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.076820373535156 + y: -22.288835525512695 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1199908819097832 + w: 0.9927749937717572 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.09624099731445 + y: -22.284072875976562 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999316750524787 + w: 0.9927747175225896 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.11566162109375 + y: -22.279308319091797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999652562817496 + w: 0.9927743116323905 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.13508605957031 + y: -22.27454376220703 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.12000191785352697 + w: 0.9927736598598269 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.15450668334961 + y: -22.269777297973633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11997983855445195 + w: 0.9927763284549282 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.173927307128906 + y: -22.2650146484375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998064480125782 + w: 0.9927762310173801 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.1933479309082 + y: -22.260250091552734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998716874039086 + w: 0.9927754425536849 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.19887161254883 + y: -22.258895874023438 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998617757536885 + w: 0.9927755623457157 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.218292236328125 + y: -22.254131317138672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998697642479893 + w: 0.9927754657969922 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.23771667480469 + y: -22.249366760253906 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998796758972455 + w: 0.9927753460041638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.257137298583984 + y: -22.244604110717773 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998921764084082 + w: 0.992775194920753 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.27655792236328 + y: -22.239839553833008 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1199908819097832 + w: 0.9927749937717572 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.29597854614258 + y: -22.235076904296875 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999316750524787 + w: 0.9927747175225896 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.315399169921875 + y: -22.23031234741211 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999652562817496 + w: 0.9927743116323905 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.33482360839844 + y: -22.225547790527344 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.12000191785352697 + w: 0.9927736598598269 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.354244232177734 + y: -22.220781326293945 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11997983855445195 + w: 0.9927763284549282 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.37366485595703 + y: -22.216018676757812 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998064480125782 + w: 0.9927762310173801 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.39308547973633 + y: -22.211254119873047 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998716874039086 + w: 0.9927754425536849 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.39860916137695 + y: -22.20989990234375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998267151306255 + w: 0.9927759860797342 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.41802978515625 + y: -22.205135345458984 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998308573147205 + w: 0.9927759360189762 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.43745422363281 + y: -22.20037078857422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998360350445456 + w: 0.9927758734427856 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.45687484741211 + y: -22.195608139038086 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998426921252696 + w: 0.9927757929872867 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.476295471191406 + y: -22.19084358215332 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998513463294043 + w: 0.9927756883944707 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.4957160949707 + y: -22.186080932617188 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998634030396275 + w: 0.9927755426784353 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.51513671875 + y: -22.181316375732422 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998809333452 + w: 0.9927753308064955 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.53456115722656 + y: -22.176551818847656 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999092629028371 + w: 0.9927749884077457 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.55398178100586 + y: -22.171789169311523 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999623715511398 + w: 0.9927743465000562 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.573402404785156 + y: -22.167024612426758 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1200098249649595 + w: 0.9927727040525841 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.59282302856445 + y: -22.16225814819336 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11986038740823486 + w: 0.9927907571741126 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.59835433959961 + y: -22.160903930664062 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998617757536885 + w: 0.9927755623457157 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.617774963378906 + y: -22.156139373779297 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998697642479893 + w: 0.9927754657969922 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.6371955871582 + y: -22.15137481689453 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998796758972455 + w: 0.9927753460041638 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.6566162109375 + y: -22.1466121673584 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998921764084082 + w: 0.992775194920753 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.67604064941406 + y: -22.141847610473633 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.1199908819097832 + w: 0.9927749937717572 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.69546127319336 + y: -22.1370849609375 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999316750524787 + w: 0.9927747175225896 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.714881896972656 + y: -22.132320404052734 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11999652562817496 + w: 0.9927743116323905 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.73430252075195 + y: -22.12755584716797 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.12000191785352697 + w: 0.9927736598598269 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.75372314453125 + y: -22.12278938293457 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11997983855445195 + w: 0.9927763284549282 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.77314758300781 + y: -22.118026733398438 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998064480125782 + w: 0.9927762310173801 +- header: + stamp: + sec: 0 + nanosec: 0 + frame_id: '' + pose: + position: + x: 43.79256820678711 + y: -22.113262176513672 + z: 0.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.11998716874039086 + w: 0.9927754425536849 + diff --git a/cabot_ui/test/test_turn_detector.py b/cabot_ui/test/test_turn_detector.py index 973a6241..e2fe8a12 100644 --- a/cabot_ui/test/test_turn_detector.py +++ b/cabot_ui/test/test_turn_detector.py @@ -21,15 +21,56 @@ # SOFTWARE. """Test cabot_ui.turn_detector module""" +import os import unittest +import yaml -from cabot_ui.geoutil import Pose +import cabot_ui.geoutil from cabot_ui.turn_detector import TurnDetector from nav_msgs.msg import Path -from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion +from std_msgs.msg import Header +from builtin_interfaces.msg import Time import rclpy from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil +def parsePath(path_data): + path_msg = Path() + # Populate header + path_msg.header = Header( + stamp=Time( + sec=path_data['header']['stamp']['sec'], + nanosec=path_data['header']['stamp']['nanosec'] + ), + frame_id=path_data['header']['frame_id'] + ) + path_msg.poses = [ + PoseStamped( + header=Header( + stamp=Time( + sec=pose['header']['stamp']['sec'], + nanosec=pose['header']['stamp']['nanosec'] + ), + frame_id=pose['header']['frame_id'] + ), + pose=Pose( + position=Point( + x=pose['pose']['position']['x'], + y=pose['pose']['position']['y'], + z=pose['pose']['position']['z'] + ), + orientation=Quaternion( + x=pose['pose']['orientation']['x'], + y=pose['pose']['orientation']['y'], + z=pose['pose']['orientation']['z'], + w=pose['pose']['orientation']['w'] + ) + ) + ) for pose in path_data['poses'] + ] + return path_msg + + class TestEvent(unittest.TestCase): """Test class""" @@ -45,7 +86,7 @@ def tearDown(self) -> None: return super().tearDown() def test_basic(self): - current_pose = Pose(x=0, y=0, r=0) + current_pose = cabot_ui.geoutil.Pose(x=0, y=0, r=0) for i in range(0, 20): path = Path() for j in range(0, i): @@ -55,3 +96,13 @@ def test_basic(self): pose.pose.orientation.w = 1.0 path.poses.append(pose) _ = TurnDetector.detects(path, current_pose=current_pose) + + def test_plan_msg1(self): + dir_path = os.path.dirname(os.path.realpath(__file__)) + + with open(dir_path+"/data/plan_msg1.yaml") as plan_file: + path_data = yaml.safe_load(plan_file) + path = parsePath(path_data) + pose = path.poses[0] + current_pose = cabot_ui.geoutil.Pose(x=pose.pose.position.x, y=pose.pose.position.y, r=0) + TurnDetector.detects(path, current_pose=current_pose) From 22551a9525ca08a3123957c45fa339a630fe93a6 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 17 Apr 2024 11:22:55 -0400 Subject: [PATCH 34/35] bugfix: incorrect announce when pause/resume on a narrow path Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/navgoal.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index a0effa29..2aefc49e 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -798,7 +798,7 @@ def enter(self): if new_mode == geojson.NavigationMode.Tight: self.delegate.please_follow_behind() delay = True - if self.mode == geojson.NavigationMode.Tight: + if self.mode == geojson.NavigationMode.Tight and new_mode != geojson.NavigationMode.Tight: self.delegate.please_return_position() delay = True self.mode = new_mode From c26ac0a28ba05dfe1fc365ead7f5dc13b79fb8cd Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Wed, 17 Apr 2024 11:25:53 -0400 Subject: [PATCH 35/35] fix lint error Signed-off-by: Daisuke Sato --- cabot_ui/test/test_turn_detector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cabot_ui/test/test_turn_detector.py b/cabot_ui/test/test_turn_detector.py index e2fe8a12..deb93866 100644 --- a/cabot_ui/test/test_turn_detector.py +++ b/cabot_ui/test/test_turn_detector.py @@ -34,6 +34,7 @@ import rclpy from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil + def parsePath(path_data): path_msg = Path() # Populate header @@ -71,7 +72,6 @@ def parsePath(path_data): return path_msg - class TestEvent(unittest.TestCase): """Test class"""