diff --git a/build/Taskfile b/build/Taskfile index ef40a638..9ad466af 100644 --- a/build/Taskfile +++ b/build/Taskfile @@ -32,7 +32,7 @@ task:shell() { ########################################## task:install() { task:install:git_hooks - task:gitconfig:copy + #task:gitconfig:copy install:gpu-support docker:install } diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 472f912d..4580d9ea 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -54,8 +54,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" + #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" logging: driver: "local" environment: diff --git a/code/acting/src/acting/pure_pursuit_controller.py b/code/acting/src/acting/pure_pursuit_controller.py index 46328f84..90d8928b 100755 --- a/code/acting/src/acting/pure_pursuit_controller.py +++ b/code/acting/src/acting/pure_pursuit_controller.py @@ -8,7 +8,7 @@ from nav_msgs.msg import Path from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber -from std_msgs.msg import Float32, Float32MultiArray +from std_msgs.msg import Float32 from acting.msg import Debug from helper_functions import vector_angle @@ -65,18 +65,12 @@ def __init__(self): f"/paf/{self.role_name}/debug", qos_profile=1) - self.max_speed_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/speed_limit", - qos_profile=1) - self.__position: (float, float) = None # x, y self.__last_pos: (float, float) = None self.__path: Path = None self.__heading: float = None self.__velocity: float = None self.__tp_idx: int = 0 # target waypoint index - self.__od_speed: Float32MultiArray = None # error when there are no targets def run(self): diff --git a/code/acting/src/acting/stanley_controller.py b/code/acting/src/acting/stanley_controller.py index 8cac099e..8d55a9bd 100755 --- a/code/acting/src/acting/stanley_controller.py +++ b/code/acting/src/acting/stanley_controller.py @@ -11,7 +11,7 @@ # from ros_compatibility.qos import QoSProfile, DurabilityPolicy from rospy import Publisher, Subscriber # from sensor_msgs.msg import NavSatFix, Imu -from std_msgs.msg import Float32, Float32MultiArray # Bool +from std_msgs.msg import Float32 # Bool from acting.msg import StanleyDebug from helper_functions import vector_angle @@ -62,18 +62,12 @@ def __init__(self): f"/paf/{self.role_name}/stanley_debug", qos_profile=1) - self.max_speed_pub: Publisher = self.new_publisher( - Float32, - f"/paf/{self.role_name}/speed_limit", - qos_profile=1) - self.__position: (float, float) = None # x, y self.__last_pos: (float, float) = None self.__path: Path = None self.__heading: float = None self.__velocity: float = None self.__tp_idx: int = 0 # target waypoint index - self.__od_speed: Float32MultiArray = None # error when there are no targets def run(self): diff --git a/code/acting/src/acting/vehicle_controller.py b/code/acting/src/acting/vehicle_controller.py index 4973e858..2f5be839 100755 --- a/code/acting/src/acting/vehicle_controller.py +++ b/code/acting/src/acting/vehicle_controller.py @@ -98,7 +98,7 @@ def __init__(self): self.__velocity: float = 0.0 self.__pure_pursuit_steer: float = 0.0 self.__stanley_steer: float = 0.0 - self.__current_steer: float = 0.0 # check emergency behaviour + self.__current_steer: float = 0.0 # todo: check emergency behaviour def run(self): """ @@ -107,7 +107,7 @@ def run(self): """ self.status_pub.publish(True) self.loginfo('VehicleController node running') - pid = PID(0.85, 0.1, 0.1, setpoint=0) # random values -> tune + pid = PID(0.85, 0.1, 0.1, setpoint=0) # random values -> todo: tune pid.output_limits = (-MAX_STEER_ANGLE, MAX_STEER_ANGLE) def loop(timer_event=None) -> None: diff --git a/code/acting/src/acting/velocity_controller.py b/code/acting/src/acting/velocity_controller.py index b29d6928..39882333 100755 --- a/code/acting/src/acting/velocity_controller.py +++ b/code/acting/src/acting/velocity_controller.py @@ -9,7 +9,7 @@ from nav_msgs.msg import Path # TODO put back to 36 when controller can handle it -SPEED_LIMIT_DEFAULT: float = 8 # 36.0 +SPEED_LIMIT_DEFAULT: float = 6 # 36.0 class VelocityController(CompatibleNode): @@ -142,8 +142,6 @@ def loop(timer_event=None): self.logerr("Velocity controller doesn't support backward " "driving yet.") return - # print(f"v_soll = min({self.__max_velocity}, {self.__max_tree_v}, - # {self.__speed_limit}) | v_ist = {self.__current_velocity} ") v = min(self.__max_velocity, self.__max_tree_v) v = min(v, self.__speed_limit) diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index 72662052..d65936e6 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -10,7 +10,7 @@ "id": "hero", "role_name": "hero", "spawn_point": { - "x": 986.0, + "x": 984.5, "y": -5442.0, "z": 371.0, "yaw": -90.0, diff --git a/code/agent/src/agent/agent.py b/code/agent/src/agent/agent.py index 46d06855..e2732f00 100755 --- a/code/agent/src/agent/agent.py +++ b/code/agent/src/agent/agent.py @@ -27,7 +27,7 @@ def sensors(self): 'yaw': 0.0, 'width': 300, 'height': 200, 'fov': 100}, {'type': 'sensor.lidar.ray_cast', 'id': 'LIDAR', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, - 'yaw': -45.0}, + 'yaw': 0.0}, {'type': 'sensor.other.radar', 'id': 'RADAR', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, 'yaw': -45.0, 'horizontal_fov': 30, 'vertical_fov': 30}, @@ -35,7 +35,7 @@ def sensors(self): 'x': 0.7, 'y': -0.4, 'z': 1.60}, {'type': 'sensor.other.imu', 'id': 'IMU', 'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0, - 'yaw': -45.0}, + 'yaw': 0.0}, {'type': 'sensor.opendrive_map', 'id': 'OpenDRIVE', 'reading_frequency': 1}, {'type': 'sensor.speedometer', 'id': 'Speed'}, diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 4af827b2..6c663cd0 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -105,6 +105,10 @@ def distance(a, b): if current_distance < 2.5 or next_distance < current_distance: self.road_options.pop(0) self.global_route.pop(0) + if self.road_options[0] in {5, 6} and \ + self.road_options[0] == self.road_options[1]: + self.road_options.pop(0) + self.global_route.pop(0) print(f"next road option = {self.road_options[0]}") def update_global_route(self, route): diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index 3f150339..2e94697d 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,11 +1,12 @@ +