pid agent fixed

augcog · Mar 4, 2021 · 8539366 · 8539366
1 parent 20f2a5a
commit 8539366
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 21 deletions.
diff --git a/ROAR/control_module/pid_controller.py b/ROAR/control_module/pid_controller.py
@@ -105,31 +105,25 @@ def run_in_series(self, next_waypoint: Transform, **kwargs) -> float:
             lat_control
         """
         # calculate a vector that represent where you are going
-        v_begin = self.agent.vehicle.transform.location
-        v_end = v_begin + Location(
-            x=math.cos(math.radians(self.agent.vehicle.transform.rotation.pitch)),
-            y=0,
-            z=math.sin(math.radians(self.agent.vehicle.transform.rotation.pitch)),
-        )
-        v_vec = np.array([v_end.x - v_begin.x, 0, v_end.z - v_begin.z])
-        v_vec = np.array([v_end.x - v_begin.x, 0, v_end.z - v_begin.z])
+        v_begin = self.agent.vehicle.transform.location.to_array()
+        direction_vector = np.array([np.cos(np.radians(self.agent.vehicle.transform.rotation.yaw)),
+                                     0,
+                                     np.sin(np.radians(self.agent.vehicle.transform.rotation.yaw))])
+        v_end = v_begin + direction_vector
+
+        v_vec = np.array([v_end[0] - v_begin[0], 0, - (v_end[2] - v_begin[2])])
         # calculate error projection
         w_vec = np.array(
             [
-                next_waypoint.location.x - v_begin.x,
+                next_waypoint.location.x - v_begin[0],
                 0,
-                next_waypoint.location.z - v_begin.z,
+                next_waypoint.location.z - v_begin[2],
             ]
         )
-        _dot = math.acos(
-            np.clip(
-                np.dot(v_vec, w_vec) / (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)),
-                -1.0,
-                1.0,
-            )
-        )
+
+        _dot = math.acos(np.clip(np.dot(v_vec, w_vec) / (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0))
         _cross = np.cross(v_vec, w_vec)
-        if _cross[1] > 0:
+        if _cross[1] < 0:
             _dot *= -1
         self._error_buffer.append(_dot)
         if len(self._error_buffer) >= 2:
@@ -144,4 +138,5 @@ def run_in_series(self, next_waypoint: Transform, **kwargs) -> float:
         lat_control = float(
             np.clip((k_p * _dot) + (k_d * _de) + (k_i * _ie), self.steering_boundary[0], self.steering_boundary[1])
         )
+        print(f"lat_control: {round(lat_control, 3)} | v_vec = {v_vec} | w_vec = {w_vec}")
         return lat_control
diff --git a/runner_sim.py b/runner_sim.py
@@ -14,7 +14,7 @@
 # from ROAR.agent_module.point_cloud_agent import PointCloudAgent
 from ROAR.configurations.configuration import Configuration as AgentConfig
 from ROAR.agent_module.special_agents.waypoint_generating_agent import WaypointGeneratigAgent
-
+from ROAR.agent_module.pid_agent import PIDAgent
 
 def main():
     """Starts game loop"""
@@ -26,8 +26,8 @@ def main():
                                npc_agent_class=PurePursuitAgent)
     try:
         my_vehicle = carla_runner.set_carla_world()
-        agent = WaypointGeneratigAgent(vehicle=my_vehicle, agent_settings=agent_config)
-        carla_runner.start_game_loop(agent=agent, use_manual_control=True)
+        agent = PIDAgent(vehicle=my_vehicle, agent_settings=agent_config)
+        carla_runner.start_game_loop(agent=agent, use_manual_control=False)
     except Exception as e:
         logging.error(f"Something bad happened during initialization: {e}")
         carla_runner.on_finish()