diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py index f5cc3bc2..47e238e8 100755 --- a/cabot_navigation2/test/run_test.py +++ b/cabot_navigation2/test/run_test.py @@ -83,7 +83,7 @@ def wrap(*args, **kwargs): rclpy.spin_once(node, timeout_sec=0.1) if not case['done']: case['success'] = False - case['error'] = "Timeout" + case['error'] = f"Timeout ({t} seconds)" # logger.error("Timeout") # continue other test else: @@ -228,82 +228,119 @@ def default_config(self): def info(self, text): logger.info(text) - ### shorthand functions - def check_collision(self): - return self.check_topic_error( - action_name=f'check_collision', - topic="/collision", - topic_type="pedestrian_plugin_msgs/msg/Collision", - condition="True" + # shorthand functions + def check_collision(self, **kwargs): + return self.check_topic_error(**dict( + dict( + action_name=f'check_collision', + topic="/collision", + topic_type="pedestrian_plugin_msgs/msg/Collision", + condition="True" + ), + **kwargs) ) - def goto_node(self, node_id): - self.pub_topic( - action_name=f'goto_node({node_id})', - topic='/cabot/event', - topic_type='std_msgs/msg/String', - message=f"data: 'navigation;destination;{node_id}'" + def goto_node(self, node_id, **kwargs): + self.pub_topic(**dict( + dict( + action_name=f'goto_node({node_id})', + topic='/cabot/event', + topic_type='std_msgs/msg/String', + message=f"data: 'navigation;destination;{node_id}'" + ), + **kwargs) ) - def cancel_navigation(self): - self.pub_topic( - action_name='cancel_navigation', - topic='/cabot/event', - topic_type='std_msgs/msg/String', - message="data: 'navigation;cancel'" + def cancel_navigation(self, **kwargs): + self.pub_topic(**dict( + dict( + action_name='cancel_navigation', + topic='/cabot/event', + topic_type='std_msgs/msg/String', + message="data: 'navigation;cancel'" + ), + **kwargs) ) - def button_down(self, button): - self.pub_topic( - action_name=f'button_down({button})', - topic='/cabot/event', - topic_type='std_msgs/msg/String', - message=f"data: 'button_down_{button}'") - - def wait_navigation_completed(self): - self.wait_topic( - action_name='wait_navigation_completed', - topic='/cabot/activity_log', - topic_type='cabot_msgs/msg/Log', - condition="msg.category=='cabot/navigation' and msg.text=='completed'", - timeout=60) - - def wait_navigation_arrived(self): - self.wait_topic( - action_name='wait_navigation_arrived', - topic='/cabot/activity_log', - topic_type='cabot_msgs/msg/Log', - condition="msg.category=='cabot/navigation' and msg.text=='navigation' and msg.memo=='arrived'", - timeout=60) - - def wait_turn_towards(self): - self.wait_topic( - action_name='wait_turn_towards', - topic='/cabot/activity_log', - topic_type='cabot_msgs/msg/Log', - condition="msg.category=='cabot/navigation' and msg.text=='turn_towards'", - timeout=60) - - def check_navigation_arrived(self): - self.check_topic( - action_name='check_navigation_arrived', - topic='/cabot/activity_log', - topic_type='cabot_msgs/msg/Log', - condition="msg.category=='cabot/navigation' and msg.text=='navigation' and msg.memo=='arrived'", - timeout=60) - - def check_turn_towards(self): - self.check_topic( - action_name='check_turn_towards', - topic='/cabot/activity_log', - topic_type='cabot_msgs/msg/Log', - condition="msg.category=='cabot/navigation' and msg.text=='turn_towards'", - timeout=60) - - def wait_for(self, seconds): - self.wait( - action_name=f'wait_for({seconds})', - seconds=seconds) + def button_down(self, button, **kwargs): + self.pub_topic(**dict( + dict( + action_name=f'button_down({button})', + topic='/cabot/event', + topic_type='std_msgs/msg/String', + message=f"data: 'button_down_{button}'" + ), + **kwargs) + ) + + def wait_navigation_completed(self, **kwargs): + self.wait_topic(**dict( + dict( + action_name='wait_navigation_completed', + topic='/cabot/activity_log', + topic_type='cabot_msgs/msg/Log', + condition="msg.category=='cabot/navigation' and msg.text=='completed'", + timeout=60 + ), + **kwargs) + ) + + def wait_navigation_arrived(self, **kwargs): + self.wait_topic(**dict( + dict( + action_name='wait_navigation_arrived', + topic='/cabot/activity_log', + topic_type='cabot_msgs/msg/Log', + condition="msg.category=='cabot/navigation' and msg.text=='navigation' and msg.memo=='arrived'", + timeout=60 + ), + **kwargs) + ) + + def wait_turn_towards(self, **kwargs): + self.wait_topic(**dict( + dict( + action_name='wait_turn_towards', + topic='/cabot/activity_log', + topic_type='cabot_msgs/msg/Log', + condition="msg.category=='cabot/navigation' and msg.text=='turn_towards'", + timeout=60 + ), + **kwargs) + ) + + def check_navigation_arrived(self, **kwargs): + self.check_topic(**dict( + dict( + action_name='check_navigation_arrived', + topic='/cabot/activity_log', + topic_type='cabot_msgs/msg/Log', + condition="msg.category=='cabot/navigation' and msg.text=='navigation' and msg.memo=='arrived'", + timeout=60 + ), + **kwargs) + ) + + def check_turn_towards(self, **kwargs): + self.check_topic(**dict( + dict( + action_name='check_turn_towards', + topic='/cabot/activity_log', + topic_type='cabot_msgs/msg/Log', + condition="msg.category=='cabot/navigation' and msg.text=='turn_towards'", + timeout=60 + ), + **kwargs) + ) + + def wait_for(self, seconds, **kwargs): + self.wait(**dict( + dict( + action_name=f'wait_for({seconds})', + seconds=seconds + ), + **kwargs) + ) ### actual task needs to be waited @wait_test() diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 110f081b..8639befd 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -791,6 +791,10 @@ def __init__(self, delegate, navcog_route, anchor, **kwargs): if 'need_to_announce_follow' in kwargs: self._need_to_announce_follow = bool(kwargs['need_to_announce_follow']) + self._is_last = False + if 'is_last' in kwargs: + self._is_last = bool(kwargs['is_last']) + self._tag = None facility = None if isinstance(navcog_route[0], geojson.RouteLink): diff --git a/cabot_ui/config/cabot3-ace2_diagnostic.yaml b/cabot_ui/config/cabot3-ace2_diagnostic.yaml index 7279c4c3..e552e2a7 100644 --- a/cabot_ui/config/cabot3-ace2_diagnostic.yaml +++ b/cabot_ui/config/cabot3-ace2_diagnostic.yaml @@ -133,12 +133,11 @@ diagnostic_aggregator: - "cabot odriver_node: " # TODO Create diagnostic_updater for hesai - # hard_velodyne: - # type: diagnostic_aggregator/GenericAnalyzer - # path: "Hard: Velodyne" - # contains: - # - "velodyne_" - # remove_prefix: - # - "velodyne_drive_node: " - # - "velodyne_transform_node: " + hard_hesailidar: + type: diagnostic_aggregator/GenericAnalyzer + path: "Hard: HesaiLidar" + contains: + - "hesai" + remove_prefix: + - "hesai_lidar: "