fix: Linter and remove test code

code/perception/src/global_plan_distance_publisher.py

@@ -47,7 +47,7 @@ def __init__(self):
         # Change comment for dev_launch
         self.global_plan_subscriber = self.new_subscription(
             CarlaRoute,
-            "/paf/" + self.role_name + "/global_plan",
+            "/carla/" + self.role_name + "/global_plan",
             self.update_global_route,
             qos_profile=1)
         # self.global_plan_subscriber = self.new_subscription(

code/planning/launch/planning.launch

@@ -16,13 +16,13 @@
            <param name="control_loop_rate" value="0.3" />
     </node>
 
-    <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">
+    <!-- <node pkg="planning" type="dev_global_route.py" name="DevGlobalRoute" output="screen">
            <param name="from_txt" value="True" />
            <param name="sampling_resolution" value="75.0" />
            <param name="routes" value="/opt/leaderboard/data/routes_devtest.xml" />
            <param name="global_route_txt" value="/code/planning/src/global_planner/global_route.txt" />
            <param name="role_name" value="hero" />
-    </node>
+    </node> -->
     <node pkg="planning" type="global_planner.py" name="PrePlanner" output="screen">
            <param name="role_name" value="hero" />
            <param name="control_loop_rate" value="1" />

code/planning/src/global_planner/global_planner.py

@@ -57,7 +57,7 @@ def __init__(self):
         # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch
         self.global_plan_sub = self.new_subscription(
             msg_type=CarlaRoute,
-            topic='/paf/' + self.role_name + '/global_plan',
+            topic='/carla/' + self.role_name + '/global_plan',
             callback=self.global_route_callback,
             qos_profile=10)
         # self.global_plan_sub = self.new_subscription(
@@ -84,7 +84,7 @@ def __init__(self):
         self.logdebug('PrePlanner-Node started')
 
         # uncomment for self.dev_load_world_info() for dev_launch
-        self.dev_load_world_info()
+        # self.dev_load_world_info()
 
     def global_route_callback(self, data: CarlaRoute) -> None:
         """

code/planning/src/local_planner/motion_planning.py

@@ -11,20 +11,18 @@
 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
 from carla_msgs.msg import CarlaSpeedometer
 import numpy as np
-import math
 
-# from behavior_agent.msg import BehaviorSpeed
 from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \
     import run_fot
 from perception.msg import Waypoint, LaneChange
-import planning
+import planning  # noqa: F401
 from behavior_agent.behaviours import behavior_speed as bs
 
 from utils import convert_to_ms, approx_obstacle_pos, \
-    hyperparameters, location_to_gps
+    hyperparameters
+
+# from scipy.spatial._kdtree import KDTree
 
-import matplotlib.pyplot as plt
-from scipy.spatial._kdtree import KDTree
 
 class MotionPlanning(CompatibleNode):
     """
@@ -178,94 +176,45 @@ def __set_current_pos(self, data: PoseStamped):
                                     data.pose.position.y,
                                     data.pose.position.z])
 
-    def change_trajectory(self, data: Bool):
-        import carla
-        import os
-        CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1')
-        CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000'))
-
-
-        client = carla.Client(CARLA_HOST, CARLA_PORT)
-
-        world = client.get_world()
-        world.wait_for_tick()
-
-
-        blueprint_library = world.get_blueprint_library()
-        # bp = blueprint_library.filter('vehicle.*')[0]
-        # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0])
-        bp = blueprint_library.filter("model3")[0]
-        for actor in world.get_actors():
-            if actor.attributes.get('role_name') == "hero":
-                ego_vehicle = actor
-                break
-
-        spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25), ego_vehicle.get_transform().rotation)
-        vehicle = world.spawn_actor(bp, spawnPoint)
-
-        vehicle.set_autopilot(False)
-        # vehicle.set_location(loc)
-        self.logerr("spawned vehicle: " + str(vehicle.get_location()))
-        # coords = vehicle.get_location()
-        # get spectator
-        spectator = world.get_spectator()
-        # set spectator to follow ego vehicle with offset
-        spectator.set_transform(
-            carla.Transform(ego_vehicle.get_location() + carla.Location(z=50),
-                            carla.Rotation(pitch=-90)))
-        print(spectator.get_location())
-        self.counter += 1
-        self.overtaking = True
+    def change_trajectory(self, distance: float):
         self.overtake_start = rospy.get_rostime()
-        # index_car = 20
         limit_waypoints = 30
         data = self.trajectory
-        self.logerr("Trajectory chagen started")
         np_array = np.array(data.poses)
-
-        obstacle_position = approx_obstacle_pos(25, self.current_heading, self.current_pos, self.current_speed)
-        trajectory_np = self.convert_pose_to_array(np_array)
-        wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1]
-        self.logerr("waypoint index obs " + str(wp))
-        self.logerr("obstacle position " + str(obstacle_position))
-        self.logerr("current position " + str(self.current_pos))
-        self.logerr("current heading " + str(self.current_heading))
-        self.logerr("current waypoint " + str(self.current_wp))
-        selection = np_array[int(self.current_wp):int(self.current_wp) + 25 + 30]
+        obstacle_position = approx_obstacle_pos(distance,
+                                                self.current_heading,
+                                                self.current_pos,
+                                                self.current_speed)
+        # trajectory_np = self.convert_pose_to_array(np_array)
+        # wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1]
+        selection = np_array[int(self.current_wp):int(self.current_wp) +
+                             int(distance + limit_waypoints)]
         waypoints = self.convert_pose_to_array(selection)
 
-        # np.save("/workspace/code/trajectory.npy", trajectory_np)
-        self.logerr("waypoints " + str(waypoints))
-        # obs = np.array([[coords.x, coords.y, coords.x, coords.y-3]]) [self.current_speed * math.cos(self.current_heading), self.current_speed * math.sin(self.current_heading)]
-        # self.logerr("obs " + str(obs))
-        pos_lat_lon = location_to_gps(0,0, self.current_pos[0], self.current_pos[1])
         initial_conditions = {
             'ps': 0,
             'target_speed': self.current_speed,
             'pos': np.array([self.current_pos[0], self.current_pos[1]]),
-            'vel': np.array([obstacle_position[2][0], obstacle_position[2][1]]),
+            'vel': np.array([obstacle_position[2][0],
+                             obstacle_position[2][1]]),
             'wp': waypoints,
-            'obs': np.array([[obstacle_position[0][0], obstacle_position[0][1], obstacle_position[1][0], obstacle_position[1][1]]])
+            'obs': np.array([[obstacle_position[0][0],
+                              obstacle_position[0][1],
+                              obstacle_position[1][0],
+                              obstacle_position[1][1]]])
         }
-
         result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \
             speeds_y, misc, costs, success = run_fot(initial_conditions,
                                                      hyperparameters)
-        self.logerr("success: " + str(success))
-        self.logerr("result_x: " + str(result_x))
-        self.logerr("result_y: " + str(result_y))
-        # fig.savefig("/workspace/code/fot.png")
-        # plt.show()
         if success:
-            self.logerr("Success")
             result = []
             for i in range(len(result_x)):
                 position = Point(result_x[i], result_y[i], 0)
                 quaternion = tf.transformations.quaternion_from_euler(0,
-                                                                        0,
-                                                                        iyaw[i])
+                                                                      0,
+                                                                      iyaw[i])
                 orientation = Quaternion(x=quaternion[0], y=quaternion[1],
-                                        z=quaternion[2], w=quaternion[3])
+                                         z=quaternion[2], w=quaternion[3])
                 pose = Pose(position, orientation)
                 pos = PoseStamped()
                 pos.header.frame_id = "global"
@@ -274,11 +223,9 @@ def change_trajectory(self, data: Bool):
             path = Path()
             path.header.stamp = rospy.Time.now()
             path.header.frame_id = "global"
-            self.logerr("result: " + str(result))
-            path.poses = list(np_array[:int(self.current_wp)]) + result + list(np_array[int(self.current_wp + 25 + 30):])
-            self.logerr("Changed traj: " + str(path.poses))
-            self.enhanced_path = path
-            self.logerr("Trajectory change finished")
+            path.poses = list(np_array[:int(self.current_wp)]) + \
+                result + list(np_array[int(self.current_wp + 25 + 30):])
+            self.trajectory = path
 
     def __set_trajectory(self, data: Path):
         """get current trajectory global planning
@@ -300,7 +247,8 @@ def convert_pose_to_array(self, poses: np.array):
         """
         result_array = np.empty((len(poses), 2))
         for pose in range(len(poses)):
-            result_array[pose] = np.array([poses[pose].pose.position.x, poses[pose].pose.position.y])
+            result_array[pose] = np.array([poses[pose].pose.position.x,
+                                           poses[pose].pose.position.y])
         return result_array
 
     def __check_emergency(self, data: Bool):
@@ -436,23 +384,9 @@ def loop(timer_event=None):
             if self.trajectory is None or self.__acc_speed is None or \
                     self.__curr_behavior is None:
                 return
-            self.velocity_pub.publish(5)
-            # self.update_target_speed(self.__acc_speed, self.__curr_behavior)
-            # self.velocity_pub.publish(10)
-
-            # if self.overtaking and rospy.get_rostime() - self.overtake_start < rospy.Duration(30):
-            #     self.logerr("overtake finished")
-            #     self.overtaking = False
-
-            # if len(self.trajectory.poses) < 1:
-            #     self.loginfo("No trajectory recieved")
-            #     return
-            if self.overtaking and self.enhanced_path is not None:
-                self.enhanced_path.header.stamp = rospy.Time.now()
-                self.traj_pub.publish(self.enhanced_path)
-            else:
-                self.trajectory.header.stamp = rospy.Time.now()
-                self.traj_pub.publish(self.trajectory)
+            self.update_target_speed(self.__acc_speed, self.__curr_behavior)
+            self.trajectory.header.stamp = rospy.Time.now()
+            self.traj_pub.publish(self.trajectory)
 
         self.new_timer(self.control_loop_rate, loop)
         self.spin()

code/planning/src/local_planner/utils.py

@@ -27,13 +27,19 @@
 }
 
 
-def location_to_gps(lat_ref, lon_ref, x, y):
-    """
-    Convert from world coordinates to GPS coordinates
-    :param lat_ref: latitude reference for the current map
-    :param lon_ref: longitude reference for the current map
-    :param location: location to translate
-    :return: dictionary with lat, lon and height
+def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float):
+    """Convert world coordinates to (lat,lon,z) coordinates
+       Copied from:
+       https://github.com/carla-simulator/scenario_runner/blob/master/srunner/tools/route_manipulation.py
+
+    Args:
+        lat_ref (float): reference lat value
+        lon_ref (float): reference lat value
+        x (float): x-Coordinate value
+        y (float): y-Coordinate value
+
+    Returns:
+        dict: Dictionary with (lat,lon,z) coordinates
     """
 
     EARTH_RADIUS_EQUA = 6378137.0   # pylint: disable=invalid-name
@@ -45,7 +51,8 @@ def location_to_gps(lat_ref, lon_ref, x, y):
     my -= y
 
     lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale)
-    lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) / math.pi - 90.0
+    lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) /\
+        math.pi - 90.0
     z = 703
 
     return {'lat': lat, 'lon': lon, 'z': z}
@@ -69,7 +76,7 @@ def approx_obstacle_pos(distance: float, heading: float,
 
     # Create distance vector with 0 rotation
     relative_position_local = np.array([distance, 0, 0])
-    
+
     # speed vector
     speed_vector = rotation_matrix.apply(np.array([speed, 0, 0]))
     # Rotate distance vector to match heading
@@ -95,5 +102,13 @@ def approx_obstacle_pos(distance: float, heading: float,
         vehicle_position_global_end, speed_vector
 
 
-def convert_to_ms(speed):
+def convert_to_ms(speed: float):
+    """Convert km/h to m/s
+
+    Args:
+        speed (float): speed in km/h
+
+    Returns:
+        float: speed in m/s
+    """
     return speed / 3.6