Introducing the navigation information in info dict (#690)

* Introducing the navigation information in info dict!

* format

* fix

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()

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):

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])