├── .gitignore ├── CMakeLists.txt ├── action ├── AgentPlan.action └── EgoPlan.action ├── cmake └── FindGooglePerfTools.cmake ├── launch ├── agents_lane_following.launch ├── autonomous_driving.launch ├── carla_server.sh ├── ego_idm_lattice_planning.launch ├── ego_lane_following.launch ├── ego_slc_lattice_planning.launch ├── ego_spatiotemporal_lattice_planning.launch ├── fixed_scenario_simulator.launch ├── no_traffic_simulator.launch ├── random_traffic_experiment.sh └── random_traffic_simulator.launch ├── msg ├── BoundingBox.msg ├── TrafficSnapshot.msg ├── Vehicle.msg └── VehicleSpeed.msg ├── package.xml ├── readme.md ├── rosdoc.yaml ├── rviz └── config.rviz ├── scripts ├── braking_scenario_data_analysis.py ├── intelligent_driver_model.py ├── lane_change_speed_control.py ├── lane_keep_speed_control.py ├── left_lane_change_path ├── merging_scenario_data_analysis.py ├── path_generation.cpp ├── path_optimization.h ├── path_test.cpp ├── random_traffic_data_analysis.py ├── road_map.patch ├── software_framework.png ├── software_framework_usage.md ├── weight_functions.py └── workspace_setup.md └── src ├── controller └── vehicle_controller.h ├── node ├── CMakeLists.txt ├── common │ ├── convert_to_visualization_msgs.cpp │ └── convert_to_visualization_msgs.h ├── planner │ ├── CMakeLists.txt │ ├── agents_lane_following_node.cpp │ ├── agents_lane_following_node.h │ ├── ego_idm_lattice_planning_node.cpp │ ├── ego_idm_lattice_planning_node.h │ ├── ego_lane_following_node.cpp │ ├── ego_lane_following_node.h │ ├── ego_slc_lattice_planning_node.cpp │ ├── ego_slc_lattice_planning_node.h │ ├── ego_spatiotemporal_lattice_planning_node.cpp │ ├── ego_spatiotemporal_lattice_planning_node.h │ ├── planning_node.cpp │ └── planning_node.h └── simulator │ ├── CMakeLists.txt │ ├── fixed_scenario_node.cpp │ ├── fixed_scenario_node.h │ ├── no_traffic_node.cpp │ ├── no_traffic_node.h │ ├── random_traffic_node.cpp │ ├── random_traffic_node.h │ ├── simulator_node.cpp │ └── simulator_node.h ├── planner ├── CMakeLists.txt ├── common │ ├── fast_waypoint_map.h │ ├── intelligent_driver_model.h │ ├── kn_path_gen.h │ ├── lattice.h │ ├── lattice_inst.h │ ├── snapshot.cpp │ ├── snapshot.h │ ├── traffic_lattice.cpp │ ├── traffic_lattice.h │ ├── traffic_manager.cpp │ ├── traffic_manager.h │ ├── traffic_simulator.cpp │ ├── traffic_simulator.h │ ├── utils.cpp │ ├── utils.h │ ├── vehicle.h │ ├── vehicle_path.cpp │ ├── vehicle_path.h │ ├── vehicle_path_planner.h │ ├── vehicle_speed_planner.h │ └── waypoint_lattice.h ├── idm_lattice_planner │ ├── idm_lattice_planner.cpp │ └── idm_lattice_planner.h ├── lane_follower │ └── lane_follower.h ├── slc_lattice_planner │ ├── slc_lattice_planner.cpp │ └── slc_lattice_planner.h ├── spatiotemporal_lattice_planner │ ├── spatiotemporal_lattice_planner.cpp │ └── spatiotemporal_lattice_planner.h └── tests │ ├── CMakeLists.txt │ ├── intelligent_driver_model.py │ └── test_intelligent_driver_model.cpp └── router ├── CMakeLists.txt ├── common └── router.h └── loop_router ├── loop_router.cpp └── loop_router.h /.gitignore: -------------------------------------------------------------------------------- 1 | doc/ 2 | backup/ 3 | *.pyc 4 | scripts/path_generation 5 | 6 | # Prerequisites 7 | *.d 8 | 9 | # Compiled Object files 10 | *.slo 11 | *.lo 12 | *.o 13 | *.obj 14 | 15 | # Precompiled Headers 16 | *.gch 17 | *.pch 18 | 19 | # Compiled Dynamic libraries 20 | *.so 21 | *.dylib 22 | *.dll 23 | 24 | # Fortran module files 25 | *.mod 26 | *.smod 27 | 28 | # Compiled Static libraries 29 | *.lai 30 | *.la 31 | *.a 32 | *.lib 33 | 34 | # Executables 35 | *.exe 36 | *.out 37 | *.app 38 | -------------------------------------------------------------------------------- /action/AgentPlan.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | # Stores the data for all the vehicles 3 | # (both the ego and agent vehicles) with the policy speed included. 4 | std_msgs/Header header 5 | float64 simulation_time 6 | conformal_lattice_planner/TrafficSnapshot snapshot 7 | --- 8 | # Result 9 | std_msgs/Header header 10 | bool success 11 | # The new desired states for all agents in the snapshot. 12 | conformal_lattice_planner/Vehicle[] agents 13 | --- 14 | # Feedback 15 | # TODO: what could a meaningful feedback? 16 | std_msgs/Header header 17 | 18 | -------------------------------------------------------------------------------- /action/EgoPlan.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | # Stores the data for all the vehicles 3 | # (both the ego and agent vehicles) with the policy speed included. 4 | std_msgs/Header header 5 | float64 simulation_time 6 | conformal_lattice_planner/TrafficSnapshot snapshot 7 | # Leading vehicles. 8 | conformal_lattice_planner/Vehicle front_leader 9 | float64 front_distance 10 | conformal_lattice_planner/Vehicle left_front_leader 11 | float64 left_front_distance 12 | conformal_lattice_planner/Vehicle right_front_leader 13 | float64 right_front_distance 14 | # Following vehicles. 15 | conformal_lattice_planner/Vehicle back_follower 16 | float64 back_distance 17 | conformal_lattice_planner/Vehicle left_back_follower 18 | float64 left_back_distance 19 | conformal_lattice_planner/Vehicle right_back_follower 20 | float64 right_back_distance 21 | 22 | #conformal_lattice_planner/Vehicle leader 23 | #float64 leading_distance 24 | #conformal_lattice_planner/Vehicle follower 25 | #float64 following_distance 26 | --- 27 | # Result 28 | uint8 LANE_KEEP = 0 29 | uint8 LEFT_LANE_CHANGE = 1 30 | uint8 RIGHT_LANE_CHANGE = 2 31 | uint8 UNKNOWN = 3 32 | 33 | std_msgs/Header header 34 | bool success 35 | # The type of action 36 | int8 path_type 37 | # The new desired state of the ego. 38 | conformal_lattice_planner/Vehicle ego 39 | # Planning time. 40 | float64 planning_time 41 | --- 42 | # Feedback 43 | # TODO: what could a meaningful feedback? 44 | std_msgs/Header header 45 | 46 | -------------------------------------------------------------------------------- /cmake/FindGooglePerfTools.cmake: -------------------------------------------------------------------------------- 1 | # -*- cmake -*- 2 | 3 | # - Find Google perftools 4 | # Find the Google perftools includes and libraries 5 | # This module defines 6 | # GOOGLE_PERFTOOLS_INCLUDE_DIR, where to find heap-profiler.h, etc. 7 | # GOOGLE_PERFTOOLS_FOUND, If false, do not try to use Google perftools. 8 | # also defined for general use are 9 | # TCMALLOC_LIBRARIES, where to find the tcmalloc library. 10 | # STACKTRACE_LIBRARIES, where to find the stacktrace library. 11 | # PROFILER_LIBRARIES, where to find the profiler library. 12 | 13 | FIND_PATH(GOOGLE_PERFTOOLS_INCLUDE_DIR google/heap-profiler.h 14 | /usr/local/include 15 | /usr/include 16 | ) 17 | 18 | SET(TCMALLOC_NAMES ${TCMALLOC_NAMES} tcmalloc) 19 | FIND_LIBRARY(TCMALLOC_LIBRARY 20 | NAMES ${TCMALLOC_NAMES} 21 | PATHS /usr/lib /usr/local/lib 22 | ) 23 | 24 | IF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 25 | SET(TCMALLOC_LIBRARIES ${TCMALLOC_LIBRARY}) 26 | SET(GOOGLE_PERFTOOLS_FOUND "YES") 27 | ELSE (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 28 | SET(GOOGLE_PERFTOOLS_FOUND "NO") 29 | ENDIF (TCMALLOC_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 30 | 31 | SET(STACKTRACE_NAMES ${STACKTRACE_NAMES} stacktrace) 32 | FIND_LIBRARY(STACKTRACE_LIBRARY 33 | NAMES ${STACKTRACE_NAMES} 34 | PATHS /usr/lib /usr/local/lib 35 | ) 36 | 37 | IF (STACKTRACE_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 38 | SET(STACKTRACE_LIBRARIES ${STACKTRACE_LIBRARY}) 39 | ENDIF (STACKTRACE_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 40 | 41 | SET(PROFILER_NAMES ${PROFILER_NAMES} profiler) 42 | FIND_LIBRARY(PROFILER_LIBRARY 43 | NAMES ${PROFILER_NAMES} 44 | PATHS /usr/lib /usr/local/lib 45 | ) 46 | 47 | IF (PROFILER_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 48 | SET(PROFILER_LIBRARIES ${PROFILER_LIBRARY}) 49 | ENDIF (PROFILER_LIBRARY AND GOOGLE_PERFTOOLS_INCLUDE_DIR) 50 | 51 | IF (GOOGLE_PERFTOOLS_FOUND) 52 | IF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) 53 | MESSAGE(STATUS "Found Google perftools: ${GOOGLE_PERFTOOLS_LIBRARIES}") 54 | ENDIF (NOT GOOGLE_PERFTOOLS_FIND_QUIETLY) 55 | ELSE (GOOGLE_PERFTOOLS_FOUND) 56 | IF (GOOGLE_PERFTOOLS_FIND_REQUIRED) 57 | MESSAGE(FATAL_ERROR "Could not find Google perftools library") 58 | ENDIF (GOOGLE_PERFTOOLS_FIND_REQUIRED) 59 | ENDIF (GOOGLE_PERFTOOLS_FOUND) 60 | 61 | MARK_AS_ADVANCED( 62 | TCMALLOC_LIBRARY 63 | STACKTRACE_LIBRARY 64 | PROFILER_LIBRARY 65 | GOOGLE_PERFTOOLS_INCLUDE_DIR 66 | ) 67 | 68 | 69 | -------------------------------------------------------------------------------- /launch/agents_lane_following.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /launch/autonomous_driving.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /launch/carla_server.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | trap "trap - TERM && kill -- -$$" INT TERM EXIT 3 | 4 | CWD=`pwd` 5 | cd ${Carla_DIST} 6 | 7 | # Start Carla server without display 8 | echo "Start CARLA server (-opengl -quality-level=Low)." 9 | DISPLAY= 10 | ./CarlaUE4.sh -opengl -quality-level=Low & 11 | #./CarlaUE4.sh -opengl -quality-level=Epic& 12 | SERVER_PID=$! 13 | 14 | sleep 5s 15 | 16 | # Change the the map 17 | echo "Change map to Town04." 18 | cd PythonAPI/util 19 | python config.py --map Town04 20 | 21 | sleep 3s 22 | 23 | # Disable rendering 24 | echo "Disable rendering of the CARLA server (GPU related data won't work)." 25 | python config.py --no-rendering 26 | 27 | sleep 3s 28 | 29 | # Set the simulation step 30 | echo "Simulation step is set to 0.05s (20Hz)." 31 | python config.py --delta-seconds 0.05 32 | 33 | # Do not exit until 34 | echo "CARLA server initialization finishes." 35 | echo "Ctrl+c to terminate the server." 36 | wait ${SERVER_PID} 37 | 38 | cd ${CWD} 39 | -------------------------------------------------------------------------------- /launch/ego_idm_lattice_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/ego_lane_following.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /launch/ego_slc_lattice_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /launch/ego_spatiotemporal_lattice_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /launch/fixed_scenario_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/no_traffic_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/random_traffic_experiment.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | trap "trap - TERM && kill -- -$$" INT TERM EXIT 3 | 4 | method="ego_slc_lattice_planner" 5 | 6 | episode_time=0.0 7 | episode_timeout=0 8 | max_episode_time=500.0 9 | 10 | experiment_time=0.0 11 | experiment_timeout=0 12 | max_experiment_time=3600.0 13 | 14 | while [ $experiment_timeout -le 0 ]; do 15 | # Start the carla server. 16 | echo "Start carla server." 17 | ./carla_server.sh 18 | sleep 10 19 | 20 | # Start the ros clients. 21 | echo "Start ROS nodes." 22 | roslaunch conformal_lattice_planner autonomous_driving.launch \ 23 | random_traffic:=true \ 24 | $method:=true \ 25 | agents_lane_follower:=true \ 26 | record_bags:=true& 27 | sleep 10 28 | 29 | # Keep track of the simulation time of this episode. 30 | while [ $episode_timeout -le 0 ]; do 31 | sleep 5 32 | current_time=$(rosservice call /carla/carla_simulator/simulation_time | tr '\n' ' ' | cut -d' ' -f4 | cut -d'"' -f2) 33 | if [ ${#current_time} -le 0 ]; then 34 | echo "ROS service /carla/carla_simulator/simulation_time unavailable" 35 | break; 36 | else 37 | episode_time=$current_time 38 | fi 39 | 40 | episode_timeout=$(echo "$episode_time>$max_episode_time" | bc -l) 41 | echo "episode time=$episode_time episode timeout=$episode_timeout" 42 | echo "experiment time=$experiment_time experiment timeout=$experiment_timeout" 43 | done 44 | 45 | # Update the total simulation time 46 | experiment_time=$(echo "$experiment_time+$episode_time" | bc -l) 47 | experiment_timeout=$(echo "$experiment_time>$max_experiment_time" | bc -l) 48 | episode_time=0.0 49 | episode_timeout=0 50 | 51 | # Kill all ROS nodes in case some nodes has not died yet. 52 | echo "Stop all ROS nodes" 53 | rosnode kill -a 54 | sleep 10 55 | 56 | # Kill the carla server. 57 | # FIXME: Need to restart and kill again. 58 | echo "Stop carla server" 59 | kill $(pidof CarlaUE4-Linux-Shipping) 60 | sleep 5 61 | ./carla_server.sh 62 | kill $(pidof CarlaUE4-Linux-Shipping) 63 | sleep 5 64 | 65 | done 66 | -------------------------------------------------------------------------------- /launch/random_traffic_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /msg/BoundingBox.msg: -------------------------------------------------------------------------------- 1 | # Defines the 3-D bounding box of a vehicle. 2 | # The message definition is compatible with the Carla Cpp API carla::geom::BoundingBox. 3 | geometry_msgs/Point extent 4 | geometry_msgs/Point location 5 | -------------------------------------------------------------------------------- /msg/TrafficSnapshot.msg: -------------------------------------------------------------------------------- 1 | # Ego vehicle 2 | conformal_lattice_planner/Vehicle ego 3 | # Agent vehicles 4 | conformal_lattice_planner/Vehicle[] agents 5 | -------------------------------------------------------------------------------- /msg/Vehicle.msg: -------------------------------------------------------------------------------- 1 | # ID of the vehicle 2 | uint64 id 3 | # Bounding box of the vehicle. 4 | conformal_lattice_planner/BoundingBox bounding_box 5 | # Left-hand transform of the vehicle, compatible with Carla simulator. 6 | geometry_msgs/Pose transform 7 | # Speed 8 | float64 speed 9 | # Acceleration 10 | float64 acceleration 11 | # Curvature of the path where the vehicle is at. 12 | float64 curvature 13 | # Policy speed 14 | float64 policy_speed 15 | 16 | -------------------------------------------------------------------------------- /msg/VehicleSpeed.msg: -------------------------------------------------------------------------------- 1 | # ID of the vehicle. 2 | uint64 id 3 | float64 speed 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | conformal_lattice_planner 4 | 0.0.0 5 | Lattice motion planning algorithms for self-driving vehicles 6 | 7 | 8 | 9 | 10 | Ke Sun 11 | 12 | 13 | 14 | 15 | 16 | BSD 3-Clause 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Ke Sun 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | roscpp 54 | 55 | tf2 56 | tf2_ros 57 | tf2_geometry_msgs 58 | 59 | std_msgs 60 | std_srvs 61 | geometry_msgs 62 | visualization_msgs 63 | image_transport 64 | 65 | actionlib 66 | actionlib_msgs 67 | 68 | message_generation 69 | message_runtime 70 | 71 | rosunit 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | # Conformal Lattice Planner 2 | 3 | This respository provides conformal lattice planner C++ implementations for autonomous driving tasks. The software is build upon [Carla](http://carla.org/) and [ROS](https://www.ros.org/). 4 | 5 | ## Related Publications 6 | 7 | * [IROS20] K. Sun, B. Schlotfelt, S. Chaves, P. Martin, G. Mandhyan, and V. Kumar, _"Feedback Enhanced Motion Planning for Autonomous Vehicles"_, Robotics and Automation Letter, 2020. 8 | 9 | IMAGE ALT TEXT HERE 12 | 13 | ## License 14 | 15 | BSD 3-clause License 16 | 17 | ## Workspace Setup 18 | 19 | The setup of the workspace can be a bit tricky, mostly because Carla 0.9.6 uses Boost 1.69 but Ubuntu 18.04 has Boost 1.65 by default. Therefore, we have to complie almost all of the dependencies from source and link against the customized compiled libraries. All dependencies are listed as follows. The details of the setup can be found [here](scripts/workspace_setup.md). 20 | 21 | ### Dependencies 22 | * [Boost 1.69.0](https://www.boost.org/doc/libs/1_69_0/) 23 | * [PCL 1.9.1](https://github.com/PointCloudLibrary/pcl/releases/tag/pcl-1.9.1) 24 | * [ROS Melodic](http://wiki.ros.org/melodic) 25 | * [catkin tools](https://catkin-tools.readthedocs.io/en/latest/) 26 | * [Carla 0.9.6](https://carla.org/2019/07/12/release-0.9.6/) 27 | 28 | ## Software Framework and Usage 29 | 30 | This repository currently focuses on motion planning algorithms for self-driving vehicles. Therefore, we have only developed the simulation and planning software stack for now. Many sensing capabilities provided by Carla has not been interfaced. For such sensing capabilities, check [Carla-ROS bridge](https://github.com/carla-simulator/ros-bridge) for a python interface. More software details can be found [here](scripts/software_framework_usage.md) 31 | 32 | -------------------------------------------------------------------------------- /rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: Conformal Lattice Planner 3 | output_dir: ./doc 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.hpp' 5 | -------------------------------------------------------------------------------- /scripts/braking_scenario_data_analysis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import division 4 | from __future__ import print_function 5 | 6 | import os 7 | import os.path 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | import rosbag 13 | from conformal_lattice_planner.msg import EgoPlanGoal, EgoPlanResult 14 | from conformal_lattice_planner.msg import AgentPlanGoal, AgentPlanResult 15 | 16 | def prune_path_type(path_type): 17 | 18 | marker = 0 19 | value = path_type[0] 20 | min_stride_len = 20 21 | 22 | for i in range(path_type.size): 23 | if path_type[i] != value: 24 | if value != 0: 25 | stride = i - marker 26 | if stride < min_stride_len: path_type[marker:i] = 0 27 | marker = i 28 | value = path_type[i] 29 | 30 | if value != 0: 31 | stride = path_type.size - marker 32 | if stride < min_stride_len: path_type[marker:path_type.size] = 0 33 | 34 | return 35 | 36 | def lane_change_num(path_type): 37 | 38 | lane_changes = 0 39 | marker = 0 40 | value = path_type[0] 41 | min_stride_len = 20 42 | 43 | for i in range(path_type.size): 44 | if path_type[i] != value: 45 | if value != 0: 46 | stride = i - marker 47 | if stride >= min_stride_len: lane_changes = lane_changes + 1 48 | marker = i 49 | value = path_type[i] 50 | 51 | if value != 0: 52 | stride = path_type.size - marker 53 | if stride >= min_stride_len: lane_changes = lane_changes + 1 54 | 55 | return lane_changes 56 | 57 | def extract_ego_data(bagfile): 58 | 59 | data_type = np.dtype([ 60 | ( 't', 'float'), 61 | ( 'speed', 'float'), 62 | ( 'acceleration', 'float'), 63 | ( 'leading_distance', 'float'), 64 | ('following_distance', 'float'), 65 | ( 'policy_speed', 'float'), 66 | ( 'path_type', 'int' ), 67 | ( 'planning_time', 'float')]) 68 | 69 | bag = rosbag.Bag(bagfile, 'r') 70 | ego_data_counter = 0 71 | ego_data = np.zeros(bag.get_message_count('/carla/carla_simulator/ego_plan/goal'), dtype=data_type) 72 | 73 | # Extract all data from the bag. 74 | for topic, msg, t in bag.read_messages(topics=[ 75 | '/carla/carla_simulator/ego_plan/goal', 76 | '/carla/carla_simulator/ego_plan/result']): 77 | 78 | if topic=='/carla/carla_simulator/ego_plan/goal': 79 | ego_data[ego_data_counter][ 't'] = msg.goal.simulation_time 80 | ego_data[ego_data_counter][ 'speed'] = msg.goal.snapshot.ego.speed 81 | ego_data[ego_data_counter][ 'acceleration'] = msg.goal.snapshot.ego.acceleration 82 | ego_data[ego_data_counter][ 'leading_distance'] = msg.goal.leading_distance 83 | ego_data[ego_data_counter]['following_distance'] = msg.goal.following_distance 84 | else: 85 | ego_data[ego_data_counter][ 'path_type'] = msg.result.path_type 86 | ego_data[ego_data_counter][ 'planning_time'] = msg.result.planning_time 87 | ego_data_counter += 1 88 | 89 | bag.close() 90 | return ego_data 91 | 92 | def main(): 93 | 94 | data_dir = '/home/ke/Data/conformal_lattice_planner/braking_scenario/' 95 | idm_bagfile = data_dir + 'idm_lane_follower/' + 'traffic_data_2020-01-03-14-44-16.bag' 96 | const_accel_bagfile = data_dir + 'spatiotemporal_lane_follower/' + 'traffic_data_2020-01-03-14-42-29.bag' 97 | 98 | idm_data = extract_ego_data(idm_bagfile) 99 | const_accel_data = extract_ego_data(const_accel_bagfile) 100 | 101 | # Acceleration. 102 | accel_fig, accel_ax = plt.subplots() 103 | accel_ax.plot( idm_data['t'], idm_data['acceleration'], linewidth=2.0) 104 | accel_ax.plot(const_accel_data['t'], const_accel_data['acceleration'], linewidth=2.0) 105 | accel_ax.grid() 106 | accel_ax.set_xlabel('time (s)') 107 | accel_ax.set_ylabel('acceleration (m/s/s)') 108 | 109 | accel_fig.tight_layout() 110 | plt.savefig('/home/ke/Desktop/braking_scenario_acceleration.png', 111 | format='png', bbox_inches='tight') 112 | 113 | # Speed. 114 | speed_fig, speed_ax = plt.subplots() 115 | speed_ax.plot( idm_data['t'], idm_data['speed']) 116 | speed_ax.plot(const_accel_data['t'], const_accel_data['speed']) 117 | speed_ax.grid() 118 | speed_ax.set_xlabel('time (s)') 119 | speed_ax.set_ylabel('speed (m/s)') 120 | 121 | speed_fig.tight_layout() 122 | plt.savefig('/home/ke/Desktop/braking_scenario_speed.png', 123 | format='png', bbox_inches='tight') 124 | 125 | # Following distance. 126 | headway_fig, headway_ax = plt.subplots() 127 | headway_ax.plot( idm_data['t'], idm_data['leading_distance']/ idm_data['speed']) 128 | headway_ax.plot(const_accel_data['t'], const_accel_data['leading_distance']/const_accel_data['speed']) 129 | headway_ax.grid() 130 | headway_ax.set_xlabel('time (s)') 131 | headway_ax.set_ylabel('headway (s)') 132 | 133 | headway_fig.tight_layout() 134 | plt.savefig('/home/ke/Desktop/braking_scenario_headway.png', 135 | format='png', bbox_inches='tight') 136 | 137 | plt.show() 138 | 139 | if __name__ == '__main__': 140 | main() 141 | -------------------------------------------------------------------------------- /scripts/intelligent_driver_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | #from __future__ import print_function 4 | #from __future__ import division 5 | from math import sqrt 6 | from math import tanh 7 | 8 | time_gap = 1.0 9 | distance_gap = 6.0 10 | 11 | accel_exp = 4.0 12 | 13 | comfort_accel = 1.5 14 | comfort_decel = 2.5 15 | 16 | max_accel = 5.0 17 | max_decel = 8.0 18 | 19 | coolness_factor = 0.9 20 | 21 | def saturate_accel(accel): 22 | 23 | if accel > max_accel: 24 | accel = max_accel 25 | if accel < -max_decel: 26 | accel = -max_decel 27 | 28 | return accel 29 | 30 | def desired_distance(ego_v, lead_v): 31 | v_diff = ego_v - lead_v 32 | return distance_gap + max( 33 | 0.0, 34 | ego_v*time_gap + ego_v*v_diff / (2.0*sqrt(comfort_accel*comfort_decel))) 35 | 36 | def intelligent_driver_model(ego_v, ego_v0, lead_v=None, s=None): 37 | 38 | if (lead_v is None) or (s is None): 39 | accel_out = comfort_accel * (1.0 - (ego_v/ego_v0)**accel_exp) 40 | else: 41 | accel_out = comfort_accel * (1.0 - 42 | (ego_v/ego_v0)**accel_exp - 43 | (desired_distance(ego_v, lead_v)/s)**2.0) 44 | 45 | return saturate_accel(accel_out) 46 | 47 | def free_accel(ego_v, ego_v0): 48 | 49 | if ego_v <= ego_v0: 50 | return comfort_accel * ( 51 | 1.0-(ego_v/ego_v0)**accel_exp) 52 | else: 53 | return -comfort_decel * ( 54 | 1.0-(ego_v0/ego_v)**(comfort_accel*accel_exp/comfort_decel)) 55 | 56 | def improved_intelligent_driver_model(ego_v, ego_v0, lead_v=None, s=None): 57 | 58 | accel_free = free_accel(ego_v, ego_v0) 59 | 60 | if (lead_v is None) or (s is None): 61 | return accel_free 62 | 63 | z = desired_distance(ego_v, lead_v) / s 64 | 65 | if ego_v=1: 66 | accel_out = comfort_accel * (1.0-z**2) 67 | elif ego_v=ego_v0 and z>=1: 70 | accel_out = accel_free + comfort_accel*(1-z**2.0) 71 | else: 72 | accel_out = accel_free 73 | 74 | return saturate_accel(accel_out) 75 | 76 | def const_accel_heuristic(ego_v, lead_v, lead_v_dot, s): 77 | 78 | accel_tilde = min(comfort_accel, lead_v_dot) 79 | 80 | if lead_v*(ego_v-lead_v) < -2.0*s*accel_tilde: 81 | return (ego_v**2.0 * accel_tilde) / ( 82 | lead_v**2.0 - 2.0*s*accel_tilde) 83 | else: 84 | Theta = lambda x : 1 if x>=0 else 0 85 | return accel_tilde - (ego_v-lead_v)**2.0*Theta(ego_v-lead_v) / (2.0*s) 86 | 87 | def adaptive_cruise_control(ego_v, ego_v0, lead_v=None, s=None): 88 | 89 | lead_v_dot = 0.0 90 | accel_iidm = improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s) 91 | 92 | if (lead_v is None) or (s is None): 93 | return accel_iidm 94 | 95 | accel_cah = const_accel_heuristic(ego_v, lead_v, lead_v_dot, s) 96 | 97 | if accel_iidm >= accel_cah: 98 | accel_out = accel_iidm 99 | else: 100 | accel_out = (1-coolness_factor) * accel_iidm + coolness_factor * ( 101 | accel_cah + comfort_decel*tanh((accel_iidm-accel_cah)/comfort_decel)) 102 | return saturate_accel(accel_out) 103 | 104 | def main(): 105 | 106 | ego_v = 10 107 | ego_v0 = 29 108 | 109 | print("ego v: ", ego_v, "ego v0: ", ego_v0) 110 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0)) 111 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0)) 112 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0)) 113 | print("") 114 | 115 | ego_v = 40 116 | ego_v0 = 29 117 | 118 | print("ego v: ", ego_v, "ego v0: ", ego_v0) 119 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0)) 120 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0)) 121 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0)) 122 | print("") 123 | 124 | ego_v = 20 125 | ego_v0 = 29 126 | lead_v = 29 127 | s = 50 128 | print("ego v: ", ego_v, "ego v0: ", ego_v0, "lead v: ", lead_v, "s: ", s) 129 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 130 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 131 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0, lead_v, s)) 132 | print("") 133 | 134 | ego_v = 25 135 | ego_v0 = 29 136 | lead_v = 15 137 | s = 50 138 | print("ego v: ", ego_v, "ego v0: ", ego_v0, "lead v: ", lead_v, "s: ", s) 139 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 140 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 141 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0, lead_v, s)) 142 | print("") 143 | 144 | ego_v = 29 145 | ego_v0 = 29 146 | lead_v = 0 147 | s = 100 148 | print("ego v: ", ego_v, "ego v0: ", ego_v0, "lead v: ", lead_v, "s: ", s) 149 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 150 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 151 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0, lead_v, s)) 152 | print("") 153 | 154 | if __name__ == '__main__': 155 | main() 156 | 157 | 158 | -------------------------------------------------------------------------------- /scripts/merging_scenario_data_analysis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import division 4 | from __future__ import print_function 5 | 6 | import os 7 | import os.path 8 | import math 9 | from math import atan2, sin, cos, degrees 10 | 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | import matplotlib.patches as patches 14 | import mpl_toolkits.mplot3d.art3d as art3d 15 | 16 | import rosbag 17 | from tf.transformations import euler_from_quaternion 18 | from conformal_lattice_planner.msg import EgoPlanGoal, EgoPlanResult 19 | from conformal_lattice_planner.msg import AgentPlanGoal, AgentPlanResult 20 | 21 | def extract_vehicle_data(bagfile, vehicle_id): 22 | 23 | data_type = np.dtype([ 24 | ( 't', 'float'), 25 | ( 'x', 'float'), 26 | ( 'y', 'float'), 27 | ( 'yaw', 'float'), 28 | ('length', 'float'), 29 | ( 'width', 'float')]) 30 | 31 | bag = rosbag.Bag(bagfile, 'r') 32 | data_counter = 0 33 | vehicle_data = np.zeros(bag.get_message_count('/carla/carla_simulator/ego_plan/goal'), dtype=data_type) 34 | 35 | # Extract all data from the bag. 36 | for topic, msg, t in bag.read_messages(topics=[ 37 | '/carla/carla_simulator/ego_plan/goal']): 38 | 39 | # Get the vehicle with the same ID. 40 | if vehicle_id == msg.goal.snapshot.ego.id: 41 | vehicle = msg.goal.snapshot.ego 42 | else: 43 | for agent in msg.goal.snapshot.agents: 44 | if agent.id != vehicle_id: continue 45 | vehicle = agent 46 | 47 | q = vehicle.transform.orientation 48 | vehicle_data[data_counter][ 't'] = msg.goal.simulation_time 49 | vehicle_data[data_counter][ 'x'] = vehicle.transform.position.x 50 | vehicle_data[data_counter][ 'y'] = vehicle.transform.position.y 51 | vehicle_data[data_counter][ 'yaw'] = atan2(2.0*(q.w*q.z+q.x*q.y), 1.0-2.0*(q.y*q.y+q.z*q.z)) 52 | vehicle_data[data_counter]['length'] = vehicle.bounding_box.extent.x 53 | vehicle_data[data_counter][ 'width'] = vehicle.bounding_box.extent.y 54 | 55 | data_counter = data_counter + 1 56 | 57 | bag.close() 58 | return vehicle_data 59 | 60 | def vehicle_path_geom(vehicle): 61 | 62 | x = -vehicle['y'] 63 | y = vehicle['x'] 64 | return x, y 65 | 66 | def vehicle_patch_geom(vehicle): 67 | 68 | p0 = np.array([-vehicle['y'], vehicle['x']]) 69 | dp = np.array([-vehicle['length'], -vehicle['width']]) 70 | 71 | theta = vehicle['yaw'] + math.pi/2.0 72 | R = np.array([[cos(theta), -sin(theta)], 73 | [sin(theta), cos(theta)]]) 74 | 75 | p = R.dot(dp) + p0 76 | 77 | return p, degrees(theta) 78 | 79 | def main(): 80 | 81 | data_dir = '/home/ke/Data/conformal_lattice_planner/merging_scenario/' 82 | bagfile = data_dir + 'slc_lattice_planner/' + 'traffic_data_2020-01-03-14-48-17.bag' 83 | 84 | ego_data = extract_vehicle_data(bagfile, 213) 85 | agent214_data = extract_vehicle_data(bagfile, 214) 86 | agent215_data = extract_vehicle_data(bagfile, 215) 87 | agent216_data = extract_vehicle_data(bagfile, 216) 88 | 89 | time_instances = [0, 30, 40, 65] 90 | figs = [] 91 | 92 | #x_min = np.amin([-ego_data['y'][ 0], -agent214_data['y'][ 0], -agent215_data['y'][ 0], -agent216_data['y'][ 0]]) 93 | #x_max = np.amax([-ego_data['y'][-1], -agent214_data['y'][-1], -agent215_data['y'][-1], -agent216_data['y'][-1]]) 94 | x_min = 65 95 | x_max = 180 96 | 97 | for i in time_instances: 98 | 99 | fig, ax = plt.subplots() 100 | ax.plot(np.linspace(x_min, x_max, 100), 4.596*np.ones(100), 'k--', alpha=0.5) 101 | ax.plot(np.linspace(x_min, x_max, 100), 7.631*np.ones(100), 'k--', alpha=0.5) 102 | 103 | # Draw the agent 214. 104 | agent214_p, agent214_theta = vehicle_patch_geom(agent214_data[i]) 105 | agent214_rect = patches.Rectangle( 106 | agent214_p, agent214_data['length'][i]*2, agent214_data['width'][i]*2, agent214_theta, 107 | alpha=1.0, fc='C7', ec='black', zorder=100) 108 | ax.add_patch(agent214_rect) 109 | 110 | agent214_x, agent214_y = vehicle_path_geom(agent214_data[0:i+1]) 111 | for j in range(0, i-1): 112 | ax.plot(agent214_x[j:j+2], agent214_y[j:j+2], color='C7', alpha=0.9*j/i+0.1, linewidth=2) 113 | 114 | # Draw the agent 215. 115 | agent215_p, agent215_theta = vehicle_patch_geom(agent215_data[i]) 116 | agent215_rect = patches.Rectangle( 117 | agent215_p, agent215_data['length'][i]*2, agent215_data['width'][i]*2, agent215_theta, 118 | alpha=1.0, fc='C7', ec='black', zorder=100) 119 | ax.add_patch(agent215_rect) 120 | 121 | agent215_x, agent215_y = vehicle_path_geom(agent215_data[0:i+1]) 122 | for j in range(0, i-1): 123 | ax.plot(agent215_x[j:j+2], agent215_y[j:j+2], color='C7', alpha=0.9*j/i+0.1, linewidth=2) 124 | 125 | # Draw the agent 216. 126 | agent216_p, agent216_theta = vehicle_patch_geom(agent216_data[i]) 127 | agent216_rect = patches.Rectangle( 128 | agent216_p, agent216_data['length'][i]*2, agent216_data['width'][i]*2, agent216_theta, 129 | alpha=1.0, fc='C7', ec='black', zorder=100) 130 | ax.add_patch(agent216_rect) 131 | 132 | agent216_x, agent216_y = vehicle_path_geom(agent216_data[0:i+1]) 133 | for j in range(0, i-1): 134 | ax.plot(agent216_x[j:j+2], agent216_y[j:j+2], color='C7', alpha=0.9*j/i+0.1, linewidth=2) 135 | 136 | # Draw the ego vehicle. 137 | ego_p, ego_theta = vehicle_patch_geom(ego_data[i]) 138 | ego_rect = patches.Rectangle( 139 | ego_p, ego_data['length'][i]*2, ego_data['width'][i]*2, ego_theta, 140 | alpha=1.0, fc='C0', ec='black', zorder=100) 141 | ax.add_patch(ego_rect) 142 | 143 | ego_x, ego_y = vehicle_path_geom(ego_data[0:i+1]) 144 | for j in range(0, i-1): 145 | ax.plot(ego_x[j:j+2], ego_y[j:j+2], color='C0', alpha=0.9*j/i+0.1, linewidth=2) 146 | 147 | ax.set_ylim(2, 10) 148 | ax.set_xlim(65, 180) 149 | ax.set_aspect('equal') 150 | figs.append(fig) 151 | 152 | fig.tight_layout() 153 | plt.savefig('/home/ke/Desktop/merging_scenario_snapshot_'+str(i)+'.png', 154 | format='png', bbox_inches='tight') 155 | 156 | plt.show() 157 | 158 | if __name__ == '__main__': 159 | main() 160 | -------------------------------------------------------------------------------- /scripts/path_generation.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include "path_optimization.h" 9 | 10 | using namespace planner; 11 | using namespace std; 12 | 13 | int main(int argc, char** argv) { 14 | 15 | NonHolonomicPath::State lc_start(0.0, 0.0, 0.0, 0.0); 16 | NonHolonomicPath::State lc_end(50.0, 3.7, 0.0, 0.0); 17 | 18 | NonHolonomicPath path; 19 | const bool success = path.optimizePath(lc_start, lc_end); 20 | 21 | 22 | 23 | if (!success) { 24 | boost::format state_format("%1% %2% %3% %4%\n"); 25 | std::string error_msg("Path optimization failed.\n"); 26 | error_msg += "lc start state (x y theta kappa): " + ( 27 | state_format % lc_start.x 28 | % lc_start.y 29 | % lc_start.theta 30 | % lc_start.kappa).str(); 31 | error_msg += "lc end state (x y theta kappa): " + ( 32 | state_format % lc_end.x 33 | % lc_end.y 34 | % lc_end.theta 35 | % lc_end.kappa).str(); 36 | throw std::runtime_error(error_msg); 37 | return -1; 38 | } 39 | 40 | boost::format waypoint_format("%1% %2% %3% %4% %5%\n"); 41 | std::string path_msg; 42 | for (double s = 0.0; s <= path.sf; s+=0.05) { 43 | NonHolonomicPath::State state = path.evaluate(lc_start, s); 44 | path_msg += (waypoint_format % s 45 | % state.x 46 | % state.y 47 | % state.theta 48 | % state.kappa).str(); 49 | } 50 | 51 | ofstream fout; 52 | fout.open("left_lane_change_path"); 53 | fout << path_msg; 54 | fout.close(); 55 | 56 | return 0; 57 | } 58 | -------------------------------------------------------------------------------- /scripts/path_test.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include "path_optimization.h" 10 | 11 | using namespace planner; 12 | using namespace std; 13 | 14 | int main(int argc, char** argv) { 15 | 16 | NonHolonomicPath::State lc_start(14.254, 125.566, -96.9345/(2*M_PI), 0.0461623); 17 | NonHolonomicPath::State lc_end(12.2967, 75.585, -90.2891/(2*M_PI), 0.0); 18 | 19 | NonHolonomicPath path; 20 | const bool success = path.optimizePath(lc_start, lc_end); 21 | 22 | if (!success) { 23 | boost::format state_format("%1% %2% %3% %4%\n"); 24 | std::string error_msg("Path optimization failed.\n"); 25 | error_msg += "lc start state (x y theta kappa): " + ( 26 | state_format % lc_start.x 27 | % lc_start.y 28 | % lc_start.theta 29 | % lc_start.kappa).str(); 30 | error_msg += "lc end state (x y theta kappa): " + ( 31 | state_format % lc_end.x 32 | % lc_end.y 33 | % lc_end.theta 34 | % lc_end.kappa).str(); 35 | throw std::runtime_error(error_msg); 36 | return -1; 37 | } 38 | 39 | return 0; 40 | } 41 | 42 | -------------------------------------------------------------------------------- /scripts/random_traffic_data_analysis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import division 4 | from __future__ import print_function 5 | 6 | import os 7 | import os.path 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | import rosbag 13 | from conformal_lattice_planner.msg import EgoPlanGoal, EgoPlanResult 14 | from conformal_lattice_planner.msg import AgentPlanGoal, AgentPlanResult 15 | 16 | #def prune_path_type(path_type): 17 | # 18 | # marker = 0 19 | # value = path_type[0] 20 | # min_stride_len = 20 21 | # 22 | # for i in range(path_type.size): 23 | # if path_type[i] != value: 24 | # if value != 0: 25 | # stride = i - marker 26 | # if stride < min_stride_len: path_type[marker:i] = 0 27 | # marker = i 28 | # value = path_type[i] 29 | # 30 | # if value != 0: 31 | # stride = path_type.size - marker 32 | # if stride < min_stride_len: path_type[marker:path_type.size] = 0 33 | # 34 | # return 35 | 36 | def lane_change_num(path_type): 37 | 38 | lane_changes = 0 39 | marker = 0 40 | value = path_type[0] 41 | min_stride_len = 20 42 | 43 | for i in range(path_type.size): 44 | if path_type[i] != value: 45 | if value != 0: 46 | stride = i - marker 47 | if stride >= min_stride_len: lane_changes = lane_changes + 1 48 | marker = i 49 | value = path_type[i] 50 | 51 | if value != 0: 52 | stride = path_type.size - marker 53 | if stride >= min_stride_len: lane_changes = lane_changes + 1 54 | 55 | return lane_changes 56 | 57 | def extract_ego_data(bagfile): 58 | 59 | data_type = np.dtype([ 60 | ( 't', 'float'), 61 | ( 'speed', 'float'), 62 | ( 'acceleration', 'float'), 63 | ( 'leading_distance', 'float'), 64 | ('following_distance', 'float'), 65 | ( 'policy_speed', 'float'), 66 | ( 'path_type', 'int' ), 67 | ( 'planning_time', 'float')]) 68 | 69 | bag = rosbag.Bag(bagfile, 'r') 70 | ego_data_counter = 0 71 | ego_data = np.zeros(bag.get_message_count('/carla/carla_simulator/ego_plan/goal'), dtype=data_type) 72 | 73 | # Extract all data from the bag. 74 | for topic, msg, t in bag.read_messages(topics=[ 75 | '/carla/carla_simulator/ego_plan/goal', 76 | '/carla/carla_simulator/ego_plan/result']): 77 | 78 | if topic=='/carla/carla_simulator/ego_plan/goal': 79 | ego_data[ego_data_counter][ 't'] = msg.goal.simulation_time 80 | ego_data[ego_data_counter][ 'speed'] = msg.goal.snapshot.ego.speed 81 | ego_data[ego_data_counter][ 'acceleration'] = msg.goal.snapshot.ego.acceleration 82 | ego_data[ego_data_counter][ 'leading_distance'] = msg.goal.leading_distance 83 | ego_data[ego_data_counter]['following_distance'] = msg.goal.following_distance 84 | else: 85 | ego_data[ego_data_counter][ 'path_type'] = msg.result.path_type 86 | ego_data[ego_data_counter][ 'planning_time'] = msg.result.planning_time 87 | ego_data_counter += 1 88 | 89 | bag.close() 90 | 91 | # Compute the variance of the acceleration data. 92 | accel_var = np.zeros(ego_data['acceleration'].size) 93 | for i in range(ego_data['acceleration'].size): 94 | l = i-49 if i-49>=0 else 0 95 | r = i+50 if i+50<=accel_var.size else accel_var.size 96 | accel_var[i] = np.var(ego_data['acceleration'][l:r]) 97 | 98 | # Compute the average headway. 99 | valid_idx = np.nonzero(ego_data['leading_distance'] > 0.0) 100 | headway = ego_data['leading_distance'][valid_idx] / ego_data['speed'][valid_idx] 101 | 102 | # Return the tuple of 103 | # duration of the bag, speed, acceleration, leading headway, lane changes, planning time (all in average). 104 | return { 'simulation time': ego_data['t'][-1], 105 | 'average speed': np.mean(ego_data['speed']), 106 | 'average acceleration': np.mean(ego_data['acceleration']), 107 | 'average acceleration variance': np.mean(accel_var), 108 | 'average headway': np.mean(headway), 109 | 'lane changes': lane_change_num(ego_data['path_type']), 110 | 'average planning time': np.mean(ego_data['planning_time']), 111 | 'count': ego_data.size} 112 | 113 | def main(): 114 | 115 | data_dir = '/home/ke/Data/conformal_lattice_planner/random_traffic/' 116 | method = 'spatiotemporal_lattice_planner/' 117 | bagfiles = [data_dir+method+f for f in os.listdir(data_dir+method) 118 | if os.path.isfile(os.path.join(data_dir+method, f))] 119 | 120 | average_data = { 'simulation time': 0.0, 121 | 'average speed': 0.0, 122 | 'average acceleration': 0.0, 123 | 'average acceleration variance': 0.0, 124 | 'average headway': 0.0, 125 | 'lane changes': 0.0, 126 | 'average planning time': 0.0, 127 | 'count': 0} 128 | 129 | for bag in bagfiles: 130 | 131 | print(bag) 132 | ego_data = extract_ego_data(bag) 133 | for key, value in ego_data.items(): 134 | print(key, ': ', value) 135 | 136 | average_data[ 'simulation time'] += ego_data[ 'simulation time'] 137 | average_data[ 'average speed'] += ego_data[ 'average speed'] * ego_data['count'] 138 | average_data[ 'average acceleration'] += ego_data[ 'average acceleration'] * ego_data['count'] 139 | average_data['average acceleration variance'] += ego_data['average acceleration variance'] * ego_data['count'] 140 | average_data[ 'average headway'] += ego_data[ 'average headway'] * ego_data['count'] 141 | average_data[ 'lane changes'] += ego_data[ 'lane changes'] 142 | average_data[ 'average planning time'] += ego_data[ 'average planning time'] * ego_data['count'] 143 | average_data[ 'count'] += ego_data[ 'count'] 144 | 145 | average_data[ 'average speed'] /= average_data['count'] 146 | average_data[ 'average acceleration'] /= average_data['count'] 147 | average_data['average acceleration variance'] /= average_data['count'] 148 | average_data[ 'average headway'] /= average_data['count'] 149 | average_data[ 'average planning time'] /= average_data['count'] 150 | 151 | print() 152 | for key, value in average_data.items(): 153 | print(key, ': ', value) 154 | 155 | #fig, ax = plt.subplots() 156 | #ax.plot(ego_data['t'], ego_data['acceleration']) 157 | #ax.grid() 158 | #ax.set_xlabel('time (s)') 159 | ##ax.set_ylabel('speed (m/s)') 160 | #plt.show() 161 | 162 | if __name__ == '__main__': 163 | main() 164 | -------------------------------------------------------------------------------- /scripts/road_map.patch: -------------------------------------------------------------------------------- 1 | diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h 2 | index 360b8591..c4bd3812 100644 3 | --- a/LibCarla/source/carla/road/Map.h 4 | +++ b/LibCarla/source/carla/road/Map.h 5 | @@ -105,11 +105,15 @@ namespace road { 6 | /// map. The waypoints are placed at the entrance of each lane. 7 | std::vector> GenerateTopology() const; 8 | 9 | -#ifdef LIBCARLA_WITH_GTEST 10 | +//#ifdef LIBCARLA_WITH_GTEST 11 | + const MapData &GetMap() const { 12 | + return _data; 13 | + } 14 | + 15 | MapData &GetMap() { 16 | return _data; 17 | } 18 | -#endif // LIBCARLA_WITH_GTEST 19 | +//#endif // LIBCARLA_WITH_GTEST 20 | 21 | private: 22 | 23 | -------------------------------------------------------------------------------- /scripts/software_framework.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KumarRobotics/conformal_lattice_planner/56e878a5ccb9daca2bb77bd47cbbfb097b412e2d/scripts/software_framework.png -------------------------------------------------------------------------------- /scripts/software_framework_usage.md: -------------------------------------------------------------------------------- 1 | # Software Framework 2 | 3 | The following figure presents an overview of the node structure of this repository. 4 | 5 | ![node structure](/scripts/software_framework.png) 6 | 7 | ## Simulator Node 8 | 9 | Traffic simulation nodes are the main interface between ROS and Carla server, responsible for configuring the Carla server, spawning and deleting actors, retrieving actor information, etc. Three different types of traffic simulations are implemented (see `src/node/simulator`): 10 | 11 | * **No Traffic**: only the ego vehicle is simulated with no agent vehicles. 12 | * **Fixed Scenario**: agent vehicles can be preset around the ego, which aims at producing reproducable results from motion planning algorithms. 13 | * **Random Traffic**: A fixed number of agent vehicles are maintained around the ego vehicle. Based on the traffic state, new agent vehicles may be spawned or existing agent vehicles may be removed from the simulation. 14 | 15 | ## Agent Vehicle Planning Node 16 | 17 | There is currently only one motion planning algorithm implemented for agent vehicles, which control all agent vehicles to follow their lanes. The speed of the vehicles are modulated through the intelligent driver model. See `src/node/planner/agents_lane_following_node.h` for more details. 18 | 19 | ## Ego Vehicle Planning Node 20 | 21 | Several lattice motion planning algorithms are implemented for the ego vehicle. 22 | 23 | * Lane following: control the ego vehicle to follow the current lane. 24 | * IDM lattice planning: see [Ke RAL 2020] for more details. 25 | * Single-lane-change lattice planning: this is mostly a variation of IDM lattice planning. Over the spatial planning horizon, only a single lane change is allowed for the ego vehicle. 26 | * Spatiotemporal lattice planning: a primitive implementation of the work from [[McNaughton ICRA 2011]](https://ieeexplore.ieee.org/abstract/document/5980223?casa_token=-Y27ZPo4PIUAAAAA:UQVVS0j-gVQgGdA1QU-On6icfexBZOGnOjzXSo6IJnxMp0bg7rdTXmCkPYU-C6-Y3riD9_mZ) for more details. 27 | 28 | # Usage 29 | 30 | All launch files can be found in the `launch` directory. To launch the simulation, start with, 31 | ``` 32 | ./carla_server.sh 33 | ``` 34 | This script starts and configs the Carla simulator server. Note only the map Town04 is supported, otherwise the user has to re-define the route to be followed by the vehicle. For now, we hard-coded the road sequence to be followed by the vehicles in Town04. See `src/router/loop_router` for more details. 35 | 36 | Use different launch files to start different types of simulation nodes and planning nodes for agent and ego vehicles. For simplicity, `autonomous_driving.launch` can be used to launch all three nodes within one terminal. For example, 37 | ``` 38 | roslaunch autonomous_driving.launch no_traffic:=true ego_lane_follower:=true 39 | ``` 40 | will launch the trivial simulation with no traffic and a lane-following ego vehicle. See `launch/autonomous_driving.launch` for more details. `rviz/config.rviz` is prepared for visualization. 41 | 42 | -------------------------------------------------------------------------------- /scripts/weight_functions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from math import exp 4 | 5 | import numpy as np 6 | import matplotlib.pyplot as plt 7 | 8 | def sigmoid_func(x): 9 | #return 1.0 / (1.0 + np.exp(-15.0*(x-0.5))) 10 | return 1.0-np.exp(-5.0*x) 11 | 12 | x = np.arange(0.0, 1.0, 0.01) 13 | 14 | v13w = 1.0 - sigmoid_func(x) 15 | v3w = v13w * (x*(1.0-x)) 16 | v1w = v13w - v3w 17 | v2w = sigmoid_func(x) 18 | 19 | fig, ax = plt.subplots() 20 | ax.plot(x, v1w, label='v1') 21 | ax.plot(x, v2w, label='v2') 22 | ax.plot(x, v3w, label='v3') 23 | ax.grid() 24 | ax.legend() 25 | plt.show() 26 | -------------------------------------------------------------------------------- /scripts/workspace_setup.md: -------------------------------------------------------------------------------- 1 | # Workspace Setup 2 | 3 | Before starting the setup process, it may not be a bad idea to get yourself a beverage :coffee: since the setup will take quite some time :grin:. 4 | 5 | ## Carla Workspace 6 | 7 | Although it is not compulsory, we would recommend to have all dependencies collected under one ROS workspace. In the following of this documentation, we would assume this workspace is `~/carla_ws`. 8 | 9 | ``` 10 | cd 11 | mkdir -p carla_ws/src 12 | catkin init --workspace carla_ws 13 | ``` 14 | 15 | Make sure that the `carla_ws` does not extending any other ROS workspaces (not even the default ROS workspace `/opt/ros/melodic/`). 16 | 17 | ## Boost 1.69.0 18 | 19 | Download Boost 1.69.0 and install it to `carla_ws`. The details of compiling and installing Boost libraries can be found [here](https://www.boost.org/doc/libs/1_69_0/more/getting_started/unix-variants.html). For simplicity, they are summarized as follows. 20 | 21 | In the unzipped Boost folder, do the following, 22 | ``` 23 | ./bootstrap.sh --prefix=/home/$USER/carla_ws/boost_1_69_0 24 | ./b2 install 25 | ``` 26 | After these steps, there should be two directories in `/home/$USER/carla_ws/boost_1_69_0`, `include` and `lib`. Make sure that `libboost_python27.*` is in the `lib` directory, since we will be using python2.7 with ROS melodic (compile ROS with python3 is beyond the scope of this documentation). In case `libboost_python27.*` is missing, see [this](https://www.boost.org/doc/libs/1_69_0/libs/python/doc/html/building.html) for building Boost.Python. Especially, check [this](https://www.boost.org/doc/libs/1_69_0/libs/python/doc/html/building/configuring_boost_build.html) for setting up the correct python version. 27 | 28 | ## PCL 1.9.1 29 | 30 | The details of compiling PCL from source can be found [here](https://pcl-tutorials.readthedocs.io/en/latest/building_pcl.html#). The following commands are related to using PCL in this project. In the unzipped PCL directory, do the following. 31 | 32 | ``` 33 | mkdir build && cd build 34 | cmake -DBOOST_ROOT=/home/$USER/carla_ws/boost_1_69_0 \ 35 | -DCMAKE_INSTALL_PREFIX=/home/$USER/carla_ws/pcl_1_9_1 \ 36 | -DCMAKE_BUILD_TYPE=Release .. 37 | make install 38 | ``` 39 | 40 | After these steps, there should be `bin`, `include`, `lib`, and `share` subdirectories in `/home/$USER/carla_ws/pcl_1_9_1`. 41 | 42 | ## ROS Melodic 43 | 44 | Follow the instructions [here](http://wiki.ros.org/melodic/Installation/Source) to compile ROS melodic from source. Related instructions are listed as follows. 45 | 46 | ``` 47 | cd ~/carla_ws 48 | rosinstall_generator desktop_full --rosdistro melodic --deps --tar > melodic-desktop-full.rosinstall 49 | wstool init -j8 src melodic-desktop-full.rosinstall 50 | catkin config -DCMAKE_BUILD_TYPE=Release \ 51 | -DBOOST_ROOT=/home/${USER}/carla_ws/boost_1_69_0 \ 52 | -DBoost_NO_SYSTEM_PATHS=ON \ 53 | -DBOOST_INCLUDEDIR=/home/${USER}/carla_ws/boost_1_69_0/include \ 54 | -DBOOST_LIBRARYDIR=/home/${USER}/carla_ws/boost_1_69_0/lib \ 55 | -DPCL_DIR=/home/${USER}/carla_ws/pcl_1_9_1/share/pcl-1.9 56 | ``` 57 | 58 | Of course, ROS will not be compiled successfully at the first pass, since Boost 1.69.0 has renamed and removed some libraries compared to Boost 1.65.0. Some touches of `CMakeLists.txt` in a few packages are required. The required modifications (maybe not all) are summarized below. 59 | 60 | * `tf2`: remove `signals` in Boost components. 61 | * `cv_bridge`: replace `python` with `python27`. 62 | * `camera_calibration_parsers`: replace `python` with `python27`. 63 | * `gazebo_ros_packages`: add a `CATKIN_IGNORE` in this directory since gazebo won't be used. 64 | 65 | ## Carla 0.9.6 66 | 67 | **Carla is still under heavy development. Although we will try to, we may not always use the latest version of Carla.** 68 | 69 | Follow the steps [here](https://carla.readthedocs.io/en/0.9.6/how_to_build_on_linux/) to build Carla from source. A few things to note, 70 | 71 | * Clone the Carla reponsitory into `~/carla_ws`. 72 | * Use the 0.9.6 tag instead of the master branch. 73 | * Once the head of the repository is switched to 0.9.6, apply the [patch](scripts/road_map.patch) to unlock a hidden map interface, which the simulator uses to find more road information. The patch can be applied with `git apply road_map.patch` at the root of carla repository. 74 | * Test Carla as given [here](https://carla.readthedocs.io/en/0.9.6/getting_started/) to make sure it has been correctly built. 75 | 76 | ## Conformal Lattice Planner Workspace 77 | 78 | Before setting up the conformal lattice planner workspace, we have to add a few lines into `.bashrc` to make sure the previously built libraries can be found and linked against. 79 | 80 | ``` 81 | if [ -d /home/$USER/carla_ws ]; then 82 | # You may already have this line while compling Carla. 83 | export UE4_ROOT=/home/ke/Downloads/UnrealEngine 84 | 85 | # Set up the Carla directories. 86 | export Carla_ROOT=/home/$USER/carla_ws/carla 87 | export Carla_DIST=${Carla_ROOT}/Dist/CARLA_Shipping_0.9.6-16-gd1d9174a-dirty/LinuxNoEditor 88 | export Carla_INCLUDE_DIRS=${Carla_ROOT}/Examples/CppClient/libcarla-install/include 89 | export Carla_LIB_DIRS=${Carla_ROOT}/Examples/CppClient/libcarla-install/lib 90 | 91 | # Python and Cpp libraries. 92 | export PYTHONPATH="$PYTHONPATH:${Carla_DIST}/PythonAPI/carla/dist/carla-0.9.6-py2.7-linux-x86_64.egg" 93 | export LD_LIBRARY_PATH="${LD_LIBRARY_PATH}:${Carla_LIB_DIRS}:/home/$USER/carla_ws/boost_1_69_0/lib:/home/$USER/carla_ws/pcl_1_9_1/lib" 94 | fi 95 | ``` 96 | 97 | Finally we are able to set up the conformal lattice planner workspace. Instead of cloning this code repository into `carla_ws`, we will create another workspace, `self_driving_ws`, extending `carla_ws`. Hence, `carla_ws` will contain dependency files only. 98 | 99 | ``` 100 | mkdir -p ~/self_driving_ws/src 101 | catkin init --workspace self_driving_ws 102 | cd self_driving_ws 103 | catkin config --extend /home/$USER/carla_ws/devel 104 | catkin config -DBOOST_ROOT=/home/$USER/carla_ws/boost_1_69_0 \ 105 | -DBoost_NO_SYSTEM_PATHS=ON \ 106 | -DBOOST_INCLUDEDIR=/home/$USER/carla_ws/boost_1_69_0/include \ 107 | -DBOOST_LIBRARYDIR=/home/$USER/carla_ws/boost_1_69_0/lib \ 108 | -DCMAKE_BUILD_TYPE=Release \ 109 | -DPCL_DIR=/home/$USER/carla_ws/pcl_1_9_1/share/pcl-1.9 110 | cd src 111 | git clone link_to_this_repository 112 | catkin build 113 | ``` 114 | 115 | 116 | 117 | 118 | -------------------------------------------------------------------------------- /src/node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(planner) 2 | add_subdirectory(simulator) 3 | -------------------------------------------------------------------------------- /src/node/common/convert_to_visualization_msgs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace node { 42 | 43 | sensor_msgs::ImagePtr createImageMsg( 44 | const boost::shared_ptr&); 45 | 46 | visualization_msgs::MarkerPtr createWaypointMsg( 47 | const std::vector>&); 48 | 49 | visualization_msgs::MarkerPtr createJunctionMsg( 50 | const carla::client::Map::TopologyList&); 51 | 52 | visualization_msgs::MarkerArrayPtr createRoadIdsMsg( 53 | const std::unordered_map&); 54 | 55 | visualization_msgs::MarkerArrayPtr createVehiclesMsg( 56 | const std::vector>&); 57 | 58 | visualization_msgs::MarkerArrayPtr createVehicleIdsMsg( 59 | const std::vector>&); 60 | 61 | geometry_msgs::TransformStampedPtr createVehicleTransformMsg( 62 | const boost::shared_ptr&, const std::string&); 63 | 64 | visualization_msgs::MarkerArrayPtr createWaypointLatticeMsg( 65 | const boost::shared_ptr&); 66 | 67 | visualization_msgs::MarkerArrayPtr createTrafficLatticeMsg( 68 | const boost::shared_ptr&); 69 | 70 | visualization_msgs::MarkerArrayPtr createTrafficManagerMsg( 71 | const boost::shared_ptr&); 72 | 73 | template 74 | void populatePathMsg(const Path& path, const visualization_msgs::MarkerPtr& path_msg) { 75 | 76 | std::vector> 77 | transforms = path.samples(); 78 | 79 | for (auto& transform : transforms) { 80 | utils::convertTransformInPlace(transform.first); 81 | geometry_msgs::Point pt; 82 | pt.x = transform.first.location.x; 83 | pt.y = transform.first.location.y; 84 | pt.z = transform.first.location.z; 85 | 86 | path_msg->points.push_back(pt); 87 | path_msg->colors.push_back(path_msg->color); 88 | } 89 | 90 | return; 91 | } 92 | 93 | template 94 | visualization_msgs::MarkerPtr createEgoPathMsg(const Path& path) { 95 | 96 | visualization_msgs::MarkerPtr path_msg(new visualization_msgs::Marker); 97 | 98 | std_msgs::ColorRGBA path_color; 99 | path_color.r = 0.4; 100 | path_color.g = 1.0; 101 | path_color.b = 0.4; 102 | path_color.a = 1.0; 103 | 104 | path_msg->header.stamp = ros::Time::now(); 105 | path_msg->header.frame_id = "map"; 106 | path_msg->ns = "ego_path"; 107 | path_msg->id = 0; 108 | path_msg->type = visualization_msgs::Marker::LINE_STRIP; 109 | path_msg->action = visualization_msgs::Marker::ADD; 110 | path_msg->lifetime = ros::Duration(0.0); 111 | path_msg->frame_locked = false; 112 | path_msg->pose.orientation.w = 1.0; 113 | path_msg->scale.x = 1.8; 114 | path_msg->scale.y = 1.8; 115 | path_msg->scale.z = 1.8; 116 | path_msg->color = path_color; 117 | 118 | populatePathMsg(path, path_msg); 119 | return path_msg; 120 | } 121 | 122 | visualization_msgs::MarkerArrayPtr createConformalLatticeMsg( 123 | const std::vector>& nodes, 124 | const std::vector& edges); 125 | 126 | } // End namespace node. 127 | -------------------------------------------------------------------------------- /src/node/planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Agents lane following node. 2 | add_executable(agents_lane_following_node 3 | agents_lane_following_node.cpp 4 | planning_node.cpp 5 | ../common/convert_to_visualization_msgs.cpp 6 | ) 7 | target_link_libraries(agents_lane_following_node 8 | routing_algos 9 | planning_algos 10 | ${Carla_LIBRARIES} 11 | ${Boost_LIBRARIES} 12 | ${catkin_LIBRARIES} 13 | ${PCL_LIBRARIES} 14 | ) 15 | add_dependencies(agents_lane_following_node 16 | routing_algos 17 | planning_algos 18 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 19 | ${catkin_EXPORTED_TARGETS} 20 | ) 21 | 22 | # Ego lane following node. 23 | add_executable(ego_lane_following_node 24 | ego_lane_following_node.cpp 25 | planning_node.cpp 26 | ../common/convert_to_visualization_msgs.cpp 27 | ) 28 | target_link_libraries(ego_lane_following_node 29 | routing_algos 30 | planning_algos 31 | ${Carla_LIBRARIES} 32 | ${Boost_LIBRARIES} 33 | ${catkin_LIBRARIES} 34 | ${PCL_LIBRARIES} 35 | ) 36 | add_dependencies(ego_lane_following_node 37 | routing_algos 38 | planning_algos 39 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 40 | ${catkin_EXPORTED_TARGETS} 41 | ) 42 | 43 | # Ego IDM lattice planning node 44 | add_executable(ego_idm_lattice_planning_node 45 | ego_idm_lattice_planning_node.cpp 46 | planning_node.cpp 47 | ../common/convert_to_visualization_msgs.cpp 48 | ) 49 | target_link_libraries(ego_idm_lattice_planning_node 50 | routing_algos 51 | planning_algos 52 | ${Carla_LIBRARIES} 53 | ${Boost_LIBRARIES} 54 | ${catkin_LIBRARIES} 55 | ${PCL_LIBRARIES} 56 | ) 57 | add_dependencies(ego_idm_lattice_planning_node 58 | routing_algos 59 | planning_algos 60 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 61 | ${catkin_EXPORTED_TARGETS} 62 | ) 63 | 64 | # Ego Spatiotemporal lattice planning node 65 | add_executable(ego_spatiotemporal_lattice_planning_node 66 | ego_spatiotemporal_lattice_planning_node.cpp 67 | planning_node.cpp 68 | ../common/convert_to_visualization_msgs.cpp 69 | ) 70 | target_link_libraries(ego_spatiotemporal_lattice_planning_node 71 | routing_algos 72 | planning_algos 73 | ${Carla_LIBRARIES} 74 | ${Boost_LIBRARIES} 75 | ${catkin_LIBRARIES} 76 | ${PCL_LIBRARIES} 77 | ) 78 | add_dependencies(ego_spatiotemporal_lattice_planning_node 79 | routing_algos 80 | planning_algos 81 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 82 | ${catkin_EXPORTED_TARGETS} 83 | ) 84 | 85 | # Ego SLC lattice planning node 86 | add_executable(ego_slc_lattice_planning_node 87 | ego_slc_lattice_planning_node.cpp 88 | planning_node.cpp 89 | ../common/convert_to_visualization_msgs.cpp 90 | ) 91 | target_link_libraries(ego_slc_lattice_planning_node 92 | routing_algos 93 | planning_algos 94 | ${Carla_LIBRARIES} 95 | ${Boost_LIBRARIES} 96 | ${catkin_LIBRARIES} 97 | ${PCL_LIBRARIES} 98 | ) 99 | add_dependencies(ego_slc_lattice_planning_node 100 | routing_algos 101 | planning_algos 102 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 103 | ${catkin_EXPORTED_TARGETS} 104 | ) 105 | 106 | -------------------------------------------------------------------------------- /src/node/planner/agents_lane_following_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace node { 23 | 24 | class AgentsLaneFollowingNode : public PlanningNode { 25 | 26 | private: 27 | 28 | using Base = PlanningNode; 29 | using This = AgentsLaneFollowingNode; 30 | 31 | public: 32 | 33 | using Ptr = boost::shared_ptr; 34 | using ConstPtr = boost::shared_ptr; 35 | 36 | protected: 37 | 38 | /// Stores the base policy and noise pair. 39 | std::unordered_map> agent_policy_; 40 | 41 | /// Stores the IDMs for different agents. 42 | std::unordered_map> agent_idm_; 43 | 44 | mutable actionlib::SimpleActionServer< 45 | conformal_lattice_planner::AgentPlanAction> server_; 46 | 47 | public: 48 | 49 | AgentsLaneFollowingNode(ros::NodeHandle& nh) : 50 | Base(nh), 51 | server_(nh, "agents_plan", boost::bind(&AgentsLaneFollowingNode::executeCallback, this, _1), false) {} 52 | 53 | virtual ~AgentsLaneFollowingNode() {} 54 | 55 | virtual bool initialize() override; 56 | 57 | protected: 58 | 59 | void perturbAgentPolicies( 60 | const boost::shared_ptr& snapshot); 61 | 62 | void manageAgentIdms( 63 | const boost::shared_ptr& snapshot); 64 | 65 | virtual void executeCallback( 66 | const conformal_lattice_planner::AgentPlanGoalConstPtr& goal); 67 | 68 | }; // End class AgentsLaneFollowingNode. 69 | 70 | using AgentsLaneFollowingNodePtr = AgentsLaneFollowingNode::Ptr; 71 | using AgentsLaneFollowingNodeConstPtr = AgentsLaneFollowingNode::ConstPtr; 72 | 73 | } // End namespace node. 74 | 75 | -------------------------------------------------------------------------------- /src/node/planner/ego_idm_lattice_planning_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | using namespace router; 28 | using namespace planner; 29 | using namespace planner::idm_lattice_planner; 30 | 31 | namespace node { 32 | 33 | bool EgoIDMLatticePlanningNode::initialize() { 34 | 35 | // Create the publishers. 36 | path_pub_ = nh_.advertise( 37 | "ego_path", 1, true); 38 | conformal_lattice_pub_ = nh_.advertise( 39 | "conformal_lattice", 1, true); 40 | waypoint_lattice_pub_ = nh_.advertise( 41 | "waypoint_lattice", 1, true); 42 | 43 | bool all_param_exist = true; 44 | 45 | std::string host = "localhost"; 46 | int port = 2000; 47 | all_param_exist &= nh_.param("host", host, "localhost"); 48 | all_param_exist &= nh_.param("port", port, 2000); 49 | 50 | // Get the world. 51 | ROS_INFO_NAMED("ego_planner", "connect to the server."); 52 | client_ = boost::make_shared(host, port); 53 | client_->SetTimeout(std::chrono::seconds(10)); 54 | world_ = boost::make_shared(client_->GetWorld()); 55 | ros::Duration(1.0).sleep(); 56 | 57 | // Get the world and map. 58 | world_ = boost::make_shared(client_->GetWorld()); 59 | map_ = world_->GetMap(); 60 | fast_map_ = boost::make_shared(map_); 61 | 62 | // Initialize the path and speed planner. 63 | boost::shared_ptr router = boost::make_shared(); 64 | path_planner_ = boost::make_shared(0.1, 150.0, router, map_, fast_map_); 65 | speed_planner_ = boost::make_shared(); 66 | 67 | // Start the action server. 68 | ROS_INFO_NAMED("ego_planner", "start action server."); 69 | server_.start(); 70 | 71 | ROS_INFO_NAMED("ego_planner", "initialization finishes."); 72 | return all_param_exist; 73 | } 74 | 75 | void EgoIDMLatticePlanningNode::executeCallback( 76 | const conformal_lattice_planner::EgoPlanGoalConstPtr& goal) { 77 | 78 | ROS_INFO_NAMED("ego_planner", "executeCallback()"); 79 | 80 | // Update the carla world and map. 81 | world_ = boost::make_shared(client_->GetWorld()); 82 | //map_ = world_->GetMap(); 83 | 84 | // Create the current snapshot. 85 | boost::shared_ptr snapshot = createSnapshot(goal->snapshot); 86 | 87 | // Plan path. 88 | ros::Time start_time = ros::Time::now(); 89 | const DiscretePath ego_path = path_planner_->planPath(snapshot->ego().id(), *snapshot); 90 | ros::Duration path_planning_time = ros::Time::now() - start_time; 91 | 92 | // Publish the station graph. 93 | //conformal_lattice_pub_.publish(createConformalLatticeMsg( 94 | // path_planner_->nodes(), path_planner_->edges())); 95 | path_pub_.publish(createEgoPathMsg(ego_path)); 96 | //waypoint_lattice_pub_.publish(createWaypointLatticeMsg(path_planner_->waypointLattice())); 97 | 98 | // Plan speed. 99 | const double ego_accel = speed_planner_->planSpeed(snapshot->ego().id(), *snapshot); 100 | 101 | // Update the ego vehicle in the simulator. 102 | double dt = 0.05; 103 | nh_.param("fixed_delta_seconds", dt, 0.05); 104 | 105 | const double movement = snapshot->ego().speed()*dt + 0.5*ego_accel*dt*dt; 106 | const std::pair updated_transform_curvature = ego_path.transformAt(movement); 107 | const CarlaTransform updated_transform = updated_transform_curvature.first; 108 | const double updated_curvature = updated_transform_curvature.second; 109 | const double updated_speed = snapshot->ego().speed() + ego_accel*dt; 110 | 111 | ROS_INFO_NAMED("ego_planner", "ego %lu", snapshot->ego().id()); 112 | ROS_INFO_NAMED("ego_planner", "movement:%f", movement); 113 | ROS_INFO_NAMED("ego_planner", "acceleration:%f", ego_accel); 114 | ROS_INFO_NAMED("ego_planner", "speed:%f", updated_speed); 115 | ROS_INFO_NAMED("ego_planner", "transform: x:%f y:%f z:%f r:%f p:%f y:%f", 116 | updated_transform.location.x, 117 | updated_transform.location.y, 118 | updated_transform.location.z, 119 | updated_transform.rotation.roll, 120 | updated_transform.rotation.pitch, 121 | updated_transform.rotation.yaw); 122 | 123 | boost::shared_ptr ego_vehicle = carlaVehicle(snapshot->ego().id()); 124 | ego_vehicle->SetTransform(updated_transform); 125 | //ego_vehicle->SetVelocity(updated_transform.GetForwardVector()*updated_speed); 126 | 127 | // Inform the client the result of plan. 128 | planner::Vehicle updated_ego( 129 | snapshot->ego().id(), 130 | snapshot->ego().boundingBox(), 131 | updated_transform, 132 | updated_speed, 133 | snapshot->ego().policySpeed(), 134 | ego_accel, 135 | updated_curvature); 136 | 137 | conformal_lattice_planner::EgoPlanResult result; 138 | result.header.stamp = ros::Time::now(); 139 | result.success = true; 140 | result.path_type = ego_path.laneChangeType(); 141 | result.planning_time = path_planning_time.toSec(); 142 | populateVehicleMsg(updated_ego, result.ego); 143 | server_.setSucceeded(result); 144 | 145 | return; 146 | } 147 | } // End namespace node. 148 | 149 | int main(int argc, char** argv) { 150 | ros::init(argc, argv, "~"); 151 | ros::NodeHandle nh("~"); 152 | 153 | if(ros::console::set_logger_level( 154 | ROSCONSOLE_DEFAULT_NAME, 155 | ros::console::levels::Info)) { 156 | ros::console::notifyLoggerLevelsChanged(); 157 | } 158 | 159 | node::EgoIDMLatticePlanningNodePtr planner = 160 | boost::make_shared(nh); 161 | if (!planner->initialize()) { 162 | ROS_ERROR("Cannot initialize the ego IDM lattice planner."); 163 | } 164 | 165 | //ProfilerStart("conformal_lattice_planner.stat"); 166 | ros::spin(); 167 | //ProfilerStop(); 168 | return 0; 169 | } 170 | -------------------------------------------------------------------------------- /src/node/planner/ego_idm_lattice_planning_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace node { 27 | 28 | class EgoIDMLatticePlanningNode : public PlanningNode { 29 | 30 | private: 31 | 32 | using Base = PlanningNode; 33 | using This = EgoIDMLatticePlanningNode; 34 | 35 | public: 36 | 37 | using Ptr = boost::shared_ptr; 38 | using ConstPtr = boost::shared_ptr; 39 | 40 | protected: 41 | 42 | boost::shared_ptr path_planner_ = nullptr; 43 | boost::shared_ptr speed_planner_ = nullptr; 44 | 45 | mutable ros::Publisher path_pub_; 46 | mutable ros::Publisher conformal_lattice_pub_; 47 | mutable ros::Publisher waypoint_lattice_pub_; 48 | 49 | mutable actionlib::SimpleActionServer< 50 | conformal_lattice_planner::EgoPlanAction> server_; 51 | 52 | public: 53 | 54 | EgoIDMLatticePlanningNode(ros::NodeHandle& nh) : 55 | Base(nh), 56 | server_( 57 | nh, 58 | "ego_plan", 59 | boost::bind(&EgoIDMLatticePlanningNode::executeCallback, this, _1), 60 | false) {} 61 | 62 | virtual ~EgoIDMLatticePlanningNode() {} 63 | 64 | virtual bool initialize() override; 65 | 66 | protected: 67 | 68 | virtual void executeCallback( 69 | const conformal_lattice_planner::EgoPlanGoalConstPtr& goal); 70 | 71 | }; // End class EgoIDMLatticePlanningNode. 72 | 73 | using EgoIDMLatticePlanningNodePtr = EgoIDMLatticePlanningNode::Ptr; 74 | using EgoIDMLatticePlanningNodeConstPtr = EgoIDMLatticePlanningNode::ConstPtr; 75 | 76 | } // End namespace node. 77 | -------------------------------------------------------------------------------- /src/node/planner/ego_lane_following_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | using namespace router; 30 | using namespace planner; 31 | using namespace planner::lane_follower; 32 | 33 | namespace node { 34 | 35 | bool EgoLaneFollowingNode::initialize() { 36 | 37 | // Create publishers and subscribers. 38 | path_pub_ = nh_.advertise("ego_path", 1, true); 39 | 40 | // Load parameters. 41 | bool all_param_exist = true; 42 | 43 | std::string host = "localhost"; 44 | int port = 2000; 45 | all_param_exist &= nh_.param("host", host, "localhost"); 46 | all_param_exist &= nh_.param("port", port, 2000); 47 | 48 | // Get the world. 49 | ROS_INFO_NAMED("ego_planner", "connect to the server."); 50 | client_ = boost::make_shared(host, port); 51 | client_->SetTimeout(std::chrono::seconds(10)); 52 | client_->GetWorld(); 53 | 54 | // Create world and map. 55 | world_ = boost::make_shared(client_->GetWorld()); 56 | map_ = world_->GetMap(); 57 | fast_map_ = boost::make_shared(map_); 58 | 59 | // Start the action server. 60 | ROS_INFO_NAMED("ego_planner", "start action server."); 61 | server_.start(); 62 | 63 | ROS_INFO_NAMED("ego_planner", "initialization finishes."); 64 | return all_param_exist; 65 | } 66 | 67 | void EgoLaneFollowingNode::executeCallback( 68 | const conformal_lattice_planner::EgoPlanGoalConstPtr& goal) { 69 | 70 | ROS_INFO_NAMED("ego_planner", "executeCallback()"); 71 | 72 | // Update the carla world and map. 73 | world_ = boost::make_shared(client_->GetWorld()); 74 | //map_ = world_->GetMap(); 75 | 76 | // Create the current snapshot. 77 | boost::shared_ptr snapshot = createSnapshot(goal->snapshot); 78 | 79 | boost::shared_ptr ego_waypoint = 80 | carlaVehicleWaypoint(snapshot->ego().id()); 81 | 82 | // Plan path. 83 | // The range of the lattice is just enough for the ego vehicle. 84 | boost::shared_ptr path_planner = 85 | boost::make_shared( 86 | map_, fast_map_, carlaVehicleWaypoint(snapshot->ego().id()), 55.0, router_); 87 | 88 | const DiscretePath ego_path = 89 | path_planner->planPath(snapshot->ego().id(), *snapshot); 90 | 91 | // Plan speed. 92 | boost::shared_ptr speed_planner = 93 | boost::make_shared(); 94 | const double ego_accel = speed_planner->planSpeed(snapshot->ego().id(), *snapshot); 95 | 96 | // Compute the target speed and transform of the ego. 97 | double dt = 0.05; 98 | nh_.param("fixed_delta_seconds", dt, 0.05); 99 | 100 | const double movement = snapshot->ego().speed()*dt + 0.5*ego_accel*dt*dt; 101 | const CarlaTransform updated_transform = ego_path.transformAt(movement).first; 102 | const double updated_speed = snapshot->ego().speed() + ego_accel*dt; 103 | 104 | ROS_INFO_NAMED("ego_planner", "ego %lu", snapshot->ego().id()); 105 | ROS_INFO_NAMED("ego_planner", "movement:%f", movement); 106 | ROS_INFO_NAMED("ego_planner", "acceleration:%f", ego_accel); 107 | ROS_INFO_NAMED("ego_planner", "speed:%f", updated_speed); 108 | ROS_INFO_NAMED("ego_planner", "transform: x:%f y:%f z:%f r:%f p:%f y:%f", 109 | updated_transform.location.x, 110 | updated_transform.location.y, 111 | updated_transform.location.z, 112 | updated_transform.rotation.roll, 113 | updated_transform.rotation.pitch, 114 | updated_transform.rotation.yaw); 115 | 116 | // Update the transform of the ego in the simulator. 117 | boost::shared_ptr ego_vehicle = carlaVehicle(snapshot->ego().id()); 118 | ego_vehicle->SetTransform(updated_transform); 119 | //ego_vehicle->SetVelocity(updated_transform.GetForwardVector()*updated_speed); 120 | 121 | // Publish the path planned for the ego. 122 | path_pub_.publish(createEgoPathMsg(ego_path)); 123 | 124 | // Inform the client the result of plan. 125 | planner::Vehicle updated_ego( 126 | snapshot->ego().id(), 127 | snapshot->ego().boundingBox(), 128 | updated_transform, 129 | updated_speed, 130 | snapshot->ego().policySpeed(), 131 | ego_accel, 132 | utils::curvatureAtWaypoint(map_->GetWaypoint(updated_transform.location), map_)); 133 | 134 | conformal_lattice_planner::EgoPlanResult result; 135 | result.header.stamp = ros::Time::now(); 136 | result.success = true; 137 | result.path_type = conformal_lattice_planner::EgoPlanResult::LANE_KEEP; 138 | populateVehicleMsg(updated_ego, result.ego); 139 | server_.setSucceeded(result); 140 | 141 | return; 142 | } 143 | 144 | } // End namespace node. 145 | 146 | int main(int argc, char** argv) { 147 | ros::init(argc, argv, "~"); 148 | ros::NodeHandle nh("~"); 149 | 150 | if(ros::console::set_logger_level( 151 | ROSCONSOLE_DEFAULT_NAME, 152 | ros::console::levels::Info)) { 153 | ros::console::notifyLoggerLevelsChanged(); 154 | } 155 | 156 | node::EgoLaneFollowingNodePtr planner = 157 | boost::make_shared(nh); 158 | if (!planner->initialize()) { 159 | ROS_ERROR("Cannot initialize the ego lane following planner."); 160 | } 161 | 162 | ros::spin(); 163 | return 0; 164 | } 165 | -------------------------------------------------------------------------------- /src/node/planner/ego_lane_following_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | namespace node { 22 | 23 | class EgoLaneFollowingNode : public PlanningNode { 24 | 25 | private: 26 | 27 | using Base = PlanningNode; 28 | using This = EgoLaneFollowingNode; 29 | 30 | public: 31 | 32 | using Ptr = boost::shared_ptr; 33 | using ConstPtr = boost::shared_ptr; 34 | 35 | protected: 36 | 37 | mutable ros::Publisher path_pub_; 38 | mutable actionlib::SimpleActionServer< 39 | conformal_lattice_planner::EgoPlanAction> server_; 40 | 41 | public: 42 | 43 | EgoLaneFollowingNode(ros::NodeHandle& nh) : 44 | Base(nh), 45 | server_(nh, "ego_plan", boost::bind(&EgoLaneFollowingNode::executeCallback, this, _1), false) {} 46 | 47 | virtual ~EgoLaneFollowingNode() {} 48 | 49 | virtual bool initialize() override; 50 | 51 | protected: 52 | 53 | virtual void executeCallback( 54 | const conformal_lattice_planner::EgoPlanGoalConstPtr& goal); 55 | 56 | }; // End class EgoLaneFollowingNode. 57 | 58 | using EgoLaneFollowingNodePtr = EgoLaneFollowingNode::Ptr; 59 | using EgoLaneFollowingNodeConstPtr = EgoLaneFollowingNode::ConstPtr; 60 | 61 | } // End namespace node. 62 | -------------------------------------------------------------------------------- /src/node/planner/ego_slc_lattice_planning_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace node { 27 | 28 | class EgoSLCLatticePlanningNode : public PlanningNode { 29 | 30 | private: 31 | 32 | using Base = PlanningNode; 33 | using This = EgoSLCLatticePlanningNode; 34 | 35 | public: 36 | 37 | using Ptr = boost::shared_ptr; 38 | using ConstPtr = boost::shared_ptr; 39 | 40 | protected: 41 | 42 | boost::shared_ptr path_planner_ = nullptr; 43 | boost::shared_ptr speed_planner_ = nullptr; 44 | 45 | mutable ros::Publisher path_pub_; 46 | mutable ros::Publisher conformal_lattice_pub_; 47 | mutable ros::Publisher waypoint_lattice_pub_; 48 | 49 | mutable actionlib::SimpleActionServer< 50 | conformal_lattice_planner::EgoPlanAction> server_; 51 | 52 | public: 53 | 54 | EgoSLCLatticePlanningNode(ros::NodeHandle& nh) : 55 | Base(nh), 56 | server_( 57 | nh, 58 | "ego_plan", 59 | boost::bind(&EgoSLCLatticePlanningNode::executeCallback, this, _1), 60 | false) {} 61 | 62 | virtual ~EgoSLCLatticePlanningNode() {} 63 | 64 | virtual bool initialize() override; 65 | 66 | protected: 67 | 68 | virtual void executeCallback( 69 | const conformal_lattice_planner::EgoPlanGoalConstPtr& goal); 70 | 71 | }; // End class EgoSLCLatticePlanningNode. 72 | 73 | using EgoSLCLatticePlanningNodePtr = EgoSLCLatticePlanningNode::Ptr; 74 | using EgoSLCLatticePlanningNodeConstPtr = EgoSLCLatticePlanningNode::ConstPtr; 75 | 76 | } // End namespace node. 77 | 78 | -------------------------------------------------------------------------------- /src/node/planner/ego_spatiotemporal_lattice_planning_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace node { 27 | 28 | class EgoSpatiotemporalLatticePlanningNode : public PlanningNode { 29 | 30 | private: 31 | 32 | using Base = PlanningNode; 33 | using This = EgoSpatiotemporalLatticePlanningNode; 34 | 35 | public: 36 | 37 | using Ptr = boost::shared_ptr; 38 | using ConstPtr = boost::shared_ptr; 39 | 40 | protected: 41 | 42 | boost::shared_ptr traj_planner_ = nullptr; 43 | 44 | mutable ros::Publisher path_pub_; 45 | mutable ros::Publisher conformal_lattice_pub_; 46 | mutable ros::Publisher waypoint_lattice_pub_; 47 | 48 | mutable actionlib::SimpleActionServer< 49 | conformal_lattice_planner::EgoPlanAction> server_; 50 | 51 | public: 52 | 53 | EgoSpatiotemporalLatticePlanningNode(ros::NodeHandle& nh) : 54 | Base(nh), 55 | server_( 56 | nh, 57 | "ego_plan", 58 | boost::bind(&EgoSpatiotemporalLatticePlanningNode::executeCallback, this, _1), 59 | false) {} 60 | 61 | virtual ~EgoSpatiotemporalLatticePlanningNode() {} 62 | 63 | virtual bool initialize() override; 64 | 65 | protected: 66 | 67 | virtual void executeCallback( 68 | const conformal_lattice_planner::EgoPlanGoalConstPtr& goal); 69 | 70 | }; // End class EgoSpatiotemporalLatticePlanningNode. 71 | 72 | using EgoSpatiotemporalLatticePlanningNodePtr = EgoSpatiotemporalLatticePlanningNode::Ptr; 73 | using EgoSpatiotemporalLatticePlanningNodeConstPtr = EgoSpatiotemporalLatticePlanningNode::ConstPtr; 74 | 75 | } // End namespace node. 76 | 77 | -------------------------------------------------------------------------------- /src/node/planner/planning_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | 21 | namespace node { 22 | 23 | boost::shared_ptr PlanningNode::createSnapshot( 24 | const conformal_lattice_planner::TrafficSnapshot& snapshot_msg) { 25 | 26 | // Create the ego vehicle. 27 | planner::Vehicle ego_vehicle; 28 | populateVehicleObj(snapshot_msg.ego, ego_vehicle); 29 | 30 | if (ego_vehicle.transform().location.x==0.0 && 31 | ego_vehicle.transform().location.y==0.0 && 32 | ego_vehicle.transform().location.z==0.0) { 33 | std::string error_msg( 34 | "PlanningNode::createSnapshot(): " 35 | "ego vehicle is set back to origin.\n"); 36 | std::string ego_msg = ( 37 | boost::format("Ego ID: %lu\n") % ego_vehicle.id()).str(); 38 | throw std::runtime_error(error_msg + ego_msg); 39 | } 40 | 41 | // Create the agent vehicles. 42 | std::unordered_map agent_vehicles; 43 | for (const auto& agent : snapshot_msg.agents) { 44 | planner::Vehicle agent_vehicle; 45 | populateVehicleObj(agent, agent_vehicle); 46 | 47 | if (agent_vehicle.transform().location.x==0.0 && 48 | agent_vehicle.transform().location.y==0.0 && 49 | agent_vehicle.transform().location.z==0.0) { 50 | std::string error_msg( 51 | "PlanningNode::createSnapshot(): " 52 | "an agent vehicle is set back to origin.\n"); 53 | std::string agent_msg = ( 54 | boost::format("Agent ID: %lu\n") % agent_vehicle.id()).str(); 55 | throw std::runtime_error(error_msg + agent_msg); 56 | } 57 | 58 | agent_vehicles[agent_vehicle.id()] = agent_vehicle; 59 | } 60 | 61 | // Create the snapshot. 62 | return boost::make_shared( 63 | ego_vehicle, agent_vehicles, router_, map_, fast_map_); 64 | } 65 | 66 | void PlanningNode::populateVehicleMsg( 67 | const planner::Vehicle& vehicle_obj, 68 | conformal_lattice_planner::Vehicle& vehicle_msg) { 69 | // ID. 70 | vehicle_msg.id = vehicle_obj.id(); 71 | // Bounding box. 72 | vehicle_msg.bounding_box.extent.x = vehicle_obj.boundingBox().extent.x; 73 | vehicle_msg.bounding_box.extent.y = vehicle_obj.boundingBox().extent.y; 74 | vehicle_msg.bounding_box.extent.z = vehicle_obj.boundingBox().extent.z; 75 | vehicle_msg.bounding_box.location.x = vehicle_obj.boundingBox().location.x; 76 | vehicle_msg.bounding_box.location.y = vehicle_obj.boundingBox().location.y; 77 | vehicle_msg.bounding_box.location.z = vehicle_obj.boundingBox().location.z; 78 | // Transform. 79 | vehicle_msg.transform.position.x = vehicle_obj.transform().location.x; 80 | vehicle_msg.transform.position.y = vehicle_obj.transform().location.y; 81 | vehicle_msg.transform.position.z = vehicle_obj.transform().location.z; 82 | tf2::Matrix3x3 tf_mat; 83 | tf_mat.setRPY(vehicle_obj.transform().rotation.roll /180.0*M_PI, 84 | vehicle_obj.transform().rotation.pitch/180.0*M_PI, 85 | vehicle_obj.transform().rotation.yaw /180.0*M_PI); 86 | tf2::Quaternion tf_quat; 87 | tf_mat.getRotation(tf_quat); 88 | vehicle_msg.transform.orientation = tf2::toMsg(tf_quat); 89 | // Speed. 90 | vehicle_msg.speed = vehicle_obj.speed(); 91 | // Acceleration. 92 | vehicle_msg.acceleration = vehicle_obj.acceleration(); 93 | // Curvature. 94 | vehicle_msg.curvature = vehicle_obj.curvature(); 95 | // policy speed. 96 | vehicle_msg.policy_speed = vehicle_obj.policySpeed(); 97 | return; 98 | } 99 | 100 | void PlanningNode::populateVehicleObj( 101 | const conformal_lattice_planner::Vehicle& vehicle_msg, 102 | planner::Vehicle& vehicle_obj) { 103 | // ID. 104 | vehicle_obj.id() = vehicle_msg.id; 105 | // Bounding box. 106 | vehicle_obj.boundingBox().extent.x = vehicle_msg.bounding_box.extent.x; 107 | vehicle_obj.boundingBox().extent.y = vehicle_msg.bounding_box.extent.y; 108 | vehicle_obj.boundingBox().extent.z = vehicle_msg.bounding_box.extent.z; 109 | vehicle_obj.boundingBox().location.x = vehicle_msg.bounding_box.location.x; 110 | vehicle_obj.boundingBox().location.y = vehicle_msg.bounding_box.location.y; 111 | vehicle_obj.boundingBox().location.z = vehicle_msg.bounding_box.location.z; 112 | // Transform. 113 | vehicle_obj.transform().location.x = vehicle_msg.transform.position.x; 114 | vehicle_obj.transform().location.y = vehicle_msg.transform.position.y; 115 | vehicle_obj.transform().location.z = vehicle_msg.transform.position.z; 116 | 117 | tf2::Quaternion tf_quat; 118 | tf2::fromMsg(vehicle_msg.transform.orientation, tf_quat); 119 | tf2::Matrix3x3 tf_mat(tf_quat); 120 | double yaw, pitch, roll; 121 | tf_mat.getRPY(roll, pitch, yaw); 122 | vehicle_obj.transform().rotation.yaw = yaw /M_PI*180.0; 123 | vehicle_obj.transform().rotation.pitch = pitch/M_PI*180.0; 124 | vehicle_obj.transform().rotation.roll = roll /M_PI*180.0; 125 | // Speed. 126 | vehicle_obj.speed() = vehicle_msg.speed; 127 | // Acceleration. 128 | vehicle_obj.acceleration() = vehicle_msg.acceleration; 129 | // Curvature. 130 | vehicle_obj.curvature() = vehicle_msg.curvature; 131 | // Policy speed. 132 | vehicle_obj.policySpeed() = vehicle_msg.policy_speed; 133 | return; 134 | } 135 | 136 | } // End namespace node. 137 | 138 | -------------------------------------------------------------------------------- /src/node/planner/planning_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace node { 40 | 41 | class PlanningNode : private boost::noncopyable { 42 | 43 | protected: 44 | 45 | using CarlaClient = carla::client::Client; 46 | using CarlaWorld = carla::client::World; 47 | using CarlaMap = carla::client::Map; 48 | using CarlaWaypoint = carla::client::Waypoint; 49 | using CarlaVehicle = carla::client::Vehicle; 50 | using CarlaTransform = carla::geom::Transform; 51 | 52 | protected: 53 | 54 | boost::shared_ptr router_ = nullptr; 55 | boost::shared_ptr fast_map_ = nullptr; 56 | 57 | boost::shared_ptr client_ = nullptr; 58 | boost::shared_ptr world_ = nullptr; 59 | boost::shared_ptr map_ = nullptr; 60 | 61 | mutable ros::NodeHandle nh_; 62 | 63 | public: 64 | 65 | PlanningNode(ros::NodeHandle& nh) : 66 | router_(boost::make_shared()), nh_(nh) {} 67 | 68 | virtual ~PlanningNode() {} 69 | 70 | virtual bool initialize() = 0; 71 | 72 | protected: 73 | 74 | virtual boost::shared_ptr createSnapshot( 75 | const conformal_lattice_planner::TrafficSnapshot& snapshot_msg); 76 | 77 | /// Populate the vehicle msg through object. 78 | virtual void populateVehicleMsg( 79 | const planner::Vehicle& vehicle_obj, 80 | conformal_lattice_planner::Vehicle& vehicle_msg); 81 | 82 | /// Populate the vehicle object through msg. 83 | virtual void populateVehicleObj( 84 | const conformal_lattice_planner::Vehicle& vehicle_msg, 85 | planner::Vehicle& vehicle_obj); 86 | 87 | /// Get the carla vehicle by ID. 88 | boost::shared_ptr carlaVehicle(const size_t id) const { 89 | boost::shared_ptr vehicle = 90 | boost::dynamic_pointer_cast(world_->GetActor(id)); 91 | if (!vehicle) { 92 | throw std::runtime_error( 93 | "PlanningNode::carlaVehicle(): " 94 | "Cannot get the required vehicle in the carla server."); 95 | } 96 | return vehicle; 97 | } 98 | 99 | /// Get the carla vehicle transform by the vehicle ID. 100 | CarlaTransform carlaVehicleTransform(const size_t id) const { 101 | return carlaVehicle(id)->GetTransform(); 102 | } 103 | 104 | /// Get the carla waypoint a vehicle is at by its ID. 105 | boost::shared_ptr carlaVehicleWaypoint(const size_t id) const { 106 | return fast_map_->waypoint(carlaVehicleTransform(id).location); 107 | } 108 | 109 | }; // End class PlanningNode. 110 | 111 | } // End namespace node. 112 | 113 | -------------------------------------------------------------------------------- /src/node/simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # No traffic simulator node 2 | add_executable(no_traffic_node 3 | no_traffic_node.cpp 4 | simulator_node.cpp 5 | ../common/convert_to_visualization_msgs.cpp 6 | ) 7 | target_link_libraries(no_traffic_node 8 | routing_algos 9 | planning_algos 10 | ${Carla_LIBRARIES} 11 | ${Boost_LIBRARIES} 12 | ${catkin_LIBRARIES} 13 | ${PCL_LIBRARIES} 14 | ) 15 | add_dependencies(no_traffic_node 16 | routing_algos 17 | planning_algos 18 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 19 | ${catkin_EXPORTED_TARGETS} 20 | ) 21 | 22 | # Fixed scenario simulator node 23 | add_executable(fixed_scenario_node 24 | fixed_scenario_node.cpp 25 | simulator_node.cpp 26 | ../common/convert_to_visualization_msgs.cpp 27 | ) 28 | target_link_libraries(fixed_scenario_node 29 | routing_algos 30 | planning_algos 31 | ${Carla_LIBRARIES} 32 | ${Boost_LIBRARIES} 33 | ${catkin_LIBRARIES} 34 | ${PCL_LIBRARIES} 35 | ) 36 | add_dependencies(fixed_scenario_node 37 | routing_algos 38 | planning_algos 39 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 40 | ${catkin_EXPORTED_TARGETS} 41 | ) 42 | 43 | # Random traffic simulator node 44 | add_executable(random_traffic_node 45 | random_traffic_node.cpp 46 | simulator_node.cpp 47 | ../common/convert_to_visualization_msgs.cpp 48 | ) 49 | target_link_libraries(random_traffic_node 50 | routing_algos 51 | planning_algos 52 | ${Carla_LIBRARIES} 53 | ${Boost_LIBRARIES} 54 | ${catkin_LIBRARIES} 55 | ${PCL_LIBRARIES} 56 | ) 57 | add_dependencies(random_traffic_node 58 | routing_algos 59 | planning_algos 60 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 61 | ${catkin_EXPORTED_TARGETS} 62 | ) 63 | -------------------------------------------------------------------------------- /src/node/simulator/fixed_scenario_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | using namespace router; 25 | using namespace planner; 26 | 27 | namespace node { 28 | 29 | void FixedScenarioNode::spawnVehicles() { 30 | 31 | // The start position. 32 | // TODO: This may be load from the ROS parameter server. 33 | std::array start_pt{0, 0, 0}; 34 | 35 | // Find the available spawn point cloest to the start point. 36 | std::vector spawn_points = 37 | map_->GetRecommendedSpawnPoints(); 38 | CarlaTransform start_transform; 39 | double min_distance_sq = std::numeric_limits::max(); 40 | 41 | for (const auto& pt : spawn_points) { 42 | const double x_diff = pt.location.x - start_pt[0]; 43 | const double y_diff = pt.location.y - start_pt[1]; 44 | const double z_diff = pt.location.z - start_pt[2]; 45 | const double distance_sq = x_diff*x_diff + y_diff*y_diff + z_diff*z_diff; 46 | if (distance_sq < min_distance_sq) { 47 | start_transform = pt; 48 | min_distance_sq = distance_sq; 49 | } 50 | } 51 | ROS_INFO_NAMED("carla_simulator", "Start waypoint transform\nx:%f y:%f z:%f", 52 | start_transform.location.x, start_transform.location.y, start_transform.location.z); 53 | 54 | // Start waypoint of the lattice. 55 | boost::shared_ptr start_waypoint = 56 | fast_map_->waypoint(start_transform.location); 57 | 58 | boost::shared_ptr waypoint_lattice= 59 | boost::make_shared(start_waypoint, 150, 1.0, loop_router_); 60 | 61 | // Spawn the ego vehicle. 62 | // The ego vehicle is at 50m on the lattice, and there is an 100m buffer 63 | // in the front of the ego vehicle. 64 | boost::shared_ptr ego_waypoint = 65 | waypoint_lattice->front(start_waypoint, 50.0)->waypoint(); 66 | if (!ego_waypoint) { 67 | throw std::runtime_error("Cannot find the ego waypoint on the traffic lattice."); 68 | } 69 | ROS_INFO_NAMED("carla_simulator", "Ego vehicle initial transform\nx:%f y:%f z:%f", 70 | ego_waypoint->GetTransform().location.x, 71 | ego_waypoint->GetTransform().location.y, 72 | ego_waypoint->GetTransform().location.z); 73 | 74 | if (!spawnEgoVehicle(ego_waypoint, 20, false)) { 75 | throw std::runtime_error("Cannot spawn the ego vehicle."); 76 | } 77 | ego_.speed() = 15.0; 78 | 79 | // Spawn agent vehicles. 80 | // Braking scenario. 81 | //{ 82 | // boost::shared_ptr agent_waypoint = 83 | // waypoint_lattice->front(ego_waypoint, 30.0)->waypoint(); 84 | // if (!spawnAgentVehicle(agent_waypoint, 5.0, false)) { 85 | // throw std::runtime_error("Cannot spawn an agent vehicle."); 86 | // } 87 | //} 88 | 89 | // Lane merging scenario. 90 | { 91 | boost::shared_ptr agent_waypoint = 92 | waypoint_lattice->front(ego_waypoint, 20.0)->waypoint(); 93 | if (!spawnAgentVehicle(agent_waypoint, 15.0, false)) { 94 | throw std::runtime_error("Cannot spawn an agent vehicle."); 95 | } 96 | } 97 | 98 | { 99 | boost::shared_ptr agent_waypoint = 100 | waypoint_lattice->rightBack(ego_waypoint, 20.0)->waypoint(); 101 | if (!spawnAgentVehicle(agent_waypoint, 20.0, false)) { 102 | throw std::runtime_error("Cannot spawn an agent vehicle."); 103 | } 104 | } 105 | 106 | { 107 | boost::shared_ptr agent_waypoint = 108 | waypoint_lattice->rightFront(ego_waypoint, 20.0)->waypoint(); 109 | if (!spawnAgentVehicle(agent_waypoint, 20.0, false)) { 110 | throw std::runtime_error("Cannot spawn an agent vehicle."); 111 | } 112 | } 113 | 114 | { 115 | boost::shared_ptr intermediate_waypoint = 116 | waypoint_lattice->rightFront(ego_waypoint, 25.0)->waypoint(); 117 | boost::shared_ptr agent_waypoint = 118 | intermediate_waypoint->GetRight(); 119 | if (!spawnAgentVehicle(agent_waypoint, 15.0, false)) { 120 | throw std::runtime_error("Cannot spawn an agent vehicle."); 121 | } 122 | } 123 | 124 | // Spawn the following camera of the ego vehicle. 125 | spawnCamera(); 126 | return; 127 | } 128 | 129 | } // End namespace node. 130 | 131 | int main(int argc, char** argv) { 132 | 133 | ros::init(argc, argv, "~"); 134 | ros::NodeHandle nh("~"); 135 | 136 | if(ros::console::set_logger_level( 137 | ROSCONSOLE_DEFAULT_NAME, 138 | ros::console::levels::Info)) { 139 | ros::console::notifyLoggerLevelsChanged(); 140 | } 141 | 142 | node::FixedScenarioNodePtr sim = 143 | boost::make_shared(nh); 144 | if (!sim->initialize()) { 145 | ROS_ERROR("Cannot initialize the CARLA simulator."); 146 | } 147 | 148 | ros::spin(); 149 | return 0; 150 | } 151 | -------------------------------------------------------------------------------- /src/node/simulator/fixed_scenario_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace node { 20 | 21 | class FixedScenarioNode : public SimulatorNode { 22 | 23 | private: 24 | 25 | using Base = SimulatorNode; 26 | using This = FixedScenarioNode; 27 | 28 | public: 29 | 30 | using Ptr = boost::shared_ptr; 31 | using ConstPtr = boost::shared_ptr; 32 | 33 | protected: 34 | 35 | public: 36 | 37 | FixedScenarioNode(ros::NodeHandle nh) : Base(nh) {} 38 | 39 | protected: 40 | 41 | virtual void spawnVehicles() override; 42 | 43 | }; // End class FixedScenarioNode. 44 | 45 | using FixedScenarioNodePtr = FixedScenarioNode::Ptr; 46 | using FixedScenarioNodeConstPtr = FixedScenarioNode::ConstPtr; 47 | 48 | } // End namespace node. 49 | 50 | -------------------------------------------------------------------------------- /src/node/simulator/no_traffic_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | namespace node { 20 | 21 | class NoTrafficNode : public SimulatorNode { 22 | 23 | private: 24 | 25 | using Base = SimulatorNode; 26 | using This = NoTrafficNode; 27 | 28 | public: 29 | 30 | using Ptr = boost::shared_ptr; 31 | using ConstPtr = boost::shared_ptr; 32 | 33 | protected: 34 | 35 | 36 | public: 37 | 38 | NoTrafficNode(ros::NodeHandle nh) : Base(nh) {} 39 | 40 | virtual bool initialize() override; 41 | 42 | protected: 43 | 44 | virtual void spawnVehicles() override; 45 | 46 | /// Since there is no agent vehicles in this object, we don't have 47 | /// to send the goals for the agent vehicles. 48 | virtual void tickWorld() override { 49 | world_->Tick(); 50 | updateSimTime(); 51 | publishTraffic(); 52 | sendEgoGoal(); 53 | return; 54 | } 55 | 56 | }; // End class NoTrafficNode. 57 | 58 | using NoTrafficNodePtr = NoTrafficNode::Ptr; 59 | using NoTrafficNodeConstPtr = NoTrafficNode::ConstPtr; 60 | 61 | } // End namespace node. 62 | 63 | -------------------------------------------------------------------------------- /src/node/simulator/random_traffic_node.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace node { 21 | 22 | class RandomTrafficNode : public SimulatorNode { 23 | 24 | private: 25 | 26 | using Base = SimulatorNode; 27 | using This = RandomTrafficNode; 28 | 29 | public: 30 | 31 | using Ptr = boost::shared_ptr; 32 | using ConstPtr = boost::shared_ptr; 33 | 34 | protected: 35 | 36 | using CarlaSensor = carla::client::Sensor; 37 | using CarlaSensorData = carla::sensor::SensorData; 38 | using CarlaBGRAImage = carla::sensor::data::Image; 39 | 40 | protected: 41 | 42 | /// Traffic lattice. 43 | boost::shared_ptr traffic_manager_; 44 | 45 | /// Nominal policy speed of all vehicles. 46 | const double nominal_policy_speed_ = 20.0; 47 | 48 | public: 49 | 50 | RandomTrafficNode(ros::NodeHandle nh) : Base(nh) {} 51 | 52 | protected: 53 | 54 | virtual void spawnVehicles() override; 55 | 56 | virtual boost::optional spawnEgoVehicle( 57 | const boost::shared_ptr& waypoint, 58 | const double policy_speed, 59 | const bool noisy_speed = true) override; 60 | 61 | virtual boost::optional spawnAgentVehicle( 62 | const boost::shared_ptr& waypoint, 63 | const double policy_speed, 64 | const bool noisy_speed = true) override; 65 | 66 | /// Manager (add/delete) the vehicles in the simulation. 67 | void manageTraffic(); 68 | 69 | virtual void tickWorld() override; 70 | 71 | virtual void publishTraffic() const override; 72 | 73 | }; // End class RandomTrafficNode. 74 | 75 | using RandomTrafficNodePtr = RandomTrafficNode::Ptr; 76 | using RandomTrafficNodeConstPtr = RandomTrafficNode::ConstPtr; 77 | 78 | } // End namespace node. 79 | -------------------------------------------------------------------------------- /src/planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(planner_srcs 2 | common/traffic_lattice.cpp 3 | common/traffic_manager.cpp 4 | common/snapshot.cpp 5 | common/utils.cpp 6 | common/vehicle_path.cpp 7 | common/traffic_simulator.cpp 8 | idm_lattice_planner/idm_lattice_planner.cpp 9 | spatiotemporal_lattice_planner/spatiotemporal_lattice_planner.cpp 10 | slc_lattice_planner/slc_lattice_planner.cpp 11 | ) 12 | 13 | add_library(planning_algos 14 | ${planner_srcs} 15 | ) 16 | target_link_libraries(planning_algos 17 | routing_algos 18 | ${Carla_LIBRARIES} 19 | ${Boost_LIBRARIES} 20 | ${PCL_LIBRARIES} 21 | ) 22 | add_dependencies(planning_algos 23 | routing_algos 24 | ) 25 | 26 | add_subdirectory(tests) 27 | -------------------------------------------------------------------------------- /src/planner/common/fast_waypoint_map.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | namespace utils { 33 | 34 | class FastWaypointMap : private boost::noncopyable { 35 | 36 | protected: 37 | 38 | using CarlaMap = carla::client::Map; 39 | using CarlaWaypoint = carla::client::Waypoint; 40 | using CarlaTransform = carla::geom::Transform; 41 | using CarlaLocation = carla::geom::Location; 42 | 43 | /// Hash function of \c pcl::PointXYZ. 44 | struct PointXYZHash { 45 | size_t operator()(const pcl::PointXYZ& point) const { 46 | size_t seed = 0; 47 | hashCombine(seed, point.x, point.y, point.z); 48 | return seed; 49 | } 50 | }; 51 | 52 | /// Compare function of \c pcl::PointXYZ. 53 | struct PointXYZEqual { 54 | bool operator()(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2) const { 55 | return (p1.x==p2.x) & (p1.y==p2.y) & (p1.z==p2.z); 56 | } 57 | }; 58 | 59 | protected: 60 | 61 | /// Resolution of the waypoints (minimum distance). 62 | double resolution_; 63 | 64 | /// The map is stored as location-to-waypoint pair. 65 | std::unordered_map, 67 | PointXYZHash, 68 | PointXYZEqual> xyz_to_waypoint_table_; 69 | 70 | /// KDtree built with all waypoint locations. 71 | pcl::KdTreeFLANN kdtree_; 72 | 73 | /// The point cloud built with all waypoint locations. 74 | pcl::PointCloud::Ptr cloud_ = nullptr; 75 | 76 | public: 77 | 78 | FastWaypointMap(const boost::shared_ptr& map, 79 | const double resolution = 0.05) : resolution_(resolution) { 80 | 81 | // Generate waypoints on the map with the given resolution. 82 | const std::vector> 83 | waypoints = map->GenerateWaypoints(resolution_); 84 | 85 | // Add all waypoints to the table. 86 | // At the same time, collect all locations into a point cloud. 87 | cloud_ = boost::make_shared>(); 88 | cloud_->width = waypoints.size(); 89 | cloud_->height = 1; 90 | 91 | for (const auto& waypoint : waypoints) { 92 | const pcl::PointXYZ point = carlaLocationToPointXYZ(waypoint->GetTransform().location); 93 | xyz_to_waypoint_table_[point] = waypoint; 94 | cloud_->push_back(point); 95 | } 96 | 97 | // Build a KDtree with the point cloud. 98 | kdtree_.setEpsilon(resolution_); 99 | kdtree_.setInputCloud(cloud_); 100 | 101 | return; 102 | } 103 | 104 | /// Get the resolution of the map. 105 | const double resolution() const { return resolution_; } 106 | 107 | /// Get the number of waypoints stored in the map. 108 | const size_t size() const { return xyz_to_waypoint_table_.size(); } 109 | 110 | boost::shared_ptr waypoint( 111 | const CarlaLocation& location) const { 112 | 113 | // Create the query point based on the location. 114 | const pcl::PointXYZ query_point = carlaLocationToPointXYZ(location); 115 | 116 | // Search for the closest point. 117 | std::vector indices(1); 118 | std::vector sqr_distance(1); 119 | int num = kdtree_.nearestKSearch(query_point, 1, indices, sqr_distance); 120 | 121 | if (num <= 0) { 122 | std::string error_msg("Cannot find a waypoint close to the query location.\n"); 123 | std::string location_msg = ( 124 | boost::format("Query location: x:%1% y:%2% z:%3%\n") 125 | % location.x % location.y % location.z).str(); 126 | std::string fast_map_msg = ( 127 | boost::format("fast map size:%lu resolution:%f\n") 128 | % size() % resolution()).str(); 129 | 130 | throw std::runtime_error(error_msg + location_msg); 131 | } 132 | 133 | const pcl::PointXYZ target_point = cloud_->at(indices[0]); 134 | const auto iter = xyz_to_waypoint_table_.find(target_point); 135 | 136 | const CarlaLocation target_location( 137 | target_point.x, target_point.y, target_point.z); 138 | if ((target_location-location).Length() > resolution_) { 139 | std::string error_msg = (boost::format( 140 | "FastWaypointMap::waypoint(): " 141 | "the distance between query location and closest location > %1%") % resolution_).str(); 142 | std::string location_msg = ( 143 | boost::format("query location x:%1% y:%2% z:%3%\n") 144 | % location.x % location.y % location.z).str(); 145 | std::string target_location_msg = ( 146 | boost::format("closest location x:%1% y:%2% z:%3%\n") 147 | % target_location.x % target_location.y % target_location.z).str(); 148 | } 149 | 150 | return iter->second; 151 | } 152 | 153 | boost::shared_ptr waypoint( 154 | const CarlaTransform& transform) const { 155 | return waypoint(transform.location); 156 | } 157 | 158 | protected: 159 | 160 | pcl::PointXYZ carlaLocationToPointXYZ(const CarlaLocation& location) const { 161 | pcl::PointXYZ point; 162 | point.x = location.x; 163 | point.y = location.y; 164 | point.z = location.z; 165 | return point; 166 | } 167 | 168 | }; // End class FastWaypointMap. 169 | 170 | } // End namespace utils. 171 | -------------------------------------------------------------------------------- /src/planner/common/snapshot.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | namespace planner { 23 | 24 | Snapshot::Snapshot( 25 | const Vehicle& ego, 26 | const std::unordered_map& agents, 27 | const boost::shared_ptr& router, 28 | const boost::shared_ptr& map, 29 | const boost::shared_ptr& fast_map) : 30 | ego_(ego), 31 | agents_(agents) { 32 | 33 | // Collect all vehicles into an array of tuples. 34 | std::vector> vehicles; 35 | vehicles.push_back(ego_.tuple()); 36 | for (const auto& agent : agents_) vehicles.push_back(agent.second.tuple()); 37 | 38 | // Generate the waypoint lattice. 39 | std::unordered_set disappear_vehicles; 40 | traffic_lattice_ = boost::make_shared( 41 | vehicles, map, fast_map, router, disappear_vehicles); 42 | 43 | // Remove the disappeared vehicles. 44 | if (disappear_vehicles.count(ego_.id()) != 0) { 45 | std::string error_msg( 46 | "Snapshot::Snapshot(): " 47 | "the ego vehicle is removed from the snapshot.\n"); 48 | std::string snapshot_msg = this->string(); 49 | throw std::runtime_error(error_msg + snapshot_msg); 50 | } 51 | 52 | for (const size_t agent : disappear_vehicles) 53 | agents_.erase(agent); 54 | 55 | return; 56 | } 57 | 58 | Snapshot::Snapshot(const Snapshot& other) : 59 | ego_(other.ego_), 60 | agents_(other.agents_), 61 | traffic_lattice_(boost::make_shared(*(other.traffic_lattice_))) {} 62 | 63 | Snapshot& Snapshot::operator=(const Snapshot& other) { 64 | ego_ = other.ego_; 65 | agents_ = other.agents_; 66 | traffic_lattice_ = boost::make_shared(*(other.traffic_lattice_)); 67 | return *this; 68 | } 69 | 70 | const Vehicle& Snapshot::agent(const size_t id) const { 71 | std::unordered_map::const_iterator iter = agents_.find(id); 72 | if (iter == agents_.end()) { 73 | std::string error_msg = (boost::format( 74 | "Snapshot::agent(): " 75 | "the required agent %1% does not exist in the snapshot.\n") % id).str(); 76 | std::string snapshot_msg = this->string(); 77 | throw std::runtime_error(error_msg + snapshot_msg); 78 | } 79 | return iter->second; 80 | } 81 | 82 | Vehicle& Snapshot::agent(const size_t id) { 83 | std::unordered_map::iterator iter = agents_.find(id); 84 | if (iter == agents_.end()) { 85 | std::string error_msg = (boost::format( 86 | "Snapshot::agent(): " 87 | "the required agent %1% does not exist in the snapshot.\n") % id).str(); 88 | std::string snapshot_msg = this->string(); 89 | throw std::runtime_error(error_msg + snapshot_msg); 90 | } 91 | return iter->second; 92 | } 93 | 94 | const Vehicle& Snapshot::vehicle(const size_t id) const { 95 | if (id == ego_.id()) return ego_; 96 | else return agent(id); 97 | } 98 | 99 | Vehicle& Snapshot::vehicle(const size_t id) { 100 | if (id == ego_.id()) return ego_; 101 | else return agent(id); 102 | } 103 | 104 | bool Snapshot::updateTraffic( 105 | const std::vector>& updates) { 106 | 107 | // The tuple consists of the vehicle ID, transform, speed, acceleration, curvature. 108 | 109 | // Check the input vector has the updates for every vehicle in the snapshot. 110 | std::unordered_set updated_vehicles; 111 | for (const auto& update : updates) 112 | updated_vehicles.insert(std::get<0>(update)); 113 | 114 | // Update the transform, speed, and acceleration for all vehicles. 115 | for (const auto& update : updates) { 116 | size_t id; CarlaTransform update_transform; 117 | double update_speed; double update_acceleration; double update_curvature; 118 | std::tie(id, update_transform, update_speed, update_acceleration, update_curvature) = update; 119 | 120 | // Update the status for the ego vehicle. 121 | if (id == ego_.id()) { 122 | ego_.transform() = update_transform; 123 | ego_.speed() = update_speed; 124 | ego_.acceleration() = update_acceleration; 125 | ego_.curvature() = update_curvature; 126 | continue; 127 | } 128 | 129 | // Update the status for the agent vehicles. 130 | std::unordered_map::iterator agent_iter = agents_.find(id); 131 | // This vehicle is not in the snapshot. 132 | if (agent_iter == agents_.end()) continue; 133 | 134 | agent_iter->second.transform() = update_transform; 135 | agent_iter->second.speed() = update_speed; 136 | agent_iter->second.acceleration() = update_acceleration; 137 | agent_iter->second.curvature() = update_curvature; 138 | } 139 | 140 | // Update the traffic lattice. 141 | std::vector> vehicles; 142 | vehicles.push_back(ego_.tuple()); 143 | for (const auto& agent : agents_) 144 | vehicles.push_back(agent.second.tuple()); 145 | 146 | std::unordered_set disappear_vehicles; 147 | const bool no_collision = traffic_lattice_->moveTrafficForward(vehicles, disappear_vehicles); 148 | 149 | // Remove the \c disappear_vehicles from the snapshot. 150 | if (disappear_vehicles.count(ego_.id()) != 0) { 151 | std::string error_msg( 152 | "Snapshot::UpdateTraffic(): " 153 | "the ego vehicle is removed from the snapshot.\n"); 154 | std::string snapshot_msg = this->string(); 155 | throw std::runtime_error(error_msg + snapshot_msg); 156 | } 157 | 158 | for (const size_t disappear_vehicle : disappear_vehicles) 159 | agents_.erase(disappear_vehicle); 160 | 161 | return no_collision; 162 | } 163 | } // End namespace planner. 164 | -------------------------------------------------------------------------------- /src/planner/common/snapshot.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | namespace planner { 27 | 28 | /** 29 | * \brief Snapshot records the status of all vehicles within a micro traffic at 30 | * one time instance. 31 | * 32 | * The snapshot objects uses TrafficLattice to bookkeep the relative locations 33 | * of the vehicles. 34 | */ 35 | class Snapshot { 36 | 37 | protected: 38 | 39 | using CarlaMap = carla::client::Map; 40 | using CarlaVehicle = carla::client::Vehicle; 41 | using CarlaTransform = carla::geom::Transform; 42 | using CarlaBoundingBox = carla::geom::BoundingBox; 43 | 44 | protected: 45 | 46 | /// The ego vehicle. 47 | Vehicle ego_; 48 | 49 | /// Agents in the micro traffic, i.e. all vechiles other than the ego. 50 | //std::vector agents_; 51 | std::unordered_map agents_; 52 | 53 | /// Traffic lattice which is used to keep track of the relative 54 | /// location among the vehicles. 55 | boost::shared_ptr traffic_lattice_; 56 | 57 | public: 58 | 59 | Snapshot(const Vehicle& ego, 60 | const std::unordered_map& agents, 61 | const boost::shared_ptr& router, 62 | const boost::shared_ptr& map, 63 | const boost::shared_ptr& fast_map); 64 | 65 | Snapshot(const Snapshot& other); 66 | 67 | Snapshot& operator=(const Snapshot& other); 68 | 69 | const Vehicle& ego() const { return ego_; } 70 | Vehicle& ego() { return ego_; } 71 | 72 | const std::unordered_map& agents() const { return agents_; } 73 | std::unordered_map& agents() { return agents_; } 74 | 75 | const Vehicle& agent(const size_t id) const; 76 | Vehicle& agent(const size_t id); 77 | 78 | const Vehicle& vehicle(const size_t id) const; 79 | Vehicle& vehicle(const size_t id); 80 | 81 | const boost::shared_ptr 82 | trafficLattice() const { return traffic_lattice_; } 83 | const boost::shared_ptr 84 | trafficLattice() { return traffic_lattice_; } 85 | 86 | // The input \c new_transforms should cover every vehicle in the snapshot. 87 | // The tuple consists of the vehicle ID, transform, speed, acceleration, curvature. 88 | bool updateTraffic( 89 | const std::vector>& transforms); 90 | 91 | std::string string(const std::string& prefix = "") const { 92 | std::string output = prefix; 93 | output += ego_.string("ego "); 94 | for (const auto& agent : agents_) 95 | output += agent.second.string("agent "); 96 | output += "waypoint lattice range: " + std::to_string(traffic_lattice_->range()) + "\n"; 97 | return output; 98 | } 99 | 100 | }; 101 | } // End namespace planner. 102 | 103 | -------------------------------------------------------------------------------- /src/planner/common/traffic_simulator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | namespace planner { 30 | 31 | /** 32 | * \brief TrafficSimulator simulates the traffic forward by a given 33 | * period of time. 34 | * 35 | * All vehicles are assumed to have constant acceleration during the 36 | * simulation period. The path of the ego vehicle should be given as 37 | * a paremeter, while the rest of the agents are assumed to be lane 38 | * followers. 39 | * 40 | */ 41 | class TrafficSimulator : private boost::noncopyable { 42 | 43 | protected: 44 | 45 | using CarlaMap = carla::client::Map; 46 | using CarlaWaypoint = carla::client::Waypoint; 47 | using CarlaTransform = carla::geom::Transform; 48 | 49 | protected: 50 | 51 | /// The snapshot of the traffic scenario. 52 | Snapshot snapshot_; 53 | 54 | /// Router. 55 | boost::shared_ptr router_ = nullptr; 56 | 57 | /// Carla map. 58 | boost::shared_ptr map_ = nullptr; 59 | 60 | /// Fast waypoint map. 61 | boost::shared_ptr fast_map_ = nullptr; 62 | 63 | public: 64 | 65 | TrafficSimulator(const Snapshot& snapshot, 66 | const boost::shared_ptr& router, 67 | const boost::shared_ptr& map, 68 | const boost::shared_ptr& fast_map) : 69 | snapshot_(snapshot), 70 | router_(router), 71 | map_(map), 72 | fast_map_(fast_map) {} 73 | 74 | TrafficSimulator(const Snapshot& snapshot, 75 | const boost::shared_ptr& map, 76 | const boost::shared_ptr& fast_map) : 77 | snapshot_(snapshot), 78 | router_(boost::make_shared()), 79 | map_(map), 80 | fast_map_(fast_map) {} 81 | 82 | const Snapshot& snapshot() const { return snapshot_; } 83 | 84 | const boost::shared_ptr router() const { return router_; } 85 | boost::shared_ptr& router() { return router_; } 86 | 87 | /** 88 | * \brief Simulate the traffic. 89 | * 90 | * The simulation stops if either of the following two conditions is met: 91 | * -1 The maximum duration \c max_time is reached. 92 | * -2 The ego vehicle reaches the end of the path. 93 | * The actual terminate cause can be determined by the returned \c time. 94 | * 95 | * In the case that the function returns false, i.e. collision is detected, 96 | * the output \c time and \c cost are invalid. Once the function returns false, 97 | * the object should not be used anymore. 98 | * 99 | * \param[in] path The path to be executed by the ego vehicle. 100 | * \param[in] default_dt The default simulation time step. 101 | * \param[in] max_time The maximum duration to simulate. 102 | * \param[out] time The actual simulation duration. 103 | * \param[out] cost The accumulated cost during the simulation. 104 | * \return false If collision detected during the simulation. 105 | */ 106 | virtual const bool simulate( 107 | const ContinuousPath& path, const double default_dt, const double max_time, 108 | double& time, double& cost); 109 | 110 | protected: 111 | 112 | /// Compute the acceleration of the ego vehicle given the current traffic scenario. 113 | virtual const double egoAcceleration() const = 0; 114 | 115 | /// Compute the acceleration of the agent vehicle given the current traffic scenario. 116 | virtual const double agentAcceleration(const size_t agent) const = 0; 117 | 118 | virtual const std::tuple 119 | updatedAgentTuple(const size_t id, const double accel, const double dt) const; 120 | 121 | /// Compute the ttc cost based on the input ttc. 122 | virtual const double ttcCost(const double ttc) const; 123 | 124 | /// Compute the ttc cost based on the ttc of the ego vehicle in the snapshot. 125 | virtual const double ttcCost() const; 126 | 127 | /// Compute the accel cost based on the input accel. 128 | virtual const double accelCost(const double accel) const; 129 | 130 | /// Compute the accel cost based on the accel of the vehicles in the snapshot. 131 | virtual const double accelCost() const; 132 | 133 | /** 134 | * \brief This function is used to determine how much longer a vehicle can travel. 135 | * 136 | * The remaining time depends on two factors: 137 | * - 1. When will the speed decelerate to 0. 138 | * - 2. When will the vehicle travel the given distance. 139 | * 140 | * The output of this function should be compared with the default simulation 141 | * time resolution. Whichever is smaller should be used as the actual simulation 142 | * time step. 143 | * 144 | * \param[in] speed The current speed. 145 | * \param[in] accel The current acceleration. 146 | * \param[in] distance The maximum distance to travel. 147 | * \return The remaining time the vehicle can still travel. 148 | */ 149 | virtual const double remainingTime( 150 | const double speed, const double accel, const double distance) const; 151 | 152 | 153 | }; // End class TrafficSimulator. 154 | 155 | } // End namespace planner. 156 | 157 | -------------------------------------------------------------------------------- /src/planner/common/utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace utils { 25 | 26 | void convertLocationInPlace(carla::geom::Location& in) { 27 | in.y = -in.y; 28 | return; 29 | } 30 | 31 | carla::geom::Location convertLocation(const carla::geom::Location& in) { 32 | carla::geom::Location out = in; 33 | convertLocationInPlace(out); 34 | return out; 35 | } 36 | 37 | void convertRotationInPlace(carla::geom::Rotation& in) { 38 | in.roll = -in.roll; 39 | in.yaw = -in.yaw; 40 | return; 41 | } 42 | 43 | carla::geom::Rotation convertRotation(const carla::geom::Rotation& in) { 44 | carla::geom::Rotation out = in; 45 | convertRotationInPlace(out); 46 | return out; 47 | } 48 | 49 | void convertTransformInPlace(carla::geom::Transform& in) { 50 | convertRotationInPlace(in.rotation); 51 | convertLocationInPlace(in.location); 52 | return; 53 | } 54 | 55 | carla::geom::Transform convertTransform(const carla::geom::Transform& in) { 56 | carla::geom::Transform out = in; 57 | convertTransformInPlace(out); 58 | return out; 59 | } 60 | 61 | const double curvatureAtWaypoint( 62 | const boost::shared_ptr& waypoint, 63 | const boost::shared_ptr& map) { 64 | // Get the road. 65 | const carla::road::Road& road = 66 | map->GetMap().GetMap().GetRoad(waypoint->GetRoadId()); 67 | 68 | // Get the road geometry info. 69 | const carla::road::element::RoadInfoGeometry* road_info = 70 | road.GetInfo(waypoint->GetDistance()); 71 | 72 | // Get the actual geometry of the road. 73 | const carla::road::element::Geometry& geometry = road_info->GetGeometry(); 74 | 75 | // Get the curvature. 76 | double curvature = 0.0; 77 | 78 | if (geometry.GetType() == carla::road::element::GeometryType::LINE) { 79 | curvature = 0.0; 80 | 81 | } else if (geometry.GetType() == carla::road::element::GeometryType::ARC) { 82 | const carla::road::element::GeometryArc& geometry_arc = 83 | dynamic_cast(geometry); 84 | curvature = geometry_arc.GetCurvature(); 85 | 86 | } else if (geometry.GetType() == carla::road::element::GeometryType::SPIRAL) { 87 | //FIXME: Not sure how to deal with this. 88 | // But there is no example for this road type from Town01 to Town07. 89 | std::string error_msg( 90 | "curvatureAtWaypoint(): " 91 | "cannot get curvature for waypointa on spiral roads.\n"); 92 | boost::format format( 93 | "waypoint %1% x:%2% y:%3% z:%4% r:%5% p:%6% y:%7% road:%8% lane:%9%\n"); 94 | format % waypoint->GetId() 95 | % waypoint->GetTransform().location.x 96 | % waypoint->GetTransform().location.y 97 | % waypoint->GetTransform().location.z 98 | % waypoint->GetTransform().rotation.roll 99 | % waypoint->GetTransform().rotation.pitch 100 | % waypoint->GetTransform().rotation.yaw 101 | % waypoint->GetRoadId() 102 | % waypoint->GetLaneId(); 103 | 104 | throw std::runtime_error(error_msg+format.str()); 105 | } else { 106 | } 107 | 108 | // Fix the curvature based on the sign of the lane ID. 109 | if (waypoint->GetLaneId() >= 0) return curvature; 110 | else return -curvature; 111 | } 112 | 113 | const double unrollAngle(double angle) { 114 | angle = std::remainder(angle, 360.0); 115 | if (angle < 0.0) angle += 360.0; 116 | return angle; 117 | } 118 | 119 | const double shortestAngle(double angle1, double angle2) { 120 | angle1 = unrollAngle(angle1); 121 | angle2 = unrollAngle(angle2); 122 | 123 | double diff = angle1 - angle2; 124 | if (std::abs(diff+360.0) < std::abs(diff)) diff += 360.0; 125 | if (std::abs(diff-360.0) < std::abs(diff)) diff -= 360.0; 126 | 127 | return diff; 128 | } 129 | 130 | const double distanceToLaneCenter( 131 | const carla::geom::Location& location, 132 | const boost::shared_ptr& waypoint) { 133 | 134 | // Compute the location difference. 135 | const carla::geom::Transform waypoint_transform = waypoint->GetTransform(); 136 | const carla::geom::Vector3D diff = location - waypoint_transform.location; 137 | 138 | // Compute the unit vector in the lateral direction. 139 | const double angle = (waypoint_transform.rotation.yaw+90.0)/180.0*M_PI; 140 | const carla::geom::Vector3D lateral_unit(std::cos(angle), std::sin(angle), 0.0); 141 | 142 | return diff.x*lateral_unit.x + diff.y*lateral_unit.y; 143 | } 144 | 145 | } // End namespace utils. 146 | -------------------------------------------------------------------------------- /src/planner/common/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace utils { 25 | 26 | /** 27 | * \defgroup Hashing Functions 28 | * 29 | * Hash various hashable values into a single hash value. 30 | * 31 | * @{ 32 | */ 33 | template 34 | void hashCombine(size_t& seed, T val) { 35 | boost::hash_combine(seed, val); 36 | return; 37 | } 38 | 39 | template 40 | void hashCombine(size_t& seed, T val, Args... args) { 41 | boost::hash_combine(seed, val); 42 | hashCombine(seed, args...); 43 | return; 44 | } 45 | /**@}*/ 46 | 47 | /** 48 | * \defgroup Left and Right Hand Cooordinate System Conversion 49 | * 50 | * Carla uses left hand coordinate systems. However, right hand coordinate 51 | * system is preferred in robotics (e.g. ROS). The incompatiblity can cause 52 | * great confusion leading to software bugs that are hard to trace. Therefore, 53 | * consider using the following functions before using a coordinate. 54 | * 55 | * Other than \c carla::geom::Transform uses left hand coordinate systems, 56 | * some are harder to recognize, like \c carla::client::Waypoint::GetLeft(), 57 | * which actually returns a right waypoint in the right hand coordinate systems. 58 | * 59 | * The conversion is done by flipping the positive y-axis. Therefore the y 60 | * coordinate is inverted in translation. The roll and yaw are inverted in 61 | * euler angles. 62 | * 63 | * @{ 64 | */ 65 | void convertLocationInPlace(carla::geom::Location& in); 66 | 67 | carla::geom::Location convertLocation(const carla::geom::Location& in); 68 | 69 | void convertRotationInPlace(carla::geom::Rotation& in); 70 | 71 | carla::geom::Rotation convertRotation(const carla::geom::Rotation& in); 72 | 73 | void convertTransformInPlace(carla::geom::Transform& in); 74 | 75 | carla::geom::Transform convertTransform(const carla::geom::Transform& in); 76 | /**@}*/ 77 | 78 | const double curvatureAtWaypoint( 79 | const boost::shared_ptr& waypoint, 80 | const boost::shared_ptr& map); 81 | 82 | const double unrollAngle(double angle); 83 | 84 | const double shortestAngle(double angle1, double angle2); 85 | 86 | /** 87 | * \brief Compute the distance from the given location to the lane center. 88 | * \param[in] location The query location. 89 | * \param[in] waypoint The waypoint obtained by projecting the given location 90 | * to the closest lane. 91 | * \return The distance of the query location to the lane center. If the returned 92 | * value is less than 0.0, the location is at the left of the lane. If 93 | * the returned value is greater than 0.0, the location is on the right. 94 | */ 95 | const double distanceToLaneCenter( 96 | const carla::geom::Location& location, 97 | const boost::shared_ptr& waypoint); 98 | 99 | } // End namespace utils. 100 | -------------------------------------------------------------------------------- /src/planner/common/vehicle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace planner { 23 | 24 | /** 25 | * \brief Vehicle keeps tracks the information of a vehicle during the planning. 26 | */ 27 | class Vehicle { 28 | 29 | protected: 30 | 31 | using CarlaBoundingBox = carla::geom::BoundingBox; 32 | using CarlaTransform = carla::geom::Transform; 33 | using CarlaVehicle = carla::client::Vehicle; 34 | 35 | protected: 36 | 37 | /// ID of the vehicle in the carla simulator. 38 | size_t id_; 39 | 40 | /// Bounding box of the vehicle. 41 | CarlaBoundingBox bounding_box_; 42 | 43 | /// Transform of the vehicle, left handed to be compatible with the carla simualtor. 44 | CarlaTransform transform_; 45 | 46 | /// Speed of the vehicle. 47 | double speed_ = 0.0; 48 | 49 | /// The policy speed of this vehicle. 50 | double policy_speed_ = 0.0; 51 | 52 | /// Acceleration of the vehicle, brake should be negative. 53 | double acceleration_ = 0.0; 54 | 55 | /// Curvature of the path where the vehicle is currently at. 56 | /// Compatible with carla. 57 | double curvature_ = 0.0; 58 | 59 | public: 60 | 61 | Vehicle() = default; 62 | 63 | /** 64 | * The \c acceleration_ of the object won't be filled in using \c CarlaVehicle's 65 | * \c GetAcceleration() API, since the vehicles are assumed to be teleported 66 | * instead of controlled through physical dynamics. 67 | */ 68 | //Vehicle(const boost::shared_ptr& actor, 69 | // const double policy_speed, 70 | // const double curvature) : 71 | // id_ (actor->GetId()), 72 | // bounding_box_(actor->GetBoundingBox()), 73 | // transform_ (actor->GetTransform()), 74 | // speed_ (actor->GetVelocity().Length()), 75 | // policy_speed_(policy_speed), 76 | // acceleration_(0.0), 77 | // curvature_(curvature) {} 78 | 79 | Vehicle(const boost::shared_ptr& actor, 80 | const double speed, 81 | const double policy_speed, 82 | const double curvature) : 83 | id_ (actor->GetId()), 84 | bounding_box_(actor->GetBoundingBox()), 85 | transform_ (actor->GetTransform()), 86 | speed_ (speed), 87 | policy_speed_(policy_speed), 88 | acceleration_(0.0), 89 | curvature_(curvature) {} 90 | 91 | Vehicle(const size_t id, 92 | const CarlaBoundingBox& bounding_box, 93 | const CarlaTransform& transform, 94 | const double speed, 95 | const double policy_speed, 96 | const double acceleration, 97 | const double curvature) : 98 | id_ (id), 99 | bounding_box_(bounding_box), 100 | transform_ (transform), 101 | speed_ (speed), 102 | policy_speed_(policy_speed), 103 | acceleration_(acceleration), 104 | curvature_(curvature){} 105 | 106 | const size_t id() const { return id_; } 107 | size_t& id() { return id_; } 108 | 109 | const CarlaBoundingBox boundingBox() const { return bounding_box_; } 110 | CarlaBoundingBox& boundingBox() { return bounding_box_; } 111 | 112 | const CarlaTransform transform() const { return transform_; } 113 | CarlaTransform& transform() { return transform_; } 114 | 115 | const double speed() const { return speed_; } 116 | double& speed() { return speed_; } 117 | 118 | const double policySpeed() const { return policy_speed_; } 119 | double& policySpeed() { return policy_speed_; } 120 | 121 | const double acceleration() const { return acceleration_; } 122 | double& acceleration() { return acceleration_; } 123 | 124 | const double curvature() const { return curvature_; } 125 | double& curvature() { return curvature_; } 126 | 127 | /** 128 | * \brief Update the vehicle in the simulator server. 129 | * 130 | * The function throws runtime error if the ID of input \c actor does not 131 | * match the ID of the object. 132 | * 133 | * The acceleration of the \c actor won't be set. 134 | * 135 | * \param[in] actor The vehicle to be updated. 136 | */ 137 | void updateCarlaVehicle(const boost::shared_ptr& actor) const { 138 | if (actor->GetId() != id_) { 139 | std::string error_msg( 140 | "Vehicle::updateCarlaVehicle(): " 141 | "failed to update the vehicle in the simulatior because of mismatched id.\n"); 142 | std::string id_msg = (boost::format( 143 | "Vehicle ID:%1% Actor ID:%2%\n") 144 | % id_ % actor->GetId()).str(); 145 | throw std::runtime_error(error_msg + id_msg); 146 | } 147 | actor->SetTransform(transform_); 148 | actor->SetVelocity(transform_.GetForwardVector()*speed_); 149 | // No need to set the acceleration of the vehicle. 150 | return; 151 | } 152 | 153 | /** 154 | * \brief Get the vehicle ID, transform, and bounding box as a tuple. 155 | */ 156 | std::tuple tuple() const { 157 | return std::make_tuple(id_, transform_, bounding_box_); 158 | } 159 | 160 | /** 161 | * \brief Get a string containing the information of this vehicle. 162 | */ 163 | std::string string(const std::string& prefix = "") const { 164 | 165 | std::string output = prefix; 166 | boost::format vehicle_format( 167 | "id:%1% x:%2% y:%3% z:%4% policy:%5% speed:%6% accel:%7% curvature:%8%\n"); 168 | 169 | vehicle_format % id_; 170 | vehicle_format % transform_.location.x; 171 | vehicle_format % transform_.location.y; 172 | vehicle_format % transform_.location.z; 173 | vehicle_format % policy_speed_; 174 | vehicle_format % speed_; 175 | vehicle_format % acceleration_; 176 | vehicle_format % curvature_; 177 | 178 | output += vehicle_format.str(); 179 | 180 | return output; 181 | } 182 | 183 | }; // End class Vehicle. 184 | } // End namespace planner. 185 | -------------------------------------------------------------------------------- /src/planner/common/vehicle_path.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | namespace planner { 25 | 26 | class ContinuousPath; 27 | class DiscretePath; 28 | 29 | class VehiclePath { 30 | 31 | public: 32 | 33 | enum LaneChangeType { 34 | KeepLane = 0, 35 | LeftLaneChange = 1, 36 | RightLaneChange = 2, 37 | None = 3 38 | }; 39 | 40 | protected: 41 | 42 | using CarlaTransform = carla::geom::Transform; 43 | 44 | protected: 45 | 46 | LaneChangeType lane_change_type_ = LaneChangeType::None; 47 | 48 | public: 49 | 50 | VehiclePath() = default; 51 | 52 | VehiclePath(const LaneChangeType lane_change_type) : 53 | lane_change_type_(lane_change_type) {} 54 | 55 | virtual ~VehiclePath() {} 56 | 57 | /// Get the carla compatible left-handed transform at the start of the path. 58 | virtual const std::pair 59 | startTransform() const = 0; 60 | 61 | /// Get the carla compatible left-handed transform at the end of the path. 62 | virtual const std::pair 63 | endTransform() const = 0; 64 | 65 | /// Return the range of the path. 66 | virtual const double range() const = 0; 67 | 68 | /// Return the lane change type of the path. 69 | virtual const LaneChangeType laneChangeType() const { 70 | return lane_change_type_; 71 | } 72 | 73 | /// Get the carla compatible left-handed transform and curvature at s, 74 | /// i.e. distance from the start of the path. 75 | virtual const std::pair transformAt(const double s) const = 0; 76 | 77 | /// Returns samples on the path from the start to the end with 0.1m resolution. 78 | virtual const std::vector> samples() const; 79 | 80 | protected: 81 | 82 | NonHolonomicPath::State carlaTransformToPathState( 83 | const std::pair& transform) const; 84 | 85 | std::pair pathStateToCarlaTransform( 86 | const NonHolonomicPath::State& state, 87 | const CarlaTransform& base_transform) const; 88 | 89 | std::pair interpolateTransform( 90 | const std::pair& t1, 91 | const std::pair& t2, 92 | const double w) const; 93 | 94 | }; // End class VehiclePath. 95 | 96 | class ContinuousPath : public VehiclePath { 97 | 98 | private: 99 | 100 | using Base = VehiclePath; 101 | using This = ContinuousPath; 102 | 103 | protected: 104 | 105 | std::pair start_; 106 | std::pair end_; 107 | 108 | NonHolonomicPath path_; 109 | 110 | public: 111 | 112 | ContinuousPath(const std::pair& start, 113 | const std::pair& end, 114 | const LaneChangeType& lane_change_type); 115 | 116 | ContinuousPath(const DiscretePath& discrete_path); 117 | 118 | virtual ~ContinuousPath() {} 119 | 120 | virtual const std::pair 121 | startTransform() const override { return start_; } 122 | 123 | virtual const std::pair 124 | endTransform() const override { return end_; } 125 | 126 | virtual const double range() const override { return path_.sf; } 127 | 128 | virtual const std::pair 129 | transformAt(const double s) const override; 130 | 131 | std::string string(const std::string& prefix="") const; 132 | 133 | }; // End class ContinuousPath. 134 | 135 | /** 136 | * TODO: Complete the implementation for this class later. 137 | */ 138 | class DiscretePath : public VehiclePath { 139 | 140 | private: 141 | 142 | using Base = VehiclePath; 143 | using This = DiscretePath; 144 | 145 | protected: 146 | 147 | /// Stores the samples on path.. 148 | std::map> samples_; 149 | 150 | double resolution_ = 0.5; 151 | 152 | public: 153 | 154 | DiscretePath(const std::pair& start, 155 | const std::pair& end, 156 | const LaneChangeType& lane_change_type); 157 | 158 | DiscretePath(const ContinuousPath& continuous_path); 159 | 160 | virtual ~DiscretePath() {} 161 | 162 | virtual const std::pair 163 | startTransform() const override { 164 | return samples_.begin()->second; 165 | } 166 | 167 | virtual const std::pair 168 | endTransform() const override { 169 | return samples_.rbegin()->second; 170 | } 171 | 172 | virtual const double range() const override { 173 | return samples_.rbegin()->first; 174 | } 175 | 176 | virtual const std::pair 177 | transformAt(const double s) const override; 178 | 179 | //virtual const std::vector> 180 | // samples() const override; 181 | 182 | virtual void append(const DiscretePath& path); 183 | 184 | virtual void append(const ContinuousPath& path) { 185 | DiscretePath discrete_path(path); 186 | append(discrete_path); 187 | return; 188 | } 189 | 190 | std::string string(const std::string& prefix="") const; 191 | 192 | }; // End class DiscretePath. 193 | 194 | } // End namespace planner. 195 | 196 | -------------------------------------------------------------------------------- /src/planner/common/vehicle_path_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace planner { 25 | 26 | /** 27 | * \brief VehiclePathPlanner is supposed to be the base class of all path planner classes. 28 | */ 29 | class VehiclePathPlanner { 30 | 31 | protected: 32 | 33 | using CarlaMap = carla::client::Map; 34 | 35 | protected: 36 | 37 | /// Carla map object. 38 | boost::shared_ptr map_ = nullptr; 39 | 40 | /// Fast waypoint map. 41 | boost::shared_ptr fast_map_ = nullptr; 42 | 43 | public: 44 | 45 | /** 46 | * \brief Class constructor. 47 | * \param[in] map The carla map pointer. 48 | * \param[in] fast_map The fast map used to retrieve waypoints based on locations. 49 | */ 50 | VehiclePathPlanner(const boost::shared_ptr& map, 51 | const boost::shared_ptr& fast_map) : 52 | map_(map), fast_map_(fast_map) {} 53 | 54 | /// Class destructor. 55 | virtual ~VehiclePathPlanner() {} 56 | 57 | /// Get the carla map pointer. 58 | const boost::shared_ptr map() const { return map_; } 59 | 60 | /// Get or set the carla map pointer. 61 | boost::shared_ptr& map() { return map_; } 62 | 63 | /// Get the fast waypoint map. 64 | const boost::shared_ptr 65 | fastWaypointMap() const { return fast_map_; } 66 | 67 | /// Get or set the fast waypoint map. 68 | boost::shared_ptr& 69 | fastWaypointMap() { return fast_map_; } 70 | 71 | /** 72 | * \brief The main interface of the path planner. 73 | * 74 | * \param[in] target The ID of the target vehicle. 75 | * \param[in] snapshot Snapshot of the current traffic scenario. 76 | * \return The planned path. 77 | */ 78 | virtual DiscretePath planPath(const size_t target, const Snapshot& snapshot) = 0; 79 | 80 | /** 81 | * \brief The main interface of the path planner. 82 | * 83 | * \param[in] target The ID of the target vehicle. 84 | * \param[in] snapshot Snapshot of the current traffic scenario. 85 | * \param[out] path The planned path. 86 | */ 87 | virtual void planPath(const size_t target, const Snapshot& snapshot, DiscretePath& path) { 88 | path = planPath(target, snapshot); 89 | return; 90 | } 91 | }; 92 | 93 | } // End namespace planner. 94 | -------------------------------------------------------------------------------- /src/planner/common/vehicle_speed_planner.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | namespace planner { 24 | 25 | class VehicleSpeedPlanner : private boost::noncopyable { 26 | 27 | protected: 28 | 29 | boost::shared_ptr intelligent_driver_model_; 30 | 31 | public: 32 | 33 | /// Default constructor. 34 | VehicleSpeedPlanner() : 35 | intelligent_driver_model_(boost::make_shared()) {} 36 | 37 | /** 38 | * \brief Class constructor. 39 | * \param[in] model The intelligent driver model to be used to compute acceleration. 40 | */ 41 | VehicleSpeedPlanner(const boost::shared_ptr& model) : 42 | intelligent_driver_model_(model) {} 43 | 44 | virtual ~VehicleSpeedPlanner() {} 45 | 46 | /// Get the intelligent driver model used in the object. 47 | const boost::shared_ptr intelligentDriverModel() const { 48 | return intelligent_driver_model_; 49 | } 50 | 51 | /// Get or set the intelligent driver model used in the object. 52 | boost::shared_ptr& intelligentDriverModel() { 53 | return intelligent_driver_model_; 54 | } 55 | 56 | /** 57 | * \brief The main interface of the speed planner. 58 | * 59 | * \param[in] target The ID of the target vehicle. 60 | * \param[in] snapshot Snapshot of the current traffic scenario. 61 | * \return The acceleration to be applied for the target vehicle. 62 | */ 63 | virtual const double planSpeed(const size_t target, const Snapshot& snapshot) { 64 | // Get the target vehicle. 65 | const Vehicle target_vehicle = snapshot.vehicle(target); 66 | 67 | // Get the lead vehicle of the target. 68 | boost::optional> lead = 69 | snapshot.trafficLattice()->front(target_vehicle.id()); 70 | 71 | // Compute the acceleration to be applied by the target vehicle. 72 | if (lead) { 73 | return intelligent_driver_model_->idm( 74 | target_vehicle.speed(), 75 | target_vehicle.policySpeed(), 76 | snapshot.vehicle(lead->first).speed(), 77 | lead->second); 78 | } else { 79 | return intelligent_driver_model_->idm( 80 | target_vehicle.speed(), 81 | target_vehicle.policySpeed()); 82 | } 83 | } 84 | 85 | /** 86 | * \brief The main interface of the speed planner. 87 | * 88 | * \param[in] target The ID of the target vehicle. 89 | * \param[in] snapshot Snapshot of the current traffic scenario. 90 | * \param[out] The acceleration to be applied for the target vehicle. 91 | */ 92 | virtual void planSpeed(const size_t target, const Snapshot& snapshot, double& accel) { 93 | accel = planSpeed(target, snapshot); 94 | return; 95 | } 96 | 97 | }; // End class VehicleSpeedPlanner. 98 | 99 | } // End namespace planner. 100 | 101 | -------------------------------------------------------------------------------- /src/planner/common/waypoint_lattice.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | namespace planner { 25 | 26 | /** 27 | * \brief WaypointNode Keeps track of the waypoints on a lattice. 28 | * 29 | * WaypointNode is used together with Lattice class template to 30 | * form the WaypointLattice class template. 31 | * 32 | * \note The copy constructor of this class performs a shallow copy, 33 | * i.e. only the shared and weak pointers are copied. This makes 34 | * sense since the class copy constructor won't know which piece 35 | * of memory the pointers should point to. In case one would like 36 | * to redirect the pointers in the class to other objects, use 37 | * the accessor interfaces. 38 | */ 39 | class WaypointNode : public LatticeNode { 40 | 41 | protected: 42 | 43 | using CarlaMap = carla::client::Map; 44 | using CarlaWaypoint = carla::client::Waypoint; 45 | 46 | using Base = LatticeNode; 47 | using This = WaypointNode; 48 | 49 | public: 50 | 51 | /// Default constructor. 52 | WaypointNode() = default; 53 | 54 | /** 55 | * \brief Construct a node with a carla waypoint. 56 | * \param[in] waypoint A carla waypoint at which a node should be created. 57 | */ 58 | WaypointNode(const boost::shared_ptr& waypoint) : 59 | Base(waypoint) {} 60 | 61 | // Get the string describing the node. 62 | std::string string(const std::string& prefix="") const { 63 | boost::format waypoint_format( 64 | "waypoint %1% x:%2% y:%3% z:%4% r:%5% p:%6% y:%7% road:%8% lane:%9%.\n"); 65 | std::string waypoint_msg = (waypoint_format 66 | % this->waypoint_->GetId() 67 | % this->waypoint_->GetTransform().location.x 68 | % this->waypoint_->GetTransform().location.y 69 | % this->waypoint_->GetTransform().location.z 70 | % this->waypoint_->GetTransform().rotation.roll 71 | % this->waypoint_->GetTransform().rotation.pitch 72 | % this->waypoint_->GetTransform().rotation.yaw 73 | % this->waypoint_->GetRoadId() 74 | % this->waypoint_->GetLaneId()).str(); 75 | std::string distance_msg = (boost::format("node distance: %1%\n") % this->distance_).str(); 76 | return prefix + waypoint_msg + distance_msg; 77 | // TODO: Add the info for neighbor waypoints as well. 78 | } 79 | }; // End class WaypointNode. 80 | 81 | /** 82 | * \brief WaypointLattice is a helper class used to query 83 | * the relative waypoints of the given waypoint. 84 | */ 85 | using WaypointLattice = Lattice; 86 | } // End namespace planner. 87 | -------------------------------------------------------------------------------- /src/planner/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | catkin_add_gtest(test_idm 2 | test_intelligent_driver_model.cpp 3 | ) 4 | -------------------------------------------------------------------------------- /src/planner/tests/intelligent_driver_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import print_function 4 | from __future__ import division 5 | from math import sqrt 6 | from math import tanh 7 | 8 | time_gap = 1.0 9 | distance_gap = 6.0 10 | 11 | accel_exp = 4.0 12 | 13 | comfort_accel = 1.5 14 | comfort_decel = 2.5 15 | 16 | max_accel = 5.0 17 | max_decel = 8.0 18 | 19 | coolness_factor = 0.9 20 | 21 | def saturate_accel(accel): 22 | 23 | if accel > max_accel: 24 | accel = max_accel 25 | if accel < -max_decel: 26 | accel = -max_decel 27 | 28 | return accel 29 | 30 | def desired_distance(ego_v, lead_v): 31 | v_diff = ego_v - lead_v 32 | return distance_gap + max( 33 | 0.0, 34 | ego_v*time_gap + ego_v*v_diff / (2.0*sqrt(comfort_accel*comfort_decel))) 35 | 36 | def intelligent_driver_model(ego_v, ego_v0, lead_v=None, s=None): 37 | 38 | if (lead_v is None) or (s is None): 39 | accel_out = comfort_accel * (1.0 - (ego_v/ego_v0)**accel_exp) 40 | else: 41 | accel_out = comfort_accel * (1.0 - 42 | (ego_v/ego_v0)**accel_exp - 43 | (desired_distance(ego_v, lead_v)/s)**2.0) 44 | 45 | return saturate_accel(accel_out) 46 | 47 | def free_accel(ego_v, ego_v0): 48 | 49 | if ego_v <= ego_v0: 50 | return comfort_accel * ( 51 | 1.0-(ego_v/ego_v0)**accel_exp) 52 | else: 53 | return -comfort_decel * ( 54 | 1.0-(ego_v0/ego_v)**(comfort_accel*accel_exp/comfort_decel)) 55 | 56 | def improved_intelligent_driver_model(ego_v, ego_v0, lead_v=None, s=None): 57 | 58 | accel_free = free_accel(ego_v, ego_v0) 59 | 60 | if (lead_v is None) or (s is None): 61 | return accel_free 62 | 63 | z = desired_distance(ego_v, lead_v) / s 64 | 65 | if ego_v<=ego_v0 and z>=1: 66 | accel_out = comfort_accel * (1.0-z**2) 67 | elif ego_v<=ego_v0 and z<1: 68 | accel_out = accel_free * (1.0-z**(2.0*comfort_accel/accel_free)) 69 | elif ego_v>ego_v0 and z>=1: 70 | accel_out = accel_free + comfort_accel*(1-z**2.0) 71 | else: 72 | accel_out = accel_free 73 | 74 | return saturate_accel(accel_out) 75 | 76 | def const_accel_heuristic(ego_v, lead_v, lead_v_dot, s): 77 | 78 | accel_tilde = min(comfort_accel, lead_v_dot) 79 | 80 | if lead_v*(ego_v-lead_v) < -2.0*s*accel_tilde: 81 | return (ego_v**2.0 * accel_tilde) / ( 82 | lead_v**2.0 - 2.0*s*accel_tilde) 83 | else: 84 | Theta = lambda x : 1 if x>=0 else 0 85 | return accel_tilde - (ego_v-lead_v)**2.0*Theta(ego_v-lead_v) / (2.0*s) 86 | 87 | def adaptive_cruise_control(ego_v, ego_v0, lead_v_dot=0.0, lead_v=None, s=None): 88 | 89 | accel_iidm = improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s) 90 | 91 | if (lead_v is None) or (s is None): 92 | return accel_iidm 93 | 94 | accel_cah = const_accel_heuristic(ego_v, lead_v, lead_v_dot, s) 95 | 96 | if accel_iidm >= accel_cah: 97 | accel_out = accel_iidm 98 | else: 99 | accel_out = (1-coolness_factor) * accel_iidm + coolness_factor * ( 100 | accel_cah + comfort_decel*tanh((accel_iidm-accel_cah)/comfort_decel)) 101 | return saturate_accel(accel_out) 102 | 103 | def main(): 104 | 105 | ego_v = 10 106 | ego_v0 = 29 107 | 108 | print("ego v: ", ego_v, "ego v0: ", ego_v0) 109 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0)) 110 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0)) 111 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0)) 112 | print("") 113 | 114 | ego_v = 40 115 | ego_v0 = 29 116 | 117 | print("ego v: ", ego_v, "ego v0: ", ego_v0) 118 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0)) 119 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0)) 120 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0)) 121 | print("") 122 | 123 | ego_v = 20 124 | ego_v0 = 29 125 | lead_v = 29 126 | s = 50 127 | print("ego v: ", ego_v, "ego v0: ", ego_v0, "lead v: ", lead_v, "s: ", s) 128 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 129 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 130 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0, 0.0, lead_v, s)) 131 | print("") 132 | 133 | ego_v = 25 134 | ego_v0 = 29 135 | lead_v = 15 136 | s = 50 137 | print("ego v: ", ego_v, "ego v0: ", ego_v0, "lead v: ", lead_v, "s: ", s) 138 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 139 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 140 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0, 0.0, lead_v, s)) 141 | print("") 142 | 143 | ego_v = 29 144 | ego_v0 = 29 145 | lead_v = 0 146 | s = 100 147 | print("ego v: ", ego_v, "ego v0: ", ego_v0, "lead v: ", lead_v, "s: ", s) 148 | print("IDM: ", intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 149 | print("IIDM: ", improved_intelligent_driver_model(ego_v, ego_v0, lead_v, s)) 150 | print("ACC: ", adaptive_cruise_control(ego_v, ego_v0, 0.0, lead_v, s)) 151 | print("") 152 | 153 | if __name__ == '__main__': 154 | main() 155 | 156 | 157 | -------------------------------------------------------------------------------- /src/planner/tests/test_intelligent_driver_model.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | using namespace planner; 19 | 20 | TEST(BaseIntelligentDriverModel, accessors) { 21 | IntelligentDriverModel idm; 22 | 23 | idm.timeGap() = 2.0; 24 | EXPECT_DOUBLE_EQ(idm.timeGap(), 2.0); 25 | 26 | idm.distanceGap() = 10.0; 27 | EXPECT_DOUBLE_EQ(idm.distanceGap(), 10.0); 28 | 29 | idm.accelExp() = 5.0; 30 | EXPECT_DOUBLE_EQ(idm.accelExp(), 5.0); 31 | 32 | idm.comfortAccel() = 2.0; 33 | EXPECT_DOUBLE_EQ(idm.comfortAccel(), 2.0); 34 | 35 | idm.comfortDecel() = 2.0; 36 | EXPECT_DOUBLE_EQ(idm.comfortDecel(), 2.0); 37 | 38 | } 39 | 40 | TEST(BasicIntelligentDriverModel, idm) { 41 | BasicIntelligentDriverModel idm; 42 | 43 | { 44 | const double ego_v = 10.0; 45 | const double ego_v0 = 29.0; 46 | const double accel = idm.idm(ego_v, ego_v0); 47 | EXPECT_NEAR(accel, 1.47879202184, 1e-3); 48 | } 49 | 50 | { 51 | const double ego_v = 40.0; 52 | const double ego_v0 = 29.0; 53 | const double accel = idm.idm(ego_v, ego_v0); 54 | EXPECT_NEAR(accel, -3.9292424086, 1e-3); 55 | } 56 | 57 | { 58 | const double ego_v = 20.0; 59 | const double ego_v0 = 29.0; 60 | const double lead_v = 29.0; 61 | const double s = 50.0; 62 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 63 | EXPECT_NEAR(accel, 1.13907234946, 1e-3); 64 | } 65 | 66 | { 67 | const double ego_v = 25.0; 68 | const double ego_v0 = 29.0; 69 | const double lead_v = 15.0; 70 | const double s = 50.0; 71 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 72 | EXPECT_NEAR(accel, -4.80628632147, 1e-3); 73 | } 74 | 75 | { 76 | const double ego_v = 29.0; 77 | const double ego_v0 = 29.0; 78 | const double lead_v = 0.0; 79 | const double s = 100.0; 80 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 81 | EXPECT_NEAR(accel, -8.0, 1e-3); 82 | } 83 | } 84 | 85 | TEST(ImprovedIntelligentDriverModel, idm) { 86 | ImprovedIntelligentDriverModel idm; 87 | 88 | { 89 | const double ego_v = 10.0; 90 | const double ego_v0 = 29.0; 91 | const double accel = idm.idm(ego_v, ego_v0); 92 | EXPECT_NEAR(accel, 1.47879202184, 1e-3); 93 | } 94 | 95 | { 96 | const double ego_v = 40.0; 97 | const double ego_v0 = 29.0; 98 | const double accel = idm.idm(ego_v, ego_v0); 99 | EXPECT_NEAR(accel, -1.34454982035, 1e-3); 100 | } 101 | 102 | { 103 | const double ego_v = 20.0; 104 | const double ego_v0 = 29.0; 105 | const double lead_v = 29.0; 106 | const double s = 50.0; 107 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 108 | EXPECT_NEAR(accel, 1.15583439997, 1e-3); 109 | } 110 | 111 | { 112 | const double ego_v = 25.0; 113 | const double ego_v0 = 29.0; 114 | const double lead_v = 15.0; 115 | const double s = 50.0; 116 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 117 | EXPECT_NEAR(accel, -3.97784967465, 1e-3); 118 | } 119 | 120 | { 121 | const double ego_v = 29.0; 122 | const double ego_v0 = 29.0; 123 | const double lead_v = 0.0; 124 | const double s = 100.0; 125 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 126 | EXPECT_NEAR(accel, -8.0, 1e-3); 127 | } 128 | } 129 | 130 | TEST(AdaptiveCruiseControl, idm) { 131 | AdaptiveCruiseControl idm; 132 | 133 | { 134 | const double ego_v = 10.0; 135 | const double ego_v0 = 29.0; 136 | const double accel = idm.idm(ego_v, ego_v0); 137 | EXPECT_NEAR(accel, 1.47879202184, 1e-3); 138 | } 139 | 140 | { 141 | const double ego_v = 40.0; 142 | const double ego_v0 = 29.0; 143 | const double accel = idm.idm(ego_v, ego_v0); 144 | EXPECT_NEAR(accel, -1.34454982035, 1e-3); 145 | } 146 | 147 | { 148 | const double ego_v = 20.0; 149 | const double ego_v0 = 29.0; 150 | const double lead_v = 29.0; 151 | const double s = 50.0; 152 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 153 | EXPECT_NEAR(accel, 1.15583439997, 1e-3); 154 | } 155 | 156 | { 157 | const double ego_v = 25.0; 158 | const double ego_v0 = 29.0; 159 | const double lead_v = 15.0; 160 | const double s = 50.0; 161 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 162 | EXPECT_NEAR(accel, -3.16738208405, 1e-3); 163 | } 164 | 165 | { 166 | const double ego_v = 29.0; 167 | const double ego_v0 = 29.0; 168 | const double lead_v = 0.0; 169 | const double s = 100.0; 170 | const double accel = idm.idm(ego_v, ego_v0, lead_v, s); 171 | EXPECT_NEAR(accel, -6.62828409616, 1e-3); 172 | } 173 | } 174 | 175 | 176 | int main(int argc, char** argv) { 177 | testing::InitGoogleTest(&argc, argv); 178 | return RUN_ALL_TESTS(); 179 | } 180 | -------------------------------------------------------------------------------- /src/router/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | #file(GLOB_RECURSE router_srcs *.cpp) 2 | set(router_srcs 3 | loop_router/loop_router.cpp 4 | ) 5 | 6 | add_library(routing_algos 7 | ${router_srcs} 8 | ) 9 | target_link_libraries(routing_algos 10 | ${Carla_LIBRARIES} 11 | ${Boost_LIBRARIES} 12 | ) 13 | -------------------------------------------------------------------------------- /src/router/common/router.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | namespace router { 22 | 23 | /** 24 | * \brief Router is supposed to be the base class for all router classes. 25 | * 26 | * The Router class should contain the road sequence and a vehicle is supposed 27 | * to follow. This helps the vehicle to define the forward direction. 28 | */ 29 | class Router { 30 | 31 | protected: 32 | 33 | using CarlaWaypoint = carla::client::Waypoint; 34 | 35 | public: 36 | 37 | virtual ~Router() {} 38 | 39 | /** 40 | * \brief Tells whether the query road is in the road sequence. 41 | * \param[in] road The ID of the query road. 42 | * \return True if the query road is in the sequence. 43 | */ 44 | virtual bool hasRoad(const size_t road) const = 0; 45 | 46 | /** 47 | * \brief This function is supposed to find a carla waypoint at the 48 | * same location of the query waypoint but have a different 49 | * road ID that is on the route. 50 | * 51 | * It could happen in the carla simulator that two waypoints shares 52 | * the same location but have different road IDs (e.g. the off-ramp 53 | * overlaps with the highway road). 54 | * 55 | * FIXME: It is hard to implement such a function because of the 56 | * limited APIs in the carla client. 57 | * 58 | * \param[in] waypoint The query waypoint. 59 | * \return The waypoint on the route that shares the same location with 60 | * the query waypoint. If such a waypoint cannot be found, 61 | * \c nullptr is returned. 62 | */ 63 | virtual boost::shared_ptr waypointOnRoute( 64 | const boost::shared_ptr& waypoint) const = 0; 65 | 66 | /** 67 | * \brief Get the next road ID on the route of the query road. 68 | * \param[in] road The query road ID. 69 | * \return The next road ID if there is one. Otherwise \c boost::none 70 | * is returned, which can happen the given road is the last 71 | * one in the route. 72 | */ 73 | virtual boost::optional nextRoad(const size_t road) const = 0; 74 | 75 | /** 76 | * \brief Get the previous road ID on the route of the query road. 77 | * \param[in] road The query road ID. 78 | * \return The previous road ID if there is one. Otherwise \c boost::none 79 | * is returned, which can happen the given road is the first 80 | * one in the route. 81 | */ 82 | virtual boost::optional prevRoad(const size_t road) const = 0; 83 | 84 | /** 85 | * \brief Get the next road ID on the route of the road which has the 86 | * query waypoint. 87 | * \param[in] waypoint The query waypoint. 88 | * \return The next road ID if there is one. Otherwise \c boost::none 89 | * is returned, which can happen the road contains the query 90 | * waypoint is the last road on the route. 91 | */ 92 | virtual boost::optional nextRoad( 93 | const boost::shared_ptr& waypoint) const = 0; 94 | 95 | /** 96 | * \brief Get the previous road ID on the route of the road which has the 97 | * query waypoint. 98 | * \param[in] waypoint The query waypoint. 99 | * \return The previous road ID if there is one. Otherwise \c boost::none 100 | * is returned, which can happen the road contains the query 101 | * waypoint is the first road on the route. 102 | */ 103 | virtual boost::optional prevRoad( 104 | const boost::shared_ptr& waypoint) const = 0; 105 | 106 | /** 107 | * \brief Get the front waypoint of the query one with a certain distance. 108 | * 109 | * In the case such a front waypoint is available on the same road, the front 110 | * waypoint is returned. Otherwise, the function will search for a waypoint on 111 | * the next road on the route. 112 | * 113 | * \param[in] waypoint The query waypoint. 114 | * \param[in] distance The distance to look for the front waypoint. 115 | * \return If there is no such front waypoint, \c nullptr is returned. 116 | */ 117 | virtual boost::shared_ptr frontWaypoint( 118 | const boost::shared_ptr& waypoint, const double distance) const = 0; 119 | 120 | /** 121 | * \brief Get the road sequence in the router 122 | */ 123 | virtual const std::vector& roadSequence() const = 0; 124 | 125 | }; // End class Router. 126 | 127 | } // End namespace router. 128 | -------------------------------------------------------------------------------- /src/router/loop_router/loop_router.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace router { 24 | 25 | LoopRouter::LoopRouter() : 26 | road_sequence_({47, 558, 48, 887, 49, 717, 50, 42, 276, 43, 35, 636, 36, 27 | 540, 37, 1021, 38, 678, 39, 728, 40, 841, 41, 6, 45, 103, 28 | 46, 659}){ return; } 29 | 30 | boost::shared_ptr LoopRouter::waypointOnRoute( 31 | const boost::shared_ptr& waypoint) const { 32 | 33 | std::vector> candidates = waypoint->GetNext(0.01); 34 | for (const auto& candidate : candidates) { 35 | std::vector::const_iterator iter = std::find( 36 | road_sequence_.begin(), road_sequence_.end(), candidate->GetRoadId()); 37 | if (iter != road_sequence_.end()) 38 | return candidate; 39 | } 40 | 41 | return nullptr; 42 | } 43 | 44 | boost::optional LoopRouter::nextRoad(const size_t road) const { 45 | std::vector::const_iterator iter = std::find( 46 | road_sequence_.begin(), road_sequence_.end(), road); 47 | if (iter == road_sequence_.end()) 48 | throw std::runtime_error((boost::format( 49 | "LoopRouter::nextRoad(): " 50 | "given road %1% is not on the route.\n") % road).str()); 51 | 52 | if (iter != road_sequence_.end()-1) return *(++iter); 53 | else return road_sequence_.front(); 54 | } 55 | 56 | boost::optional LoopRouter::prevRoad(const size_t road) const { 57 | std::vector::const_iterator iter = std::find( 58 | road_sequence_.begin(), road_sequence_.end(), road); 59 | if (iter == road_sequence_.end()) 60 | throw std::runtime_error((boost::format( 61 | "LoopRouter::nextRoad(): " 62 | "given road %1% is not on the route.\n") % road).str()); 63 | 64 | if (iter != road_sequence_.begin()) return *(--iter); 65 | else return road_sequence_.back(); 66 | } 67 | 68 | boost::optional LoopRouter::nextRoad( 69 | const boost::shared_ptr& waypoint) const { 70 | return nextRoad(waypoint->GetRoadId()); 71 | } 72 | 73 | boost::optional LoopRouter::prevRoad( 74 | const boost::shared_ptr& waypoint) const { 75 | return prevRoad(waypoint->GetRoadId()); 76 | } 77 | 78 | boost::shared_ptr LoopRouter::frontWaypoint( 79 | const boost::shared_ptr& waypoint, 80 | const double distance) const { 81 | 82 | if (distance <= 0.0) { 83 | std::string error_msg("LoopRouter::frontWaypoint(): distance < 0 when searching for the front waypoint.\n"); 84 | std::string waypoint_msg = 85 | (boost::format("waypoint %1% x:%2% y:%3% z:%4% r:%5% p:%6% y:%7% road:%8% lane:%9%.\n") 86 | % waypoint->GetId() 87 | % waypoint->GetTransform().location.x 88 | % waypoint->GetTransform().location.y 89 | % waypoint->GetTransform().location.z 90 | % waypoint->GetTransform().rotation.roll 91 | % waypoint->GetTransform().rotation.pitch 92 | % waypoint->GetTransform().rotation.yaw 93 | % waypoint->GetRoadId() 94 | % waypoint->GetLaneId()).str(); 95 | std::string distance_msg = (boost::format("Distance:%1%\n") % distance).str(); 96 | throw std::runtime_error(error_msg + waypoint_msg + distance_msg); 97 | } 98 | 99 | std::vector> candidates = waypoint->GetNext(distance); 100 | const size_t this_road = waypoint->GetRoadId(); 101 | 102 | boost::optional is_next_road = nextRoad(this_road); 103 | const size_t next_road = is_next_road ? *is_next_road : -1; 104 | 105 | boost::shared_ptr next_waypoint = nullptr; 106 | for (const auto& candidate : candidates) { 107 | // If we find a candidate on the same road with the given waypoint, this is it. 108 | if (candidate->GetRoadId() == this_road) return candidate; 109 | // Otherwise we keep track of which candidate is on the next road. 110 | if (candidate->GetRoadId() == next_road) next_waypoint = candidate; 111 | } 112 | 113 | return next_waypoint; 114 | } 115 | 116 | } // End namespace router. 117 | -------------------------------------------------------------------------------- /src/router/loop_router/loop_router.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2020 Ke Sun 3 | * 4 | * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | * 6 | * 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | * 8 | * 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | * 10 | * 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 11 | * 12 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 13 | */ 14 | 15 | #pragma once 16 | 17 | #include 18 | #include 19 | 20 | namespace router { 21 | 22 | /** 23 | * \brief LoopRouter implements a predefined loop router which never ends. 24 | * 25 | * For now, the predefined route is the highway loop in the carla map Town04. 26 | */ 27 | class LoopRouter : public Router { 28 | 29 | protected: 30 | 31 | std::vector road_sequence_; 32 | 33 | public: 34 | 35 | /** 36 | * \brief Default constructor. 37 | * 38 | * The constructor has a predefined road sequence, which forms a 39 | * a loop in the map. 40 | */ 41 | LoopRouter(); 42 | 43 | /// Destructor of the class. 44 | ~LoopRouter() { return; } 45 | 46 | bool hasRoad(const size_t road) const override { 47 | std::vector::const_iterator iter = std::find( 48 | road_sequence_.begin(), road_sequence_.end(), road); 49 | return iter != road_sequence_.end(); 50 | } 51 | 52 | boost::shared_ptr waypointOnRoute( 53 | const boost::shared_ptr& waypoint) const override; 54 | 55 | boost::optional nextRoad(const size_t road) const override; 56 | 57 | boost::optional prevRoad(const size_t road) const override; 58 | 59 | boost::optional nextRoad( 60 | const boost::shared_ptr& waypoint) const override; 61 | 62 | boost::optional prevRoad( 63 | const boost::shared_ptr& waypoint) const override; 64 | 65 | boost::shared_ptr frontWaypoint( 66 | const boost::shared_ptr& waypoint, 67 | const double distance) const override; 68 | 69 | /** 70 | * \brief Get the road sequence in the LoopRouter. 71 | * 72 | * Keep in mind that the next road of the last element in the returned 73 | * vector is the first element. 74 | */ 75 | const std::vector& roadSequence() const override { 76 | return road_sequence_; 77 | } 78 | 79 | }; // End class LoopRouter. 80 | 81 | } // End namespace router. 82 | --------------------------------------------------------------------------------