diff --git a/metadrive/component/navigation_module/node_network_navigation.py b/metadrive/component/navigation_module/node_network_navigation.py index 1ee2bc75a..596d08358 100644 --- a/metadrive/component/navigation_module/node_network_navigation.py +++ b/metadrive/component/navigation_module/node_network_navigation.py @@ -199,12 +199,12 @@ def update_localization(self, ego_vehicle): ego_vehicle=ego_vehicle ) + self.navi_arrow_dir = [lanes_heading1, lanes_heading2] if self._show_navi_info: # Whether to visualize little boxes in the scene denoting the checkpoints pos_of_goal = checkpoint self._goal_node_path.setPos(panda_vector(pos_of_goal[0], pos_of_goal[1], self.MARK_HEIGHT)) self._goal_node_path.setH(self._goal_node_path.getH() + 3) - self.navi_arrow_dir = [lanes_heading1, lanes_heading2] dest_pos = self._dest_node_path.getPos() self._draw_line_to_dest(start_position=ego_vehicle.position, end_position=(dest_pos[0], dest_pos[1])) navi_pos = self._goal_node_path.getPos() diff --git a/metadrive/component/vehicle/base_vehicle.py b/metadrive/component/vehicle/base_vehicle.py index 943bfdfdf..cb4494d61 100644 --- a/metadrive/component/vehicle/base_vehicle.py +++ b/metadrive/component/vehicle/base_vehicle.py @@ -250,6 +250,30 @@ def after_step(self): "policy": my_policy.name if my_policy is not None else my_policy } ) + + lanes_heading = self.navigation.navi_arrow_dir + lane_0_heading = lanes_heading[0] + lane_1_heading = lanes_heading[1] + navigation_straight = False + navigation_turn_left = False + navigation_turn_right = False + if abs(wrap_to_pi(lane_0_heading - lane_1_heading)) < 10 / 180 * math.pi: + navigation_straight = True + else: + dir_0 = np.array([math.cos(lane_0_heading), math.sin(lane_0_heading), 0]) + dir_1 = np.array([math.cos(lane_1_heading), math.sin(lane_1_heading), 0]) + cross_product = np.cross(dir_1, dir_0) + navigation_turn_left = True if cross_product[-1] < 0 else False + navigation_turn_right = not navigation_turn_left + step_info.update( + { + "navigation_command": "forward" if navigation_straight else + ("left" if navigation_turn_left else "right"), + "navigation_forward": navigation_straight, + "navigation_left": navigation_turn_left, + "navigation_right": navigation_turn_right + } + ) return step_info def _out_of_route(self): diff --git a/metadrive/examples/drive_in_single_agent_env.py b/metadrive/examples/drive_in_single_agent_env.py index d4d12aec7..52007c684 100755 --- a/metadrive/examples/drive_in_single_agent_env.py +++ b/metadrive/examples/drive_in_single_agent_env.py @@ -66,6 +66,7 @@ "Keyboard Control": "W,A,S,D", } ) + print("Navigation information: ", info["navigation_command"]) if args.observation == "rgb_camera": cv2.imshow('RGB Image in Observation', o["image"][..., -1])