From a1a1b3b8704edce7a81e139e13ee17caa001803b Mon Sep 17 00:00:00 2001
From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com>
Date: Mon, 29 Jan 2024 00:08:46 +0100
Subject: [PATCH] fix: Linter and remove test code
---
.../src/global_plan_distance_publisher.py | 2 +-
code/planning/launch/planning.launch | 4 +-
.../src/global_planner/global_planner.py | 4 +-
.../src/local_planner/motion_planning.py | 126 +++++-------------
code/planning/src/local_planner/utils.py | 35 +++--
5 files changed, 60 insertions(+), 111 deletions(-)
diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py
index 4e4cf37b..40da367c 100755
--- a/code/perception/src/global_plan_distance_publisher.py
+++ b/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(
diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch
index 72c1f88c..9dee1352 100644
--- a/code/planning/launch/planning.launch
+++ b/code/planning/launch/planning.launch
@@ -16,13 +16,13 @@
-
+
diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py
index bfed5f32..a32936ab 100755
--- a/code/planning/src/global_planner/global_planner.py
+++ b/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:
"""
diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py
index 528543be..44019360 100755
--- a/code/planning/src/local_planner/motion_planning.py
+++ b/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()
diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py
index 6483b16e..dee2fc25 100644
--- a/code/planning/src/local_planner/utils.py
+++ b/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