From 23872d09f425e9a521c355513e455194adb8909b Mon Sep 17 00:00:00 2001 From: MaxJa4 Date: Sun, 28 Jan 2024 15:33:50 +0100 Subject: [PATCH] feat: Expand dev launch with vehicle-spawn + fixed spectator --- build/CustomCarlaSettings.ini | 1 + build/docker-compose.yml | 12 +- code/agent/launch/agent.launch | 2 +- code/agent/launch/dev.launch | 8 +- code/test-route/CMakeLists.txt | 202 +++++++++++++++++++++++ code/test-route/launch/test-route.launch | 14 ++ code/test-route/package.xml | 59 +++++++ code/test-route/src/test_route.py | 165 ++++++++++++++++++ 8 files changed, 455 insertions(+), 8 deletions(-) create mode 100644 build/CustomCarlaSettings.ini create mode 100644 code/test-route/CMakeLists.txt create mode 100644 code/test-route/launch/test-route.launch create mode 100644 code/test-route/package.xml create mode 100755 code/test-route/src/test_route.py diff --git a/build/CustomCarlaSettings.ini b/build/CustomCarlaSettings.ini new file mode 100644 index 00000000..597dbdd8 --- /dev/null +++ b/build/CustomCarlaSettings.ini @@ -0,0 +1 @@ +bThrottleCPUWhenNotForeground=False diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 2e9b1d57..23ee3795 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -21,9 +21,7 @@ services: # based on https://github.com/ll7/paf21-1/blob/master/scenarios/docker-carla-sim-compose.yml carla-simulator: - command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound - # Use this image version to enable instance segmentation cameras: (it does not match the leaderboard version) - # image: carlasim/carla:0.9.14 + command: /bin/bash CarlaUE4.sh -quality-level=Epic -world-port=2000 -resx=800 -resy=600 -nosound -carla-settings="/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini" image: ghcr.io/una-auxme/paf23:leaderboard-2.0 init: true deploy: @@ -34,10 +32,13 @@ services: - 2000 - 2001 - 2002 + environment: + - XDG_RUNTIME_DIR=/tmp/runtime-carla networks: - carla volumes: - /tmp/.X11-unix:/tmp/.X11-unix + - ./CustomCarlaSettings.ini:/home/carla/CarlaUE4/Config/CustomCarlaSettings.ini roscore: image: ros:noetic @@ -61,8 +62,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 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && 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 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && 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: @@ -70,6 +71,7 @@ services: - ROS_MASTER_URI=http://roscore:11311 - CARLA_SIM_HOST=carla-simulator - ROS_HOSTNAME=agent + - XDG_RUNTIME_DIR=/tmp/runtime-carla volumes: - /tmp/.X11-unix:/tmp/.X11-unix # if you change the volume here also change the copy command diff --git a/code/agent/launch/agent.launch b/code/agent/launch/agent.launch index d076596d..3c99c354 100644 --- a/code/agent/launch/agent.launch +++ b/code/agent/launch/agent.launch @@ -1,6 +1,6 @@ - + diff --git a/code/agent/launch/dev.launch b/code/agent/launch/dev.launch index 5b0a595e..071e9813 100644 --- a/code/agent/launch/dev.launch +++ b/code/agent/launch/dev.launch @@ -15,7 +15,7 @@ - + @@ -32,7 +32,11 @@ - + + + + + diff --git a/code/test-route/CMakeLists.txt b/code/test-route/CMakeLists.txt new file mode 100644 index 00000000..0b73ff9d --- /dev/null +++ b/code/test-route/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 3.0.2) +project(test-route) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES test-route +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/test-route.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/test-route_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_test-route.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/code/test-route/launch/test-route.launch b/code/test-route/launch/test-route.launch new file mode 100644 index 00000000..de985c6b --- /dev/null +++ b/code/test-route/launch/test-route.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/code/test-route/package.xml b/code/test-route/package.xml new file mode 100644 index 00000000..592de860 --- /dev/null +++ b/code/test-route/package.xml @@ -0,0 +1,59 @@ + + + test-route + 0.0.0 + The test-route package + + + + + carla + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/code/test-route/src/test_route.py b/code/test-route/src/test_route.py new file mode 100755 index 00000000..72709778 --- /dev/null +++ b/code/test-route/src/test_route.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +import os +from time import sleep +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +import carla +# from carla import command +import rospy +import random + + +class TestRoute(CompatibleNode): + def __init__(self): + super(TestRoute, self).__init__('testRoute') + + self.control_loop_rate = self.get_param('control_loop_rate', 0.025) + self.role_name = self.get_param('role_name', 'ego_vehicle') + self.follow_hero = self.get_param('follow_hero', True) + self.vehicle_number = self.get_param('vehicle_number', 50) + self.only_cars = self.get_param('only_cars', False) + self.disable_vehicle_lane_change = \ + self.get_param('disable_vehicle_lane_change', False) + + host = os.environ.get('CARLA_SIM_HOST', 'localhost') + + self.client = carla.Client(host, 2000) + self.client.set_timeout(60.0) + + self.traffic_manager = self.client.get_trafficmanager(8000) + self.traffic_manager.set_synchronous_mode(True) + self.traffic_manager.set_hybrid_physics_mode(True) + self.traffic_manager.set_hybrid_physics_radius(70.0) + self.traffic_manager.set_global_distance_to_leading_vehicle(1.0) + self.traffic_manager.set_random_device_seed(0) + self.traffic_manager.set_respawn_dormant_vehicles(True) + self.traffic_manager.set_boundaries_respawn_dormant_vehicles(25, 700) + self.traffic_manager.global_percentage_speed_difference(10.0) + + self.world = self.client.get_world() + + settings = self.world.get_settings() + settings.tile_stream_distance = 650 + settings.actor_active_distance = 650 + settings.spectator_as_ego = False + self.world.apply_settings(settings) + + self.wait_for_hero() + + self.spectator = self.world.get_spectator() + + def spawn_traffic(self): + self.loginfo('Spawning traffic') + + spawn_points = self.world.get_map().get_spawn_points() + hero_location = self.hero.get_location() + spawn_points.sort(key=lambda x: x.location.distance(hero_location)) + + blueprints = self.world.get_blueprint_library().filter('vehicle.*') + if self.only_cars: + blueprints = [b for b in blueprints + if int(b.get_attribute('number_of_wheels')) == 4] + vehicles = [] + max_vehicles = min([self.vehicle_number, len(spawn_points)]) + + # SpawnActor = command.SpawnActor + # SetAutopilot = command.SetAutopilot + # FutureActor = command.FutureActor + + # batch = [] + for _, transform in enumerate(spawn_points[:max_vehicles]): + blueprint = random.choice(blueprints) + + if blueprint.has_attribute('driver_id'): + driver_id = random.choice(blueprint.get_attribute('driver_id') + .recommended_values) + blueprint.set_attribute('driver_id', driver_id) + + if blueprint.has_attribute('color'): + color = random.choice(blueprint.get_attribute('color') + .recommended_values) + blueprint.set_attribute('color', color) + + blueprint.set_attribute('role_name', 'autopilot') + + vehicle = self.world.try_spawn_actor(blueprint, transform) + + # batch.append(SpawnActor(blueprint, transform) + # .then(SetAutopilot(FutureActor, True))) + + if vehicle is not None: + vehicles.append(vehicle) + vehicle.set_autopilot(True) + + if self.disable_vehicle_lane_change: + self.traffic_manager.auto_lange_change(vehicle, False) + + # for response in self.client.apply_batch_sync(batch, False): + # if response.error: + # print(response.error) + # else: + # vehicles.append(response.actor_id) + + self.loginfo('Spawned {} vehicles'.format(len(vehicles))) + + def wait_for_hero(self): + while not rospy.is_shutdown(): + actors = self.world.get_actors() + if not actors: + continue + + self.hero = [a for a in actors + if 'role_name' in a.attributes and + a.attributes.get('role_name') == self.role_name] + if self.hero: + self.hero = self.hero[0] + break + + def set_spectator(self): + transform = self.hero.get_transform() + location = carla.Location(x=transform.location.x, + y=transform.location.y, + z=transform.location.z + 2) + self.spectator.set_transform( + carla.Transform( + location, carla.Rotation( + pitch=transform.rotation.pitch - 15, + yaw=transform.rotation.yaw, + roll=transform.rotation.roll + ) + ) + ) + + def run(self): + self.loginfo('Test-Route node running') + + def loop(timer_event=None): + self.set_spectator() + + if self.follow_hero: + self.loginfo('Following hero') + self.new_timer(self.control_loop_rate, loop) + else: + self.loginfo('Not following hero, setting spectator only once') + self.set_spectator() + + sleep(5) + self.spawn_traffic() + + self.spin() + + +def main(args=None): + roscomp.init('testRoute', args=args) + + try: + node = TestRoute() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() + + +if __name__ == '__main__': + main()