├── .gitattributes ├── .gitignore ├── .gitmodules ├── README.md ├── control_tree_car ├── CMakeLists.txt ├── applications │ ├── CMakeLists.txt │ ├── common │ │ ├── CMakeLists.txt │ │ ├── common │ │ │ ├── CMakeLists.txt │ │ │ └── common │ │ │ │ ├── behavior_base.h │ │ │ │ ├── behavior_manager.cpp │ │ │ │ ├── behavior_manager.h │ │ │ │ ├── common.cpp │ │ │ │ ├── common.h │ │ │ │ ├── control_tree.h │ │ │ │ ├── utility.cpp │ │ │ │ └── utility.h │ │ └── nodes │ │ │ ├── CMakeLists.txt │ │ │ └── nodes │ │ │ └── trajectory_controller_node.cpp │ ├── obstacles │ │ ├── CMakeLists.txt │ │ ├── komo │ │ │ ├── CMakeLists.txt │ │ │ └── komo │ │ │ │ ├── komo_factory.cpp │ │ │ │ ├── komo_factory.h │ │ │ │ ├── obstacle_avoidance_dec.cpp │ │ │ │ ├── obstacle_avoidance_dec.h │ │ │ │ ├── obstacle_avoidance_linear.h │ │ │ │ ├── obstacle_avoidance_tree.cpp │ │ │ │ ├── obstacle_avoidance_tree.h │ │ │ │ ├── trajectory_plot.cpp │ │ │ │ ├── trajectory_plot.h │ │ │ │ ├── utility_komo.cpp │ │ │ │ ├── utility_komo.h │ │ │ │ └── velocity_axis.h │ │ └── nodes │ │ │ ├── CMakeLists.txt │ │ │ └── nodes │ │ │ ├── obstacle_avoidance_evaluation_node.cpp │ │ │ ├── obstacle_avoidance_tree_node.cpp │ │ │ ├── obstacle_common.cpp │ │ │ ├── obstacle_common.h │ │ │ └── obstacle_popping_control_node.cpp │ └── pedestrians │ │ ├── CMakeLists.txt │ │ ├── nodes │ │ ├── CMakeLists.txt │ │ └── nodes │ │ │ ├── pedestrian_common.cpp │ │ │ ├── pedestrian_common.h │ │ │ ├── pedestrian_popping_control_node.cpp │ │ │ └── pedestrian_tree_qp_node.cpp │ │ ├── qp │ │ ├── CMakeLists.txt │ │ └── qp │ │ │ ├── MPC_model.cpp │ │ │ ├── MPC_model.h │ │ │ ├── QP_constraints.cpp │ │ │ ├── QP_constraints.h │ │ │ ├── QP_tree_problem_DecQP.cpp │ │ │ ├── QP_tree_problem_DecQP.h │ │ │ ├── QP_tree_problem_DecQP_utils.cpp │ │ │ ├── QP_tree_problem_DecQP_utils.h │ │ │ ├── QP_tree_problem_OSQP.cpp │ │ │ ├── QP_tree_problem_OSQP.h │ │ │ ├── QP_tree_solver_base.cpp │ │ │ ├── QP_tree_solver_base.h │ │ │ ├── control_tree_plot.cpp │ │ │ ├── control_tree_plot.h │ │ │ ├── stopline_qp_tree.cpp │ │ │ └── stopline_qp_tree.h │ │ └── test │ │ ├── CMakeLists.txt │ │ ├── common │ │ ├── komo_problems_cpp │ │ ├── komo_problems_h │ │ ├── qp_problems.cpp │ │ └── qp_problems.h │ │ ├── test_KOMO_collision_avoidance_cpp │ │ ├── test_KOMO_utility_cpp │ │ ├── test_MPC_model.cpp │ │ ├── test_QP_DecQP.cpp │ │ ├── test_QP_DecQP_utils.cpp │ │ ├── test_QP_OSQP.cpp │ │ ├── test_QP_constraints.cpp │ │ ├── test_emergency_braking.cpp │ │ ├── test_obstacle_markers_cpp │ │ ├── test_probability_fusion.cpp │ │ ├── test_sparsification.cpp │ │ └── test_utility_cpp ├── data │ ├── LGP-real-time-1d.g │ ├── LGP-real-time.g │ ├── doc │ │ ├── obstacles.png │ │ └── pedestrians.png │ ├── meshes │ │ ├── bmw │ │ │ ├── BMW.png │ │ │ ├── G12Pos8.dae │ │ │ ├── G12Pos8simple.dae │ │ │ ├── _6a00d83451db4269e200e55380b5f68834-800wi.jpg │ │ │ ├── black_walnut.jpg │ │ │ ├── bleather.png.001.png │ │ │ ├── bmw.dae │ │ │ ├── bmw_.dae │ │ │ ├── bmw__.dae │ │ │ ├── buton.png.002.png │ │ │ ├── carpet.jpg │ │ │ ├── disp.png.001.png │ │ │ ├── grill.png │ │ │ ├── tleather.png │ │ │ └── wheel.png │ │ ├── person_standing │ │ │ ├── materials │ │ │ │ └── textures │ │ │ │ │ ├── eyebrow001-unmodified.png │ │ │ │ │ ├── eyebrow001.png │ │ │ │ │ ├── green_eye.png │ │ │ │ │ ├── jeans01_normals.png │ │ │ │ │ ├── jeans_basic_diffuse.png │ │ │ │ │ ├── male02_diffuse_black-unmodified.png │ │ │ │ │ ├── male02_diffuse_black.png │ │ │ │ │ ├── teeth.png │ │ │ │ │ ├── tshirt02_normals.png │ │ │ │ │ ├── tshirt02_texture.png │ │ │ │ │ └── young_lightskinned_male_diffuse.png │ │ │ ├── meshes │ │ │ │ └── standing.dae │ │ │ ├── model.config │ │ │ └── model.sdf │ │ └── person_walking │ │ │ ├── materials │ │ │ └── textures │ │ │ │ ├── eyebrow001-unmodified.png │ │ │ │ ├── eyebrow001.png │ │ │ │ ├── green_eye.png │ │ │ │ ├── jeans01_normals.png │ │ │ │ ├── jeans_basic_diffuse.png │ │ │ │ ├── male02_diffuse_black-unmodified.png │ │ │ │ ├── male02_diffuse_black.png │ │ │ │ ├── teeth.png │ │ │ │ ├── tshirt02_normals.png │ │ │ │ ├── tshirt02_texture.png │ │ │ │ └── young_lightskinned_male_diffuse.png │ │ │ ├── meshes │ │ │ └── walking.dae │ │ │ ├── model.config │ │ │ └── model.sdf │ └── urdf │ │ └── car.urdf ├── externals │ └── CMakeLists.txt ├── launch │ ├── display_obstacle_avoidance.launch │ ├── display_pedestrian.launch │ ├── display_single_car.launch │ ├── display_three_cars.launch │ ├── display_two_cars.launch │ ├── obstacle_avoidance.launch │ ├── obstacle_avoidance_.launch │ ├── pedestrian.launch │ ├── pedestrian_komo.launch │ ├── single_car.launch │ ├── three_cars.launch │ └── two_cars.launch ├── package.xml └── rviz │ ├── car.rviz │ ├── car.vcg │ ├── obstacle_avoidance.rviz │ ├── pedestrian.rviz │ ├── three_cars.rviz │ └── two_cars.rviz ├── lgp_car_gazebo_plugin ├── CMakeLists.txt ├── meshes │ ├── car.blend │ ├── car.blend1 │ └── car.dae ├── package.xml ├── src │ ├── lgp_car.cpp │ ├── lgp_car.hpp │ ├── lgp_pedestrian.cpp │ └── lgp_pedestrian.hpp └── world │ ├── obstacle_avoidance.world │ ├── obstacle_avoidance_2.world │ ├── pedestrian.world │ ├── pedestrian_1.world │ ├── pedestrian_2.world │ ├── pedestrian_3.world │ ├── pedestrian_4.world │ ├── pedestrian_9.world │ ├── single_car.world │ ├── three_cars.world │ └── two_cars.world └── lgp_car_models ├── lgp_car ├── model.config └── model.sdf ├── lgp_obstacle ├── model.config └── model.sdf └── lgp_pedestrian ├── model.config └── model.sdf /.gitattributes: -------------------------------------------------------------------------------- 1 | *.png filter=lfs diff=lfs merge=lfs -text 2 | *.dae filter=lfs diff=lfs merge=lfs -text 3 | *.jpg filter=lfs diff=lfs merge=lfs -text 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | *.user* 3 | *.global 4 | z.* 5 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "control_tree_car/externals/rai"] 2 | path = control_tree_car/externals/rai 3 | url = git@github.com:ControlTrees/rai.git 4 | [submodule "control_tree_car/externals/tamp"] 5 | path = control_tree_car/externals/tamp 6 | url = git@github.com:ControlTrees/tamp.git 7 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This repo contains the code for 2 examples of the ICRA 2021 submission. 2 | 3 | [Accompanying video](https://youtu.be/Ju5hv2gIlxw) 4 | 5 | A standalone version of the solver can be found by following the link [Solver only](https://github.com/ControlTrees/solver). 6 | This version needs way less dependencies (e.g. no ros) and doesn't contain the examples of the paper. 7 | 8 | 9 | # Dependencies 10 | 11 | ### ros 12 | The planner is intergated as a ros node. It has been developped with ros kinetic but should be compatible with merely any recent ros distro. 13 | [Ros installation](https://www.ros.org/install/) 14 | 15 | ### gazebo 16 | Car dynamic is simulated with Gazebo. 17 | [Gazebo installation](http://gazebosim.org/tutorials?tut=ros_wrapper_versions&cat=connect_ros) 18 | 19 | ### osqp 20 | We use the QP solver OSQP as a baseline (see paper). 21 | [OSQP installation](https://osqp.org/docs/installation/cc++) 22 | 23 | ### other dependencies 24 | 25 | In addition, the following packages are necessary: gnuplot, libjsoncpp-dev, libx11-dev, liblapack-dev, libf2c2-dev, libeigen3-dev, libglew-dev, freeglut3-dev. 26 | They can be installed by calling `sudo apt install PACKAGE_NAME`. 27 | 28 | # Build 29 | Clone the repository and its submodules into your catkin workspace: 30 | ```bash 31 | git clone --recursive git@github.com:ControlTrees/icra2021.git 32 | ``` 33 | This will clone the code related to the examples and two submodules (https://github.com/ControlTrees/rai, and https://github.com/ControlTrees/tamp). 34 | 35 | ### install gazebo models 36 | The car, obstacle and pedestrian gazebo models are part of the repository and must be copied to the gazebo folder containing all models. 37 | This gazebo folder is typicall under `~/.gazebo/models`. 38 | ```bash 39 | cd icra2021/lgp_car_models 40 | cp -r * ~/.gazebo/models 41 | ``` 42 | 43 | ### build RAI 44 | The rai submodule doesn't use cmake as build system, it has to be build separatly. 45 | ```bash 46 | cd ../control_tree_car/externals/rai 47 | make 48 | ``` 49 | The build can take a few minutes. 50 | 51 | ### build ros nodes 52 | The rest of the code can be built as standard ros nodes using `catkin_make` in the source directory of the catkin workspace. 53 | ```bash 54 | catkin_make 55 | ``` 56 | 57 | # Execute tests 58 | Open a terminal in the folder corresponding to the `control_tree_car` package in `build` folder of the catkin workspace. 59 | This is typically `${CATKIN_WORKSPACE}/build/icra_2021/control_tree_car` where `CATKIN_WORKSPACE` is the root of the catkin workspace. 60 | 61 | The tests can be launched using ctest: 62 | ```bash 63 | ctest . 64 | ``` 65 | 66 | # Launch examples 67 | 68 | ### Pedestrian example 69 | Open two terminals in the catkin workspace. 70 | 71 | In the first terminal, type the following command, it will launch the planner and the visualization RViz. 72 | ```bash 73 | roslaunch control_tree_car pedestrian.launch 74 | ``` 75 | 76 | In the second terminal, type the following command, it will launch the simulator. 77 | ```bash 78 | gzserver src/icra_2021/lgp_car_gazebo_plugin/world/pedestrian_4.world 79 | ``` 80 | 81 | ![Image](control_tree_car/data/doc/pedestrians.png) 82 | 83 | ### Obstacle avoidance example 84 | Open two terminals in the catkin workspace. 85 | 86 | In the first terminal, type the following command, it will launch the planner and the visualization RViz. 87 | ```bash 88 | roslaunch control_tree_car obstacle_avoidance.launch 89 | ``` 90 | 91 | In the second terminal, type the following command, it will launch the simulator. 92 | ```bash 93 | gzserver src/icra_2021/lgp_car_gazebo_plugin/world/obstacle_avoidance_2.world 94 | ``` 95 | 96 | ![Image](control_tree_car/data/doc/obstacles.png) 97 | -------------------------------------------------------------------------------- /control_tree_car/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | enable_testing() 4 | 5 | project(control_tree_car) 6 | 7 | catkin_package() 8 | 9 | # MLR 10 | set(MLR_LIBRARIES_DIR "externals/rai/lib") 11 | set(MLR_INCLUDE_DIR "externals/rai/rai") 12 | 13 | link_directories(${MLR_LIBRARIES_DIR}) 14 | include_directories(${MLR_INCLUDE_DIR}) 15 | 16 | #Eigen 17 | find_package(Eigen3 REQUIRED) 18 | include_directories(${EIGEN3_INCLUDE_DIR}) 19 | 20 | # ROS 21 | find_package(catkin REQUIRED COMPONENTS roscpp rospy roslib std_msgs nav_msgs tf) 22 | include_directories(${catkin_INCLUDE_DIRS}) 23 | 24 | add_subdirectory(externals) 25 | add_subdirectory(applications) 26 | -------------------------------------------------------------------------------- /control_tree_car/applications/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | add_subdirectory(common) 4 | add_subdirectory(pedestrians) 5 | add_subdirectory(obstacles) 6 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | add_subdirectory(common) 4 | add_subdirectory(nodes) 5 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | get_filename_component(project_name ${CMAKE_CURRENT_SOURCE_DIR} NAME) 4 | project(${project_name}) 5 | 6 | ################################## 7 | # Locate depedencies 8 | ################################## 9 | find_package(Boost 1.40.0 COMPONENTS filesystem system iostreams REQUIRED) 10 | 11 | ################################# 12 | # Define executable 13 | ################################# 14 | add_definitions(-O3 -Wall -DRAI_LAPACK -DRAI_EIGEN -DRAI_PTHREAD -Wno-terminate -fPIC -std=c++14) 15 | 16 | file(GLOB_RECURSE SOURCES *.cpp *.h) 17 | 18 | add_library(${project_name} STATIC ${SOURCES}) 19 | target_include_directories(${project_name} PUBLIC ".") 20 | target_link_libraries(${project_name} 21 | ${catkin_LIBRARIES} 22 | ${Boost_LIBRARIES} 23 | pthread 24 | ) 25 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/common/behavior_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | class BehaviorManager; 8 | 9 | struct TimeCostPair 10 | { 11 | double planning_time; 12 | double cost; 13 | }; 14 | 15 | class BehaviorBase 16 | { 17 | public: 18 | BehaviorBase(BehaviorManager & manager) 19 | : manager_(manager) 20 | , steps_(4) 21 | {}; 22 | 23 | virtual TimeCostPair plan() = 0; 24 | virtual std::vector get_trajectories() = 0; 25 | 26 | protected: 27 | // params 28 | const uint steps_; 29 | 30 | // state 31 | BehaviorManager & manager_; 32 | }; 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/common/behavior_manager.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | 13 | BehaviorManager::BehaviorManager() 14 | : odo_received_(false) 15 | { 16 | } 17 | 18 | void BehaviorManager::odometry_callback(const nav_msgs::Odometry::ConstPtr& msg) 19 | { 20 | //ROS_INFO("odometry_callback.."); 21 | 22 | // retrieve pose 23 | odometry_ = odometry_state_from_msg(msg); 24 | 25 | odo_received_ = true; 26 | 27 | //ROS_ERROR(" x, y, yaw: [%f] [%f] [%f]", odometry_.x, odometry_.y, odometry_.yaw); 28 | //ROS_ERROR(" v, omega: [%f] [%f]", odometry_.v, odometry_.omega); 29 | } 30 | 31 | 32 | void BehaviorManager::plan() 33 | { 34 | //ROS_INFO( "plan.." ); 35 | 36 | if(!odo_received_) 37 | { 38 | ROS_INFO( "no odometry received, abort planning.." ); 39 | return; 40 | } 41 | 42 | const auto & time_cost = current_behavior_->plan(); 43 | 44 | auto now = std::chrono::high_resolution_clock::now(); 45 | auto dt = std::chrono::duration_cast(now - last_).count() / 1e6; 46 | last_ = now; 47 | 48 | //std::cout << "dt:" << dt << std::endl; 49 | 50 | ++n_plan_; // discard very few iterations since the vehicle may not have attained cruise speed which can bias costs 51 | if(n_plan_ >=100) 52 | { 53 | cost_evaluator_.acc(time_cost.cost, dt); 54 | velocity_evaluator_.acc(odometry_.v, dt); 55 | time_evaluator_.acc(time_cost.planning_time); 56 | } 57 | } 58 | 59 | std::vector BehaviorManager::get_trajectories() const 60 | { 61 | //ROS_INFO( "get_trajectories.." ); 62 | 63 | return current_behavior_->get_trajectories(); 64 | } 65 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/common/behavior_manager.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | class BehaviorBase; 10 | 11 | class BehaviorManager 12 | { 13 | 14 | public: 15 | BehaviorManager(); 16 | 17 | void odometry_callback(const nav_msgs::Odometry::ConstPtr& msg); 18 | void plan(); 19 | std::vector get_trajectories() const; 20 | 21 | void register_behavior(const std::string & key, const std::shared_ptr< BehaviorBase > & behavior) 22 | { 23 | behaviors_[key] = behavior; 24 | } 25 | 26 | void set_current_behavior(const std::string & key) 27 | { 28 | current_behavior_ = behaviors_[key]; 29 | } 30 | 31 | OdometryState odometry() const 32 | { 33 | return odometry_; 34 | } 35 | 36 | double cost() const { return cost_evaluator_.average(); } 37 | double planning_time() const { return time_evaluator_.average(); } 38 | double velocity() const { return velocity_evaluator_.average(); } 39 | 40 | private: 41 | // behaviors 42 | std::map< std::string, std::shared_ptr< BehaviorBase > > behaviors_; 43 | std::shared_ptr< BehaviorBase > current_behavior_; 44 | 45 | // state 46 | bool odo_received_; 47 | OdometryState odometry_; 48 | uint n_plan_ = 0; 49 | std::chrono::high_resolution_clock::time_point last_ = std::chrono::high_resolution_clock::now(); 50 | 51 | // evaluation 52 | ContinuousEvaluator cost_evaluator_; 53 | ContinuousEvaluator velocity_evaluator_; 54 | Evaluator time_evaluator_; 55 | }; 56 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/common/common.cpp: -------------------------------------------------------------------------------- 1 | #include "common.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace 11 | { 12 | 13 | visualization_msgs::Marker create_center_line(double map_x) 14 | { 15 | visualization_msgs::Marker marker; 16 | marker.header.stamp = ros::Time::now(); 17 | marker.header.frame_id = "map"; 18 | marker.id = std::hash()("centerline"); 19 | marker.type = visualization_msgs::Marker::LINE_STRIP;//;visualization_msgs::Marker::CUBE; 20 | marker.action = visualization_msgs::Marker::ADD; 21 | marker.pose.position.x = 0; 22 | marker.pose.position.y = 0; 23 | marker.scale.x = 0.1; // thickness 24 | marker.color.r = 0.0; 25 | marker.color.g = 0.0; 26 | marker.color.b = 0.5; 27 | marker.color.a = 0.5; 28 | 29 | geometry_msgs::Point p; 30 | p.x = map_x - 100; 31 | p.y = 0; 32 | p.z = 0; 33 | 34 | marker.points.push_back(p); 35 | 36 | p.x = map_x + 300; 37 | p.y = 0; 38 | p.z = 0; 39 | 40 | marker.points.push_back(p); 41 | 42 | return marker; 43 | } 44 | 45 | visualization_msgs::Marker create_road_border(double map_x, double road_width) 46 | { 47 | visualization_msgs::Marker marker; 48 | marker.header.stamp = ros::Time::now(); 49 | marker.header.frame_id = "map"; 50 | marker.id = std::hash()("borders"); 51 | marker.type = visualization_msgs::Marker::LINE_LIST;//;visualization_msgs::Marker::CUBE; 52 | marker.action = visualization_msgs::Marker::ADD; 53 | marker.pose.position.x = 0; 54 | marker.pose.position.y = 0; 55 | marker.scale.x = 0.1; // thickness 56 | marker.color.r = 0.0; 57 | marker.color.g = 0.0; 58 | marker.color.b = 0.5; 59 | marker.color.a = 0.8; 60 | 61 | geometry_msgs::Point p; 62 | p.x = map_x - 100; 63 | p.y = -road_width / 2; 64 | p.z = 0; 65 | 66 | marker.points.push_back(p); 67 | 68 | p.x = map_x + 300; 69 | p.y = -road_width / 2; 70 | p.z = 0; 71 | 72 | marker.points.push_back(p); 73 | 74 | p.x = map_x - 100; 75 | p.y = road_width / 2; 76 | p.z = 0; 77 | 78 | marker.points.push_back(p); 79 | 80 | p.x = map_x + 300; 81 | p.y = road_width / 2; 82 | p.z = 0; 83 | 84 | marker.points.push_back(p); 85 | 86 | return marker; 87 | } 88 | } 89 | 90 | double rand_01() 91 | { 92 | static int n = 0; 93 | ++n; 94 | 95 | auto v = (double(rand()) / RAND_MAX); 96 | 97 | //std::cout << "n:" << n << " 01 v:" << v << std::endl; 98 | 99 | return v; 100 | } 101 | 102 | double rand_m11() 103 | { 104 | static int n = 0; 105 | ++n; 106 | 107 | auto v = 2 * (double(rand()) / RAND_MAX - 0.5); 108 | 109 | //std::cout << "n:" << n << " 11 v:" << v << std::endl; 110 | 111 | return v; 112 | } 113 | 114 | double draw_p(double median_p) 115 | { 116 | double exponent = log(median_p) / log(0.5); 117 | return pow(rand_01(), exponent); 118 | } 119 | 120 | bool draw_bool(double average_p) 121 | { 122 | return (rand_01() < average_p); 123 | } 124 | 125 | RoadModelBuilder& RoadModelBuilder::add_center_line() 126 | { 127 | markers.markers.push_back(create_center_line(map_x)); 128 | return *this; 129 | } 130 | 131 | RoadModelBuilder& RoadModelBuilder::add_road_border() 132 | { 133 | markers.markers.push_back(create_road_border(map_x, road_width)); 134 | return *this; 135 | } 136 | 137 | 138 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/common/common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | double rand_01(); 7 | double rand_m11(); 8 | double draw_p(double median_p); 9 | bool draw_bool(double average_p); 10 | 11 | struct RoadModelBuilder 12 | { 13 | RoadModelBuilder(double map_x, double road_width) 14 | : map_x(map_x) 15 | , road_width(road_width) 16 | { 17 | 18 | } 19 | 20 | RoadModelBuilder& add_center_line(); 21 | 22 | RoadModelBuilder& add_road_border(); 23 | 24 | visualization_msgs::MarkerArray build() { return markers; } 25 | 26 | visualization_msgs::MarkerArray markers; 27 | double map_x; 28 | double road_width; 29 | }; 30 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/common/utility.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | 6 | OdometryState odometry_state_from_msg(const nav_msgs::Odometry::ConstPtr& msg) 7 | { 8 | OdometryState odometry; 9 | 10 | odometry.x = msg->pose.pose.position.x; 11 | odometry.y = msg->pose.pose.position.y; 12 | 13 | tf::Quaternion q( 14 | msg->pose.pose.orientation.x, 15 | msg->pose.pose.orientation.y, 16 | msg->pose.pose.orientation.z, 17 | msg->pose.pose.orientation.w); 18 | tf::Matrix3x3 m(q); 19 | double roll, pitch, yaw; 20 | m.getRPY(roll, pitch, yaw); 21 | odometry.yaw = yaw; 22 | 23 | // retrieve speeds 24 | odometry.v = msg->twist.twist.linear.x; 25 | odometry.omega = msg->twist.twist.angular.z; 26 | 27 | return odometry; 28 | } 29 | 30 | double get_yaw_from_quaternion(const geometry_msgs::Quaternion & quat) 31 | { 32 | tf::Quaternion q( 33 | quat.x, 34 | quat.y, 35 | quat.z, 36 | quat.w); 37 | tf::Matrix3x3 m(q); 38 | double roll, pitch, yaw; 39 | m.getRPY(roll, pitch, yaw); 40 | 41 | return yaw; 42 | } 43 | 44 | geometry_msgs::Quaternion get_quaternion_from_yaw(double yaw) 45 | { 46 | geometry_msgs::Quaternion qq; 47 | tf2::Quaternion q; 48 | q.setRPY(0, 0, yaw); 49 | 50 | qq.x = q.x(); 51 | qq.y = q.y(); 52 | qq.z = q.z(); 53 | qq.w = q.w(); 54 | 55 | return qq; 56 | } 57 | 58 | double calculateDifferenceBetweenAngles(double target, double source) 59 | { 60 | double difference = target - source; 61 | while (difference < -M_PI) difference += 2 * M_PI; 62 | while (difference > M_PI) difference -= 2 * M_PI; 63 | return difference; 64 | } 65 | 66 | bool near(const Pose2D & a, const Pose2D & b, double eps) 67 | { 68 | bool close = true; 69 | 70 | close = close && fabs(a.x-b.x) < eps; 71 | close = close && fabs(a.y-b.y) < eps; 72 | 73 | return close; 74 | } 75 | 76 | Pose2D project_on_trajectory(const Pose2D & p, std::vector trajectory, int & index, double & mu) 77 | { 78 | using namespace Eigen; 79 | 80 | index = -1; 81 | mu = -1; 82 | 83 | int I = 0; // number of passed valid points 84 | for(auto i = 0; i < trajectory.size()-1; ++i) 85 | { 86 | const auto a = trajectory[i]; 87 | const auto b = trajectory[i+1]; 88 | 89 | if(near(a, b)) // identical points, might be the prefix 90 | continue; 91 | 92 | Vector2D n {sin(p.yaw), -cos(p.yaw)}; 93 | Vector2D ab {b.x - a.x, b.y - a.y}; 94 | 95 | Matrix2f A; 96 | A << n.x, a.x - b.x, 97 | n.y, a.y - b.y; 98 | 99 | Vector2f B; 100 | B << a.x - p.x, a.y - p.y; 101 | 102 | if(fabs(A.determinant()) > 0.00001) 103 | { 104 | Vector2f S = A.inverse() * (B); 105 | double lambda = S[0]; 106 | double _mu = S[1]; 107 | 108 | if(0.0 <= _mu && _mu < 1.0 109 | || I == 0 && _mu <= 1.0 // hack to make sure it works if the furst points were skipped 110 | || i == trajectory.size() - 2 && 0 <= _mu) // proj found 111 | { 112 | index = i; 113 | mu = _mu; 114 | return {a.x + mu * ab.x, a.y + mu * ab.y, (1.0 - mu) * a.yaw + mu * b.yaw}; 115 | } 116 | } 117 | 118 | ++I; 119 | } 120 | 121 | return Pose2D{std::nan(""), std::nan(""), std::nan("")}; 122 | } 123 | 124 | double dist(const Position2D& a, const Position2D& b) 125 | { 126 | const auto dx = a.x - b.x; 127 | const auto dy = a.y - b.y; 128 | return sqrt(dx*dx + dy*dy); 129 | } 130 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/common/common/utility.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | class Objective; 9 | 10 | struct OdometryState 11 | { 12 | double x{0}; 13 | double y{0}; 14 | double yaw{0}; 15 | double v{0}; 16 | double omega{0}; 17 | }; 18 | 19 | struct Position2D 20 | { 21 | double x; 22 | double y; 23 | }; 24 | 25 | struct Vector2D 26 | { 27 | double x; 28 | double y; 29 | }; 30 | 31 | struct Pose2D 32 | { 33 | double x; 34 | double y; 35 | double yaw; 36 | }; 37 | 38 | class Evaluator 39 | { 40 | public: 41 | void acc(double val) 42 | { 43 | if(!std::isnan(val)) 44 | { 45 | avg = (avg * N + val) / (N+1); 46 | N++; 47 | } 48 | } 49 | 50 | double average() const { return avg; }; 51 | 52 | private: 53 | double avg = 0; 54 | int N = 0; 55 | }; 56 | 57 | class ContinuousEvaluator 58 | { 59 | public: 60 | void acc(double val, double dt) 61 | { 62 | if(!std::isnan(val)) 63 | { 64 | avg = (avg * T + val * dt) / (T+dt); 65 | T+=dt; 66 | } 67 | } 68 | 69 | double average() const { return avg; }; 70 | 71 | private: 72 | double avg = 0; 73 | double T = 0; 74 | }; 75 | 76 | OdometryState odometry_state_from_msg(const nav_msgs::Odometry::ConstPtr& msg); 77 | double get_yaw_from_quaternion(const geometry_msgs::Quaternion & quat); 78 | geometry_msgs::Quaternion get_quaternion_from_yaw(double yaw); 79 | double calculateDifferenceBetweenAngles(double target, double source); 80 | bool near(const Pose2D & a, const Pose2D & b, double eps = 0.0001); 81 | Pose2D project_on_trajectory(const Pose2D & p, std::vector trajectory, int & index, double & mu); 82 | double dist(const Position2D& a, const Position2D& b); 83 | -------------------------------------------------------------------------------- /control_tree_car/applications/common/nodes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | get_filename_component(project_name ${CMAKE_CURRENT_SOURCE_DIR} NAME) 4 | project(${project_name}) 5 | 6 | ################################## 7 | # Locate depedencies 8 | ################################## 9 | find_package(Boost 1.40.0 COMPONENTS filesystem system iostreams REQUIRED) 10 | find_package(CGAL) 11 | find_package(osqp REQUIRED) 12 | 13 | ################################# 14 | # Define executable 15 | ################################# 16 | add_definitions(-O3 -Wall -DRAI_LAPACK -DRAI_EIGEN -DRAI_PTHREAD -Wno-terminate -fPIC -std=c++14) 17 | 18 | # Trajectory controller 19 | add_executable(trajectory_controller_node nodes/trajectory_controller_node.cpp) 20 | target_link_libraries(trajectory_controller_node 21 | ${catkin_LIBRARIES} 22 | common 23 | #ros_common_lib 24 | ) 25 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | add_subdirectory(komo) 4 | add_subdirectory(nodes) 5 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | get_filename_component(project_name ${CMAKE_CURRENT_SOURCE_DIR} NAME) 4 | project(${project_name}) 5 | 6 | ################################## 7 | # Locate depedencies 8 | ################################## 9 | find_package(Boost 1.40.0 COMPONENTS filesystem system iostreams REQUIRED) 10 | 11 | ################################# 12 | # Define executable 13 | ################################# 14 | add_definitions(-O3 -Wall -DRAI_LAPACK -DRAI_EIGEN -DRAI_PTHREAD -Wno-terminate -fPIC -std=c++14) 15 | 16 | file(GLOB_RECURSE SOURCES *.cpp *.h) 17 | 18 | add_library(${project_name} STATIC ${SOURCES}) 19 | target_include_directories(${project_name} PUBLIC ".") 20 | target_link_libraries(${project_name} 21 | ${catkin_LIBRARIES} 22 | ${Boost_LIBRARIES} 23 | KOMO 24 | Kin 25 | common 26 | GeometricUtilityTasks 27 | MotionPlanning 28 | pthread 29 | ) 30 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/komo_factory.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | KomoFactory::KomoFactory() 11 | : kin_((ros::package::getPath("control_tree_car") + "/data/LGP-real-time.g").c_str()) 12 | {} 13 | 14 | std::shared_ptr< KOMO > KomoFactory::create_komo(uint n_phases, uint steps_per_phase) const 15 | { 16 | auto komo = std::shared_ptr< KOMO >(); 17 | komo = std::make_shared(); 18 | komo->sparseOptimization = true; 19 | komo->setModel(kin_, false); 20 | komo->setTiming(n_phases, steps_per_phase, 1); 21 | komo->verbose = 0; 22 | 23 | return komo; 24 | } 25 | 26 | Objectives KomoFactory::ground_komo(const std::shared_ptr< KOMO > & komo, const std::vector & obstacles, double road_width, double v_desired) const 27 | { 28 | Objectives objectives; 29 | 30 | objectives.acc_ = komo->addObjective(-123., 123., new TM_Transition(komo->world), OT_sos, NoArr, 2.0, 2); 31 | objectives.ax_ = komo->addObjective(-123., 123., new AxisBound("car_ego", AxisBound::Y, AxisBound::EQUAL, komo->world), OT_sos, NoArr, 5 * 1e-1, 0); 32 | //objectives.ax_ = komo->addObjective(-123., 123., new RoadBound("car_ego", road_width / 2.0, vehicle_width, komo->world), OT_sos, NoArr, 1.0, 0); 33 | objectives.vel_ = komo->addObjective(-123., 123., new AxisBound("car_ego", AxisBound::X, AxisBound::EQUAL, komo->world), OT_sos, {v_desired}, 1.5 * 1e-1, 1); 34 | objectives.car_kin_ = komo->addObjective(-123., 123., new CarKinematic("car_ego", komo->world), OT_eq, NoArr, 1e1, 1); 35 | 36 | if(!obstacles.empty()) 37 | { 38 | objectives.circular_obstacle_ = std::shared_ptr (new Car3CirclesCircularObstacle("car_ego", obstacles, komo->world)); 39 | objectives.collision_avoidance_ = komo->addObjective(-123., 123., objectives.circular_obstacle_, OT_ineq, NoArr, 1e2, 0); 40 | } 41 | 42 | return objectives; 43 | } 44 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/komo_factory.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | 18 | constexpr double vehicle_width = 1.8; 19 | 20 | struct Objectives 21 | { 22 | Objective * acc_{0}; 23 | Objective * ax_{0}; 24 | Objective * vel_{0}; 25 | Objective * car_kin_{0}; 26 | std::shared_ptr circular_obstacle_; 27 | Objective * collision_avoidance_{0}; 28 | }; 29 | 30 | class KomoFactory 31 | { 32 | public: 33 | KomoFactory(); 34 | std::shared_ptr< KOMO > create_komo(uint n_phases, uint steps_per_phase) const; 35 | Objectives ground_komo(const std::shared_ptr< KOMO > & komo, 36 | const std::vector & obstacles, 37 | double road_width, 38 | double v_desired) const; 39 | 40 | private: 41 | rai::KinematicWorld kin_; 42 | }; 43 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/obstacle_avoidance_dec.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | class ObstacleAvoidanceDec : public BehaviorBase 32 | { 33 | public: 34 | ObstacleAvoidanceDec(BehaviorManager&, int n_obstacles, bool tree, double road_width, double v_desired, int steps_per_phase); 35 | 36 | void desired_speed_callback(const std_msgs::Float32::ConstPtr& msg); 37 | 38 | void obstacle_callback(const visualization_msgs::MarkerArray::ConstPtr& msg); 39 | 40 | TimeCostPair plan(); 41 | 42 | std::vector get_trajectories(); 43 | 44 | void set_optim_callback(const std::function& callback) { options_.callback = callback; } 45 | 46 | static uint n_branches(uint n_obstacles, bool tree) { return tree ? pow(2.0, n_obstacles) : 1; } 47 | private: 48 | void init_tree(); 49 | void update_groundings(); 50 | void init_optimization_variable(); 51 | 52 | private: 53 | // params 54 | const uint n_obstacles_; // number of obstacles 55 | const uint n_branches_; // number of branches 56 | const bool tree_; 57 | const double road_width_; 58 | const uint horizon_; // planning horizon number of phases ~ [s] 59 | const uint steps_; 60 | 61 | // target: params than can be adapted 62 | double v_desired_; 63 | std::vector obstacles_; 64 | 65 | // komo 66 | KomoFactory komo_factory_; 67 | 68 | // objectives 69 | std::vector objectivess_; 70 | 71 | // state; 72 | mp::TreeBuilder komo_tree_; 73 | 74 | std::vector> komos_; 75 | std::vector> converters_; 76 | std::vector> constrained_problems_; 77 | DecOptConfig options_; 78 | 79 | arr x_; 80 | std::vector xmasks_; 81 | std::vector vars_; // uncompressed vars (dual of xmasks) 82 | std::vector belief_state_; 83 | 84 | intA vars_branch_order_0_; // comressed var (hence all the same) 85 | intA vars_branch_order_1_; 86 | intA vars_branch_order_2_; 87 | }; 88 | 89 | std::vector fuse_probabilities(const std::vector&, bool tree, std::vector> &); 90 | void convert(uint n_branches, uint horizon, mp::TreeBuilder& komo_tree_); 91 | 92 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/obstacle_avoidance_linear.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | 30 | class ObstacleAvoidanceLinear : public BehaviorBase 31 | { 32 | public: 33 | ObstacleAvoidanceLinear(BehaviorManager&, int n_obstacles, double road_width, int steps_per_phase); 34 | 35 | void desired_speed_callback(const std_msgs::Float32::ConstPtr& msg); 36 | 37 | void obstacle_callback(const visualization_msgs::MarkerArray::ConstPtr& msg); 38 | 39 | TimeCostPair plan(); 40 | 41 | std::vector get_trajectories(); 42 | 43 | void set_optim_callback(const std::function& _) {} 44 | 45 | private: 46 | // params 47 | const uint n_obstacles_; 48 | const double road_width_; 49 | rai::KinematicWorld kin_; 50 | const uint steps_; 51 | 52 | // target: params than can be adapted 53 | double v_desired_; 54 | std::vector obstacles_; 55 | 56 | // functional 57 | std::shared_ptr circular_obstacle_; 58 | 59 | // objectives 60 | Objective * acc_; 61 | Objective * ax_; 62 | Objective * vel_; 63 | Objective * car_kin_; 64 | Objective * collision_avoidance_; 65 | 66 | std::shared_ptr komo_; 67 | }; 68 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/obstacle_avoidance_tree.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | 28 | class ObstacleAvoidanceTree : public BehaviorBase 29 | { 30 | public: 31 | ObstacleAvoidanceTree(BehaviorManager&, int steps_per_phase); 32 | 33 | void desired_speed_callback(const std_msgs::Float32::ConstPtr& msg); 34 | 35 | void obstacle_callback(const visualization_msgs::MarkerArray::ConstPtr& msg); 36 | 37 | TimeCostPair plan(); 38 | 39 | std::vector get_trajectories(); 40 | 41 | void set_optim_callback(const std::function& _) {} 42 | 43 | private: 44 | void update_tree(double p); 45 | 46 | private: 47 | // params 48 | rai::KinematicWorld kin_; 49 | const uint steps_; 50 | 51 | // target: params than can be adapted 52 | double v_desired_; 53 | double existence_probability_; 54 | std::vector obstacles_; 55 | 56 | // functional 57 | std::shared_ptr circular_obstacle_; 58 | 59 | // objectives 60 | Objective * acc_; 61 | Objective * ax_; 62 | Objective * vel_; 63 | Objective * car_kin_; 64 | Objective * collision_avoidance_; 65 | 66 | // state; 67 | mp::TreeBuilder tree_; 68 | intA vars_branch_1_order_0_; 69 | intA vars_branch_1_order_1_; 70 | intA vars_branch_1_order_2_; 71 | intA vars_branch_2_order_0_; 72 | intA vars_branch_2_order_1_; 73 | intA vars_branch_2_order_2_; 74 | intA vars_all_order_0_; 75 | intA vars_all_order_1_; 76 | intA vars_all_order_2_; 77 | 78 | arr scales_all_; 79 | 80 | std::shared_ptr komo_; 81 | }; 82 | 83 | 84 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/trajectory_plot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | double radius(const visualization_msgs::MarkerArray::Ptr & obstacles) 4 | { 5 | if(obstacles->markers.size() < 2) 6 | return 0.0; 7 | 8 | const auto&m = obstacles->markers[1]; 9 | 10 | std::cout << m.scale.x << std::endl; 11 | 12 | return m.scale.x / 2;; 13 | } 14 | 15 | void plot(const std::vector & trajectories, 16 | const visualization_msgs::MarkerArray::Ptr & obstacles, 17 | const PlotAxis & axis) 18 | { 19 | TrajectoryPlot plotter(trajectories.size(), axis.name, axis.range); 20 | plotter.update(trajectories, obstacles); 21 | } 22 | 23 | void TrajectoryPlot::update(const std::vector & trajectories, const visualization_msgs::MarkerArray::Ptr & obstacles) 24 | { 25 | auto color_code = [](int i, double p)->std::string 26 | { 27 | std::stringstream ss; 28 | ss << "#"; 29 | ss << std::hex << int((1.0 - p) * 255); 30 | ss << "00"; 31 | ss << "00"; 32 | ss << "FF"; 33 | 34 | return ss.str(); 35 | }; 36 | 37 | auto & gp = gp_; 38 | 39 | gp << "set size ratio -1" << "\n"; 40 | gp << "set title '" << name_ << "'\n"; 41 | gp << "set xrange [-10:50]\nset yrange " << yrange_ << "\n"; 42 | gp << "plot "; 43 | 44 | for(auto i = 0; i < trajectories.size(); ++i) 45 | { 46 | gp << "'-' with lines title '" << name_ << "-" << i << "'"; 47 | gp << " lc rgb '"<< color_code(i, 1.0) <<"'"; 48 | 49 | gp << ","; 50 | } 51 | 52 | gp << "'-' with circles" << "\n"; 53 | 54 | // send trajs 55 | std::vector > > xy_pts_all; 56 | 57 | for(const auto& traj : trajectories) 58 | { 59 | std::vector > xy_pts; 60 | for(int i=0; i > xyr_pts; 77 | for(auto i = 0; i < obstacles->markers.size() / 2; ++i) 78 | { 79 | const auto&m = obstacles->markers[2 * i + 1]; 80 | 81 | /// position and geometry 82 | auto x = m.pose.position.x; 83 | auto y = m.pose.position.y; 84 | auto r = m.scale.x / 2; 85 | 86 | //std::cout << "radius:" << r << std::endl; 87 | 88 | xyr_pts.push_back(std::make_tuple(x, y, r)); 89 | } 90 | 91 | gp.send1d(xyr_pts); 92 | } 93 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/trajectory_plot.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include "gnuplot-iostream.h" 10 | 11 | 12 | struct PlotAxis 13 | { 14 | std::string name; 15 | std::string range; 16 | }; 17 | 18 | void plot(const std::vector & trajectories, 19 | const visualization_msgs::MarkerArray::Ptr & obstacles, 20 | const PlotAxis & axis); 21 | 22 | class TrajectoryPlot 23 | { 24 | public: 25 | TrajectoryPlot(int n_branches, const std::string & name, const std::string & yrange) 26 | : n_branches_(n_branches) 27 | , name_(name) 28 | , yrange_(yrange) 29 | { 30 | } 31 | 32 | void update(const std::vector & trajectories, const visualization_msgs::MarkerArray::Ptr & obstacles); 33 | 34 | private: 35 | Gnuplot gp_; 36 | int n_branches_; 37 | std::string name_; 38 | std::string yrange_; 39 | }; 40 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/utility_komo.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | class Objective; 14 | class KOMO; 15 | 16 | struct Costs 17 | { 18 | double total; 19 | std::unordered_map costs; 20 | }; 21 | 22 | geometry_msgs::PoseStamped kin_to_pose_msg(const rai::KinematicWorld * kin); 23 | geometry_msgs::PoseStamped kin_1d_to_pose_msg(const rai::KinematicWorld * kin); 24 | Costs traj_cost(const WorldL & Gs, const std::list & objectives); 25 | WorldL get_traj_start(const WorldL & configurations, int start = 0, int end = 2); 26 | void unify_prefix(std::vector>& komos); 27 | void shift_komos(std::vector>& komos, const OdometryState & o, uint steps); 28 | inline int shift_komos(const std::shared_ptr& komo, const OdometryState & o, uint steps) {NIY;return 0;} 29 | //void shift_dual(DualState& state, int dual_dim_per_step, int index); 30 | //void shift(arr& state, uint n); 31 | std::vector convert(const std::shared_ptr& komo); 32 | void slide_trajectory(uint index, uint steps, std::vector& trajectory); 33 | void slide_trajectory(uint index, double mu, uint steps, std::vector& trajectory); 34 | Pose2D interpolate(const Pose2D& a, const Pose2D& b, double mu); 35 | void translate_trajectory(const OdometryState & o, uint steps, std::vector& trajectory); 36 | void first_guess_trajectory(const OdometryState & o, uint steps, std::vector& trajectory); 37 | void first_guess_trajectory(const OdometryState & o, double v_min, uint steps, std::vector& trajectory); 38 | void update_komo(const std::vector& trajectory, const std::shared_ptr& komo); 39 | void update_x(arr&x, const std::vector>& komos, const std::vector& vars); 40 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/komo/komo/velocity_axis.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | struct VelocityAxis:Feature{ 7 | 8 | VelocityAxis(const rai::KinematicWorld& world, const std::string & agent_object) 9 | : tm_(new TM_Default(TMT_pos, world, agent_object.c_str(), NoVector, NULL, NoVector) ) 10 | { 11 | } 12 | 13 | virtual rai::String shortTag(const rai::KinematicWorld& G) 14 | { 15 | return rai::String("VelocityX"); 16 | } 17 | 18 | virtual void phi(arr& y, arr& J, const rai::KinematicWorld& G) override 19 | { 20 | tm_->phi(y, J, G); 21 | 22 | y(1) = 0; 23 | J(1, 1) = 0; 24 | } 25 | 26 | virtual uint dim_phi(const rai::KinematicWorld& K) override 27 | { 28 | return tm_->dim_phi(K); 29 | } 30 | 31 | private: 32 | std::shared_ptr tm_; 33 | }; 34 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/nodes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | get_filename_component(project_name ${CMAKE_CURRENT_SOURCE_DIR} NAME) 4 | project(${project_name}) 5 | 6 | ################################## 7 | # Locate depedencies 8 | ################################## 9 | 10 | find_package(Boost 1.40.0 COMPONENTS filesystem system iostreams REQUIRED) 11 | find_package(CGAL) 12 | find_package(osqp REQUIRED) 13 | 14 | ################################# 15 | # Define executable 16 | ################################# 17 | add_definitions(-O3 -Wall -DRAI_LAPACK -DRAI_EIGEN -DRAI_PTHREAD -Wno-terminate -fPIC -std=c++14) 18 | 19 | # common utilities 20 | file(GLOB obstacle_common_src "nodes/obstacle_common.h" "nodes/obstacle_common.cpp") 21 | add_library(obstacle_common_lib STATIC ${obstacle_common_src}) 22 | target_include_directories(obstacle_common_lib PUBLIC ".") 23 | target_link_libraries(obstacle_common_lib 24 | ${catkin_LIBRARIES} 25 | common 26 | ) 27 | 28 | # Obstacles 29 | add_executable(obstacle_avoidance_tree_node nodes/obstacle_avoidance_tree_node.cpp) 30 | target_link_libraries(obstacle_avoidance_tree_node 31 | komo 32 | obstacle_common_lib 33 | ) 34 | 35 | add_executable(obstacle_popping_control_node nodes/obstacle_popping_control_node.cpp) 36 | target_link_libraries(obstacle_popping_control_node 37 | komo 38 | obstacle_common_lib 39 | ) 40 | 41 | add_executable(obstacle_avoidance_evaluation_node nodes/obstacle_avoidance_evaluation_node.cpp) 42 | target_link_libraries(obstacle_avoidance_evaluation_node 43 | komo 44 | obstacle_common_lib 45 | ) 46 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/nodes/nodes/obstacle_avoidance_evaluation_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "ros/ros.h" 5 | #include 6 | 7 | #include "nav_msgs/Odometry.h" 8 | #include "nav_msgs/Path.h" 9 | #include "geometry_msgs/Twist.h" 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | 18 | namespace 19 | { 20 | class TrajEvaluator 21 | { 22 | public: 23 | TrajEvaluator(int steps_per_phase, double road_width, double v_desired) 24 | : steps_per_phase_(steps_per_phase) 25 | { 26 | komo_ = komo_factory_.create_komo(2, steps_per_phase); 27 | objectives_ = komo_factory_.ground_komo(komo_, {}, road_width, v_desired); 28 | 29 | komo_->reset(0); 30 | } 31 | 32 | void odometry_callback(const nav_msgs::Odometry::ConstPtr& msg) 33 | { 34 | // retrieve pose 35 | odometry_.x = msg->pose.pose.position.x; 36 | odometry_.y = msg->pose.pose.position.y; 37 | odometry_.yaw = get_yaw_from_quaternion(msg->pose.pose.orientation); 38 | 39 | // retrieve speeds 40 | odometry_.v = msg->twist.twist.linear.x; 41 | odometry_.omega = msg->twist.twist.angular.z; 42 | 43 | odo_received_ = true; 44 | } 45 | 46 | void trajectory_callback(const nav_msgs::Path::ConstPtr& msg) 47 | { 48 | trajectory_ = std::vector(msg->poses.size()); 49 | 50 | for(auto i = 0; i < msg->poses.size(); ++i) 51 | { 52 | auto pose = msg->poses[i]; 53 | trajectory_[i] = Pose2D{pose.pose.position.x, pose.pose.position.y, get_yaw_from_quaternion(pose.pose.orientation)}; 54 | } 55 | } 56 | 57 | double car_x() const { return odometry_.x; } 58 | 59 | double evaluate() 60 | { 61 | //// return early if not enough info 62 | if( trajectory_.size() == 0 || ! odo_received_ ) 63 | { 64 | return 0.0; 65 | } 66 | 67 | // get 0-0 68 | Pose2D current = {odometry_.x, odometry_.y, odometry_.yaw}; 69 | 70 | // project on trajectory 71 | int index = -1; 72 | double mu = -1; 73 | 74 | const auto projected = project_on_trajectory(current, trajectory_, index, mu); 75 | 76 | //std::cout << "index:" << index << std::endl; 77 | 78 | if(index == -1 || index == 0) 79 | { 80 | ROS_WARN_STREAM("wrong projection"); 81 | return 0.0; 82 | } 83 | 84 | update_komo(trajectory_[index - 1], komo_->configurations(0)); 85 | update_komo(trajectory_[index], komo_->configurations(1)); 86 | update_komo(trajectory_[index + 1], komo_->configurations(2)); 87 | 88 | auto Gs = get_traj_start(komo_->configurations, 0, 2); 89 | auto cost = traj_cost(Gs, {objectives_.acc_, objectives_.ax_, objectives_.vel_}); 90 | 91 | 92 | static int n = 0; 93 | ++n; 94 | 95 | if(n >=100) 96 | cost_evaluator_.acc(cost.total); 97 | 98 | return cost_evaluator_.average(); 99 | } 100 | 101 | void update_komo(const Pose2D & pose, rai::KinematicWorld* kin) const 102 | { 103 | kin->q(0) = pose.x; 104 | kin->q(1) = pose.y; 105 | kin->q(2) = pose.yaw; 106 | 107 | kin->calc_Q_from_q(); 108 | kin->calc_fwdPropagateFrames(); 109 | } 110 | 111 | private: 112 | // state 113 | bool odo_received_; 114 | OdometryState odometry_; 115 | std::vector trajectory_; 116 | KomoFactory komo_factory_; 117 | std::shared_ptr< KOMO > komo_; 118 | Objectives objectives_; 119 | Evaluator cost_evaluator_; 120 | 121 | // params 122 | const int steps_per_phase_; 123 | }; 124 | } 125 | 126 | int main(int argc, char **argv) 127 | { 128 | ROS_INFO_STREAM("Launch evaluation node.."); 129 | 130 | int steps_per_phase = 1; 131 | int trajectory_index = 0; 132 | double road_width = 3.5; 133 | double v_desired = 10; 134 | 135 | // ros init 136 | ros::init(argc, argv, "evaluation"); 137 | ros::NodeHandle n; 138 | n.getParam("/traj_planner/steps_per_phase", steps_per_phase); 139 | n.getParam("/traj_controller/trajectory_index", trajectory_index); 140 | n.getParam("/road_width", road_width); 141 | n.getParam("/v_desired", v_desired); 142 | 143 | // logging 144 | std::ofstream ofs(filename(n)); 145 | 146 | // evaluation 147 | TrajEvaluator evaluator(steps_per_phase, road_width, v_desired); 148 | 149 | boost::function trajectory_callback = 150 | boost::bind(&TrajEvaluator::trajectory_callback, &evaluator, _1); 151 | auto trajectory_subscriber = n.subscribe("/traj_planner/trajectory_" + std::to_string(trajectory_index), 100, trajectory_callback); 152 | 153 | boost::function odometry_callback = 154 | boost::bind(&TrajEvaluator::odometry_callback, &evaluator, _1); 155 | auto odo_subscriber = n.subscribe("/lgp_car/odometry", 1000, odometry_callback); 156 | 157 | ros::Rate loop_rate(10); 158 | 159 | int i = 0; 160 | while (ros::ok()) 161 | { 162 | auto avg = evaluator.evaluate(); 163 | auto car_x = evaluator.car_x(); 164 | 165 | ros::spinOnce(); 166 | 167 | loop_rate.sleep(); 168 | 169 | // logging 170 | ++i; 171 | if(i && i%100 == 0) 172 | { 173 | ROS_INFO_STREAM("[cost evaluation] cost:" << avg << " " << i << " evaluations :)"); 174 | 175 | if(i%100 == 0) 176 | log_to_file(ofs, n, car_x, i, avg); 177 | } 178 | } 179 | 180 | return 0; 181 | } 182 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/nodes/nodes/obstacle_avoidance_tree_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | //typedef ObstacleAvoidanceTree BehaviorType; 11 | typedef ObstacleAvoidanceDec BehaviorType; 12 | 13 | int main(int argc, char **argv) 14 | { 15 | ROS_INFO_STREAM("Launch lgp car control, obstacle avoidance.."); 16 | 17 | int steps_per_phase = 1; 18 | double p_obstacle = 0.1; 19 | int n_obstacles = 1; 20 | double road_width = 3.5; 21 | double v_desired = 10; 22 | bool tree = true; 23 | 24 | // ros init 25 | ros::init(argc, argv, "lgp_car_traj_planner"); 26 | ros::NodeHandle n; 27 | tf::TransformListener tf_listener; 28 | 29 | n.getParam("/traj_planner/steps_per_phase", steps_per_phase); 30 | n.getParam("p_obstacle", p_obstacle); 31 | n.getParam("n_obstacles", n_obstacles); 32 | n.getParam("road_width", road_width); 33 | n.getParam("v_desired", v_desired); 34 | n.getParam("tree", tree); 35 | 36 | std::vector trajectory_publishers; 37 | for(auto i = 0; i < BehaviorType::n_branches(n_obstacles, tree); ++i) 38 | { 39 | trajectory_publishers.push_back( 40 | n.advertise("/traj_planner/trajectory_" + std::to_string(i + 1), 1000) 41 | ); 42 | } 43 | 44 | // transformed trajectories 45 | // std::vector transformed_trajectory_publishers; 46 | // for(auto i = 0; i < BehaviorType::n_branches(n_obstacles, tree); ++i) 47 | // { 48 | // transformed_trajectory_publishers.push_back( 49 | // n.advertise("/traj_planner/transformed_trajectory_" + std::to_string(i + 1), 1000) 50 | // ); 51 | // } 52 | // 53 | ros::Publisher road_publisher = n.advertise("/environment/road_model_array", 1000); 54 | 55 | BehaviorManager manager; 56 | 57 | // instanciate behaviors 58 | auto obstacle_avoidance_tree = std::shared_ptr(new BehaviorType(manager, n_obstacles, tree, road_width, v_desired, steps_per_phase)); 59 | manager.register_behavior("ObstacleAvoidanceTree", obstacle_avoidance_tree); 60 | manager.set_current_behavior("ObstacleAvoidanceTree"); 61 | 62 | boost::function odometry_callback = 63 | boost::bind(&BehaviorManager::odometry_callback, &manager, _1); 64 | auto odo_subscriber = n.subscribe("/lgp_car/odometry", 1000, odometry_callback); 65 | 66 | // connect behaviors 67 | boost::function speed_callback_tree = 68 | boost::bind(&BehaviorType::desired_speed_callback, obstacle_avoidance_tree.get(), _1); 69 | 70 | boost::function obstacle_callback_tree = 71 | boost::bind(&BehaviorType::obstacle_callback, obstacle_avoidance_tree.get(), _1); 72 | 73 | auto speed_tree = n.subscribe("/gui_control/lgp_car/desired_speed", 1000, speed_callback_tree); 74 | auto obstacle_tree = n.subscribe("/lgp_obstacle_belief/marker_array", 1000, obstacle_callback_tree); 75 | 76 | ros::Rate loop_rate(10); 77 | 78 | std::ofstream ofs(filename(n)); 79 | 80 | double car_x = 0; 81 | int i = 0; 82 | while (ros::ok()) 83 | { 84 | manager.plan(); 85 | 86 | std::vector trajectories = manager.get_trajectories(); 87 | for(auto i = 0; i < trajectories.size(); ++i) 88 | { 89 | trajectory_publishers[i].publish(trajectories[i]); 90 | } 91 | 92 | // transform trajectories 93 | // std::vector transform_trajectories = transform(trajectories, 4.0); 94 | // for(auto i = 0; i < transform_trajectories.size(); ++i) 95 | // { 96 | // transformed_trajectory_publishers[i].publish(transform_trajectories[i]); 97 | // } 98 | // 99 | 100 | if((i - 10) % 50 == 0) 101 | { 102 | try 103 | { 104 | tf::StampedTransform transform; 105 | tf_listener.lookupTransform("/map", "/lgp_car", ros::Time(0), transform); 106 | 107 | car_x = transform(tf::Vector3(0,0,0)).x(); 108 | 109 | auto markers = RoadModelBuilder(car_x, road_width).add_center_line().add_road_border().build(); 110 | road_publisher.publish(markers); 111 | } 112 | catch (tf::TransformException ex) 113 | { 114 | --i; 115 | } 116 | } 117 | ros::spinOnce(); 118 | 119 | ++i; 120 | 121 | loop_rate.sleep(); 122 | 123 | if(i%100==0) 124 | { 125 | //log_to_file(ofs, n, car_x, i, manager.cost()); 126 | //ROS_INFO_STREAM("cost:" << manager.cost() << " time:" << manager.planning_time()); 127 | } 128 | 129 | n.setParam("/planning_time", manager.planning_time()); 130 | } 131 | 132 | return 0; 133 | } 134 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/nodes/nodes/obstacle_common.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | void log_to_file(std::ofstream & ofs, ros::NodeHandle & n, double car_x, int i, double cost) 12 | { 13 | int n_obstacles; 14 | int n_non_obstacles; 15 | double p_obstacle; 16 | double planning_time = 0; 17 | 18 | n.getParam("/n_total_obstacles", n_obstacles); 19 | n.getParam("/n_total_non_obstacles", n_non_obstacles); 20 | n.getParam("/p_obstacle", p_obstacle); 21 | n.getParam("/planning_time", planning_time); 22 | 23 | std::stringstream ss; 24 | 25 | ss << "N:" << n_obstacles << " M:" << n_non_obstacles << " M+N:"<< n_obstacles + n_non_obstacles << " ~p:" << double(n_obstacles) / (n_non_obstacles+n_obstacles) << " Travelled dist:" << car_x << " Probability:" << p_obstacle << " Avg cost:" << cost << " time(ms):" << planning_time * 1000 << " " << i << " iterations"; 26 | 27 | ROS_INFO_STREAM(ss.str()); 28 | ofs << ss.str() << std::endl; 29 | } 30 | 31 | std::string filename(ros::NodeHandle & n) 32 | { 33 | double p_obstacle = 0.1; 34 | int n_obstacles = 1; 35 | double certainty_distance_offset = 10.0; 36 | bool tree = true; 37 | 38 | n.getParam("tree", tree); 39 | n.getParam("p_obstacle", p_obstacle); 40 | n.getParam("n_obstacles", n_obstacles); 41 | n.getParam("certainty_distance_offset", certainty_distance_offset); 42 | n.getParam("tree", tree); 43 | n.getParam("tree", tree); 44 | 45 | auto now = std::chrono::system_clock::now(); 46 | std::time_t t = std::chrono::system_clock::to_time_t(now); 47 | std::stringstream ss; 48 | ss << "/home/camille/Phd/Paper/ICRA-2021/plots/gen_obstacles/data-" << std::to_string(p_obstacle) << "-" << (tree ? "tree" : "linear") << "-" << n_obstacles << "-" << "-offset-" << certainty_distance_offset << "-" << t << ".txt"; 49 | return ss.str(); 50 | } 51 | 52 | visualization_msgs::Marker create_obstacle_marker(double x, double y, double sx, double sy, double sz, double alpha, int id) 53 | { 54 | visualization_msgs::Marker marker; 55 | 56 | marker.header.frame_id = "map"; 57 | marker.id = std::hash()("obstacle_" + std::to_string(id)); 58 | marker.type = visualization_msgs::Marker::CUBE; 59 | marker.action = visualization_msgs::Marker::ADD; 60 | marker.pose.position.x = x; 61 | marker.pose.position.y = y; 62 | marker.pose.position.z = 0.5 * sz; 63 | marker.scale.x = sx; // diameter 64 | marker.scale.y = sy; 65 | marker.scale.z = sz; 66 | marker.pose.orientation = get_quaternion_from_yaw(x); 67 | marker.color.a = alpha > 0.01 ? alpha : 0.0; 68 | marker.color.r = 1.0; 69 | marker.color.g = 0.0; 70 | marker.color.b = 0.0; 71 | 72 | return marker; 73 | } 74 | 75 | visualization_msgs::Marker create_collision_marker(double x, double y, double sx, double sy, double sz, double alpha, int id) 76 | { 77 | visualization_msgs::Marker marker; 78 | 79 | const double m = 0.5; // margin 80 | double d = sx * sx / (8 * m) - sy / 2 + m / 2; 81 | double diameter = 2 * (d + sy / 2 + m); 82 | 83 | marker.header.frame_id = "map"; 84 | marker.id = std::hash()("collision_" + std::to_string(id)); 85 | marker.type = visualization_msgs::Marker::CYLINDER; 86 | marker.action = visualization_msgs::Marker::ADD; 87 | marker.pose.position.x = x; 88 | marker.pose.position.y = y + (y > 0 ? d : -d); 89 | marker.pose.position.z = 0.5 * 0.01; 90 | marker.scale.x = diameter; // diameter 91 | marker.scale.y = diameter; 92 | marker.scale.z = 0.01; 93 | marker.color.a = alpha > 0.01 ? alpha : 0.0; //(use by optimization) 94 | marker.color.r = 1.0; 95 | marker.color.g = 0.0; 96 | marker.color.b = 0.0; 97 | 98 | return marker; 99 | } 100 | 101 | //visualization_msgs::Marker create_ellipsoid_collision_marker(double x, double y, double sx, double sy, double sz, double alpha, int id) 102 | //{ 103 | // visualization_msgs::Marker marker; 104 | 105 | // const double m = 0.5; // margin 106 | // double d = sx * sx / (8 * m) - sy / 2 + m / 2; 107 | // double diameter = 2 * (d + sy / 2 + m); 108 | 109 | // marker.header.frame_id = "map"; 110 | // marker.id = std::hash()("collision_" + std::to_string(id)); 111 | // marker.type = visualization_msgs::Marker::CYLINDER; 112 | // marker.action = visualization_msgs::Marker::ADD; 113 | // marker.pose.position.x = x; 114 | // marker.pose.position.y = y + (y > 0 ? d : -d); 115 | // marker.pose.position.z = 0.5 * 0.01; 116 | // marker.scale.x = diameter; // diameter 117 | // marker.scale.y = diameter; 118 | // marker.scale.z = 0.01; 119 | // marker.color.a = alpha > 0.01 ? alpha : 0.0; 120 | // marker.color.r = 1.0; 121 | // marker.color.g = 0.0; 122 | // marker.color.b = 0.0; 123 | 124 | // //std::cout << "diameter:" << diameter << std::endl; 125 | 126 | // return marker; 127 | //} 128 | 129 | //nav_msgs::Path transform(const nav_msgs::Path & trajectory, double tx) 130 | //{ 131 | // nav_msgs::Path transformed; 132 | // transformed.header = trajectory.header; 133 | 134 | // for(auto i = 0; i < trajectory.poses.size(); ++i) 135 | // { 136 | // const auto pose = trajectory.poses[i]; 137 | // const auto x = pose.pose.position.x; 138 | // const auto y = pose.pose.position.y; 139 | // const auto yaw = get_yaw_from_quaternion(pose.pose.orientation); 140 | 141 | // geometry_msgs::PoseStamped transformed_pose; 142 | // //transformed_pose.header = pose.header; 143 | // transformed_pose.pose.position.x = x + tx * cos(yaw); 144 | // transformed_pose.pose.position.y = y + tx * sin(yaw); 145 | // transformed_pose.pose.orientation = pose.pose.orientation; 146 | 147 | // transformed.poses.push_back(transformed_pose); 148 | // } 149 | 150 | // return transformed; 151 | //} 152 | 153 | //std::vector transform(const std::vector & trajectories, double tx) 154 | //{ 155 | // std::vector transformed; 156 | // transformed.reserve(trajectories.size()); 157 | 158 | // for(const auto & trajectory: trajectories) 159 | // { 160 | // transformed.push_back(transform(trajectory, tx)); 161 | // } 162 | 163 | // return transformed; 164 | //} 165 | -------------------------------------------------------------------------------- /control_tree_car/applications/obstacles/nodes/nodes/obstacle_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | void log_to_file(std::ofstream & ofs, ros::NodeHandle & n, double car_x, int i, double cost); 7 | 8 | std::string filename(ros::NodeHandle & n); 9 | 10 | visualization_msgs::Marker create_obstacle_marker(double x, double y, double sx, double sy, double sz, double alpha, int id); 11 | visualization_msgs::Marker create_collision_marker(double x, double y, double sx, double sy, double sz, double alpha, int id); 12 | //visualization_msgs::Marker create_parked_car_collision_marker(double x, double y, double sx, double sy, double sz, double alpha, int id); 13 | //std::vector transform(const std::vector & trajectories, double tx); 14 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | add_subdirectory(qp) 4 | add_subdirectory(nodes) 5 | add_subdirectory(test) 6 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/nodes/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | get_filename_component(project_name ${CMAKE_CURRENT_SOURCE_DIR} NAME) 4 | project(${project_name}) 5 | 6 | ################################## 7 | # Locate depedencies 8 | ################################## 9 | find_package(Boost 1.40.0 COMPONENTS filesystem system iostreams REQUIRED) 10 | find_package(CGAL) 11 | find_package(osqp REQUIRED) 12 | 13 | ################################# 14 | # Define executable 15 | ################################# 16 | add_definitions(-O3 -Wall -DRAI_LAPACK -DRAI_EIGEN -DRAI_PTHREAD -Wno-terminate -fPIC -std=c++14) 17 | 18 | # Ros common utilities 19 | file(GLOB ros_common_src "nodes/pedestrian_common.h" "nodes/pedestrian_common.cpp") 20 | add_library(ros_common_lib STATIC ${ros_common_src}) 21 | target_include_directories(ros_common_lib PUBLIC ".") 22 | target_link_libraries(ros_common_lib 23 | ${catkin_LIBRARIES} 24 | ) 25 | 26 | # Pedestrian 27 | add_executable(pedestrian_tree_qp_node nodes/pedestrian_tree_qp_node.cpp) 28 | target_link_libraries(pedestrian_tree_qp_node 29 | qp 30 | common 31 | ros_common_lib 32 | ) 33 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/nodes/nodes/pedestrian_common.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | void log_to_file(std::ofstream & ofs, ros::NodeHandle & n, double car_x, int i, double cost, double velocity, double time) 11 | { 12 | int n_crossings, n_non_crossings; 13 | n.getParam("/n_crossings", n_crossings); 14 | n.getParam("/n_non_crossings", n_non_crossings); 15 | 16 | double p_crossing; 17 | n.getParam("p_crossing", p_crossing); 18 | 19 | std::stringstream ss; 20 | 21 | ss << "N:" << n_crossings << " M:" << n_non_crossings << " M+N:" << n_crossings + n_non_crossings << " ~p:" << double(n_crossings) / (n_crossings + n_non_crossings) << " Avg dist between pedestrians:" << car_x / (n_non_crossings + n_crossings) << " Probability:" << p_crossing << " TREE QP avg cost:" << cost << " velocity(m/s):" << velocity << " time(ms):" << time * 1000 << " " << i << " iterations"; 22 | 23 | ROS_INFO_STREAM(ss.str()); 24 | ofs << ss.str() << std::endl; 25 | } 26 | 27 | std::string filename(const std::string & name, double p_crossing, int n_pedestrians, int n_branches) 28 | { 29 | auto now = std::chrono::system_clock::now(); 30 | std::time_t t = std::chrono::system_clock::to_time_t(now); 31 | std::stringstream ss; 32 | ss << "/home/camille/Phd/Paper/ICRA-2021/plots/gen/data-" << std::to_string(p_crossing) << "-" << name << "-" << std::to_string(n_pedestrians) << "-" << std::to_string(n_branches) << "-" << t << ".txt"; 33 | return ss.str(); 34 | } 35 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/nodes/nodes/pedestrian_common.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | void log_to_file(std::ofstream & ofs, ros::NodeHandle & n, double car_x, int i, double cost, double velocity, double time); 7 | 8 | std::string filename(const std::string & name, double p_crossing, int n_pedestrians, int n_branches); 9 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/nodes/nodes/pedestrian_tree_qp_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) 12 | { 13 | ROS_INFO_STREAM("Launch lgp car control, pedestrian tree.."); 14 | 15 | int steps_per_phase = 1; 16 | int n_branches = 2; 17 | int n_pedestrians = 1; 18 | double p_crossing = 0; 19 | double road_width = 3.5; 20 | 21 | // ros init 22 | ros::init(argc, argv, "lgp_car_pedestrian_qp_tree_planner"); 23 | ros::NodeHandle n; 24 | 25 | n.getParam("n_branches", n_branches); 26 | n.getParam("n_pedestrians", n_pedestrians); 27 | n.getParam("p_crossing", p_crossing); 28 | n.getParam("road_width", road_width); 29 | 30 | 31 | tf::TransformListener tf_listener; 32 | n.getParam("/traj_planner/steps_per_phase", steps_per_phase); 33 | 34 | std::vector traj_publishers; 35 | for(auto i = 0; i < n_branches; ++i) 36 | { 37 | const std::string name = "/traj_planner/trajectory_1" + std::to_string(i+1); 38 | traj_publishers.push_back(n.advertise(name, 1000)); 39 | } 40 | //ros::Publisher ctrl_publisher = n.advertise("/lgp_car/vel_cmd", 1000); 41 | ros::Publisher road_publisher = n.advertise("/environment/road_model_array", 1000); 42 | 43 | BehaviorManager manager; 44 | 45 | // instanciate behaviors 46 | auto stopline = std::shared_ptr(new StopLineQPTree(manager, n_branches, steps_per_phase)); 47 | manager.register_behavior("StopLineTree", stopline); 48 | manager.set_current_behavior("StopLineTree"); 49 | 50 | boost::function odometry_callback = 51 | boost::bind(&BehaviorManager::odometry_callback, &manager, _1); 52 | auto odo_subscriber = n.subscribe("/lgp_car/odometry", 1000, odometry_callback); 53 | 54 | // connect behaviors 55 | boost::function speed_callback = 56 | boost::bind(&StopLineQPTree::desired_speed_callback, stopline.get(), _1); 57 | 58 | boost::function stopline_callback = 59 | boost::bind(&StopLineQPTree::stopline_callback, stopline.get(), _1); 60 | 61 | auto speed = n.subscribe("/gui_control/lgp_car/desired_speed", 1000, speed_callback); 62 | auto stopline_sub = n.subscribe("/lgp_pedestrian_belief/marker_array", 1000, stopline_callback); 63 | 64 | ros::Rate loop_rate(10); 65 | 66 | std::ofstream ofs(filename("tree", p_crossing, n_pedestrians, n_branches)); 67 | 68 | double car_x = 0; 69 | int i = 0; 70 | while (ros::ok()) 71 | { 72 | manager.plan(); 73 | 74 | const std::vector trajectories = manager.get_trajectories(); 75 | 76 | for(auto i = 0; i < n_branches; ++i) 77 | { 78 | traj_publishers[i].publish(trajectories[i]); 79 | } 80 | //ctrl_publisher.publish(manager.get_ctrl()); 81 | 82 | if((i - 10) % 50 == 0) 83 | { 84 | try 85 | { 86 | tf::StampedTransform transform; 87 | tf_listener.lookupTransform("/map", "/lgp_car", ros::Time(0), transform); 88 | 89 | car_x = transform(tf::Vector3(0,0,0)).x(); 90 | 91 | auto markers = RoadModelBuilder(car_x, road_width).add_road_border().build(); 92 | road_publisher.publish(markers); 93 | } 94 | catch (tf::TransformException ex) 95 | { 96 | --i; 97 | } 98 | } 99 | ros::spinOnce(); 100 | 101 | ++i; 102 | 103 | loop_rate.sleep(); 104 | 105 | // 106 | if(i%100==0) 107 | { 108 | log_to_file(ofs, n, car_x, i, manager.cost(), manager.velocity(), manager.planning_time()); 109 | } 110 | } 111 | 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | get_filename_component(project_name ${CMAKE_CURRENT_SOURCE_DIR} NAME) 4 | project(${project_name}) 5 | 6 | ################################## 7 | # Locate depedencies 8 | ################################## 9 | find_package(osqp REQUIRED) 10 | find_package(Boost 1.40.0 COMPONENTS filesystem system iostreams REQUIRED) 11 | 12 | ################################# 13 | # Define executable 14 | ################################# 15 | add_definitions(-O3 -Wall -DRAI_LAPACK -DRAI_EIGEN -DRAI_PTHREAD -Wno-terminate -fPIC -std=c++14) 16 | 17 | file(GLOB_RECURSE SOURCES *.cpp *.h *.tpp) 18 | 19 | add_library(${project_name} STATIC ${SOURCES}) 20 | target_include_directories(${project_name} PUBLIC ".") 21 | target_link_libraries(${project_name} 22 | ${catkin_LIBRARIES} 23 | ${Boost_LIBRARIES} 24 | common 25 | osqp::osqp 26 | Core 27 | Optim 28 | Optimization 29 | pthread 30 | ) 31 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/MPC_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace Eigen; 8 | using IntA = std::vector; 9 | using Arr = std::vector; 10 | 11 | struct MPC_model 12 | { 13 | MPC_model(double dt, double Q_v_weight, double R_u_weight); 14 | 15 | VectorXd predict_trajectory(const Vector2d & x0, const VectorXd & U) const; 16 | VectorXd predict_trajectory(const Vector2d & x0, const VectorXd & U, const std::vector & varss) const; 17 | 18 | double cost(const Vector2d & x0, const VectorXd & U, const Vector2d & xd) const; // cost of full control sequence 19 | double cost(const Vector2d & x, double u, const Vector2d & xd) const; // cost of single step 20 | 21 | VectorXd check_constraints(const Vector2d & x0, const VectorXd & U, const Vector2d & xmax) const; 22 | 23 | VectorXd get_Xd(const Vector2d & xd, int n_steps) const; 24 | 25 | MatrixXd get_Q_bar(int n_steps) const; 26 | MatrixXd get_Q_bar(int n_steps, const std::vector & varss, const std::vector & scaless) const; 27 | 28 | MatrixXd get_R_bar(int n_steps) const; 29 | MatrixXd get_R_bar(int n_steps, const std::vector & varss, const std::vector & scaless) const; 30 | 31 | MatrixXd get_S(int n_steps) const; 32 | MatrixXd get_S(int n_steps, const std::vector & varss) const; 33 | 34 | MatrixXd get_T(int n_steps) const; 35 | MatrixXd get_T(int n_steps, const std::vector & varss) const; 36 | 37 | int get_dim() const { return 1; } 38 | 39 | Matrix2d A; // system model 40 | Vector2d B; // system model 41 | 42 | Matrix2d Q; // state costs 43 | Matrix< double, 1, 1> R; // control costs 44 | }; 45 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_constraints.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | bool Constraints::validate() const 4 | { 5 | return varss.size() >= xmaxs.size(); 6 | } 7 | 8 | MatrixXd Constraints::getSextract() const 9 | { 10 | // collect active constraints 11 | std::list< int > active_rows; 12 | std::vector rows_activity_flag(2 * n_steps); 13 | 14 | for(uint c = 0; c < xmaxs.size(); ++c) 15 | { 16 | const auto& mask = std::get<2>(xmaxs[c]); 17 | const auto& indices = std::get<3>(xmaxs[c]); 18 | 19 | for(auto i: indices) 20 | { 21 | if(!rows_activity_flag[2*i] && mask(0)) 22 | { 23 | rows_activity_flag[2*i] = 1; 24 | active_rows.push_back(2*i); 25 | } 26 | 27 | if(!rows_activity_flag[2*i+1] && mask(1)) 28 | { 29 | rows_activity_flag[2*i+1] = 1; 30 | active_rows.push_back(2*i+1); 31 | } 32 | } 33 | } 34 | 35 | MatrixXd Sextract = Eigen::MatrixXd::Zero(active_rows.size(), 2 * n_steps); 36 | 37 | int I = 0; 38 | for(auto i : active_rows) 39 | { 40 | Sextract(I, i) = 1; 41 | ++I; 42 | } 43 | 44 | return Sextract; 45 | } 46 | 47 | VectorXd Constraints::getXmax() const 48 | { 49 | MatrixXd Xmax = Eigen::VectorXd::Constant(2 * n_steps, std::numeric_limits::max()); 50 | 51 | for(uint c = 0; c < xmaxs.size(); ++c) 52 | { 53 | const auto& xmax = std::get<1>(xmaxs[c]); 54 | const auto& mask = std::get<2>(xmaxs[c]); 55 | const auto& indices = std::get<3>(xmaxs[c]); 56 | 57 | for(auto i: indices) 58 | { 59 | if(mask(0)) 60 | { 61 | Xmax(2*i) = std::min(Xmax(2*i), xmax(0)); 62 | } 63 | 64 | if(mask(1)) 65 | { 66 | Xmax(2*i+1) = std::min(Xmax(2*i+1), xmax(1)); 67 | } 68 | } 69 | } 70 | 71 | // following is necessary?? 72 | for(uint i = 0; i < Xmax.rows(); ++i) 73 | { 74 | if(Xmax(i) == std::numeric_limits::max()) 75 | { 76 | Xmax(i) = 1.0; // put dummy value instead of max to avoid numerical instability 77 | } 78 | } 79 | 80 | return Xmax; 81 | } 82 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_constraints.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | struct Constraints // specific 2d state 6 | { 7 | Constraints(int n_steps, const std::vector & varss) 8 | : n_steps(n_steps) 9 | , varss(varss) 10 | { 11 | 12 | } 13 | 14 | void add_constraint(int branch, const Eigen::Vector2d & xmax, const Eigen::Vector2d & mask) 15 | { 16 | xmaxs.push_back(std::make_tuple(branch, xmax, mask, varss[branch])); 17 | } 18 | 19 | void add_constraint(int branch, const Eigen::Vector2d & xmax, const Eigen::Vector2d & mask, const IntA & indices_branch) 20 | { 21 | IntA indices_global(indices_branch.size()); 22 | 23 | for(uint i = 0; i < indices_branch.size(); ++i) // local -> global 24 | { 25 | int j = indices_branch[i]; 26 | if(j == -1) j = varss[branch].size() - 1; 27 | 28 | indices_global[i] = varss[branch][j]; 29 | } 30 | xmaxs.push_back(std::make_tuple(branch, xmax, mask, indices_global)); 31 | } 32 | 33 | static Constraints refined(const Constraints & constraints, int n_steps_per_phase) 34 | { 35 | // refine the varss 36 | auto refined_varss = std::vector(constraints.varss.size()); 37 | 38 | for(uint i = 0; i < constraints.varss.size(); ++i) 39 | { 40 | const auto & vars = constraints.varss[i]; 41 | 42 | IntA refined_vars; 43 | refined_vars.reserve(vars.size() * n_steps_per_phase); 44 | 45 | for(auto j = 0; j < vars.size(); ++j) 46 | { 47 | for(auto s = 0; s < n_steps_per_phase; ++s) 48 | { 49 | refined_vars.push_back(n_steps_per_phase*vars[j] + s); 50 | } 51 | } 52 | 53 | refined_varss[i] = refined_vars; 54 | } 55 | 56 | // refine the xmaxs 57 | auto refined_xmaxs = std::vector>(constraints.xmaxs.size()); 58 | 59 | for(uint i = 0; i < constraints.xmaxs.size(); ++i) 60 | { 61 | const auto & indices = std::get<3>(constraints.xmaxs[i]); 62 | 63 | IntA refined_indices; 64 | refined_indices.reserve(indices.size() * n_steps_per_phase); 65 | 66 | for(uint j = 0; j < indices.size(); ++j) 67 | { 68 | for(uint s = 0; s < n_steps_per_phase; ++s) 69 | { 70 | refined_indices.push_back(n_steps_per_phase*indices[j] + s); 71 | } 72 | } 73 | 74 | refined_xmaxs[i] = std::make_tuple( 75 | std::get<0>(constraints.xmaxs[i]), 76 | std::get<1>(constraints.xmaxs[i]), 77 | std::get<2>(constraints.xmaxs[i]), 78 | refined_indices); 79 | } 80 | 81 | Constraints refined_constraints(constraints.n_steps * n_steps_per_phase, refined_varss); 82 | refined_constraints.xmaxs = refined_xmaxs; 83 | 84 | return refined_constraints; 85 | } 86 | 87 | MatrixXd getSextract() const; 88 | VectorXd getXmax() const; 89 | 90 | bool validate() const; 91 | 92 | int n_steps; 93 | 94 | std::vector varss; 95 | std::vector> xmaxs; // branch, max value and mask, global indices, branch indices, the values of masks should be 0 or 1 96 | }; 97 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_problem_DecQP.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | //----Joint------------------------// 5 | 6 | QP_tree_problem_JointQP::QP_tree_problem_JointQP(const MPC_model & mpc, double u_min, double u_max) 7 | : QP_tree_joint_solver_base(mpc, u_min, u_max) 8 | , options(PARALLEL, false, NOOPT, false) 9 | { 10 | options.opt.verbose = 0; 11 | } 12 | 13 | VectorXd QP_tree_problem_JointQP::call_solver() 14 | { 15 | const auto P = convert(H); 16 | const auto q = convert(C); 17 | const auto K = createK(KA); 18 | const auto u = createU(Up, Lo); 19 | 20 | auto qp = std::make_shared(P, q, K, u); 21 | 22 | std::vector> pbs; 23 | pbs.push_back(qp); 24 | 25 | arr x = zeros(P.d0); 26 | 27 | DecOptConstrained opt(x, pbs, {}, AverageUpdater(), options); 28 | 29 | opt.run(); 30 | 31 | return convert(x); 32 | } 33 | 34 | //----Dec---------------------------// 35 | 36 | QP_tree_problem_DecQP::QP_tree_problem_DecQP(const MPC_model & mpc, double u_min, double u_max) 37 | : QP_tree_solver_base(mpc, u_min, u_max) 38 | , options(PARALLEL, true, NOOPT, false) 39 | { 40 | options.opt.verbose = 0; 41 | options.opt.aulaMuInc = 1; 42 | } 43 | 44 | VectorXd QP_tree_problem_DecQP::solve(const Vector2d & x0, const Vector2d & xd, const Constraints& joint_k, 45 | int n_steps, 46 | const std::vector & joint_varss, 47 | const std::vector & joint_scaless) 48 | { 49 | // generate compressed var and masks 50 | IntA var, global_to_branch; 51 | const auto masks = get_compressed_masks(n_steps, mpc.get_dim(), joint_varss, var, global_to_branch); 52 | const auto bs = get_belief_state(joint_scaless); 53 | // const auto scales = get_one_scale(joint_scaless); 54 | const auto scaless = get_compressed_scales(joint_scaless); 55 | 56 | // compress constraints 57 | const auto ks = get_compressed_constraints(joint_k, var, global_to_branch); 58 | 59 | // build subproblems 60 | const auto branch_n_steps = var.size(); 61 | std::vector branch_varss({var}); 62 | std::vector> pbs; pbs.reserve(masks.size()); 63 | 64 | std::vector>> futures; 65 | 66 | futures.reserve(masks.size()); 67 | for(uint i = 0; i < masks.size(); ++i) 68 | { 69 | futures.push_back( 70 | std::async(std::launch::async, 71 | [&, i]() 72 | { 73 | return build_qp(i, branch_n_steps, branch_varss, {scaless[i]}/*{scales}*/, ks, x0, xd); 74 | } 75 | ) 76 | ); 77 | } 78 | 79 | for(auto& future: futures) 80 | { 81 | pbs.emplace_back(future.get()); 82 | } 83 | 84 | // solve 85 | arr x = zeros(n_steps * mpc.get_dim()); 86 | 87 | //DecOptConstrained opt(x, pbs, masks, BeliefState(bs), options); 88 | DecOptConstrained opt(x, pbs, masks, AverageUpdater(), options); 89 | 90 | opt.run(); 91 | 92 | return convert(x); 93 | } 94 | 95 | std::shared_ptr QP_tree_problem_DecQP::build_qp(int i, 96 | int n_steps, 97 | const std::vector& varss, 98 | const std::vector& scaless, 99 | const std::unordered_map & constraints, 100 | const Vector2d & x0, 101 | const Vector2d & xd) const 102 | { 103 | // build MPC matrices 104 | // costs (could be computed just once and scaled?) 105 | const auto S = mpc.get_S(n_steps, varss); 106 | const auto T = mpc.get_T(n_steps, varss); 107 | 108 | const auto Q_bar = mpc.get_Q_bar(n_steps, varss, scaless); 109 | const auto R_bar = mpc.get_R_bar(n_steps, varss, scaless); 110 | 111 | const auto H = 2 * (R_bar + S.transpose() * Q_bar * S); 112 | const auto F = 2 * (T.transpose() * Q_bar * S); 113 | const auto G = 2 * Q_bar * S; 114 | 115 | const Eigen::VectorXd & Xd = mpc.get_Xd(xd, n_steps); 116 | const auto C = (x0.transpose() * F).transpose() - (Xd.transpose() * G).transpose(); 117 | 118 | // constraints 119 | Eigen::MatrixXd KA; 120 | Eigen::VectorXd Up, Lo; 121 | 122 | const auto kit = constraints.find(i); 123 | if(kit!=constraints.end()) 124 | { 125 | const auto& k = kit->second; 126 | const Eigen::VectorXd Xmax = k.getXmax(); 127 | const Eigen::MatrixXd Sextract = k.getSextract(); 128 | 129 | const auto nk = Sextract.rows() + H.rows(); 130 | KA.resize(nk, H.rows()); 131 | Up.resize(nk); 132 | Lo.resize(nk); 133 | 134 | // traj constraints 135 | KA.block(0, 0, Sextract.rows(), H.rows()) = Sextract * S; 136 | Up.head(Sextract.rows()) = Sextract * (Xmax - T * x0); 137 | Lo.head(Up.rows()) = VectorXd::Constant(Up.rows(), std::numeric_limits::lowest()); 138 | 139 | // control bounds constraints 140 | KA.block(Sextract.rows(), 0, H.rows(), H.rows()) = MatrixXd::Identity(H.rows(), H.rows()); 141 | Up.tail(H.rows()) = VectorXd::Constant(H.rows(), u_max_); 142 | Lo.tail(H.rows()) = VectorXd::Constant(H.rows(), u_min_); 143 | } 144 | else // no traj constraints, add control bounds only 145 | { 146 | // control bounds constraints 147 | KA = MatrixXd::Identity(H.rows(), H.rows()); 148 | Up = VectorXd::Constant(H.rows(), u_max_); 149 | Lo = VectorXd::Constant(H.rows(), u_min_); 150 | } 151 | 152 | // build QP matrices 153 | const auto P = convert(H); 154 | const auto q = convert(C); 155 | const auto K = createK(KA); 156 | const auto u = createU(Up, Lo); 157 | 158 | return std::make_shared(P, q, K, u); 159 | } 160 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_problem_DecQP.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | 13 | /* 14 | * Use custom QP solver for solving the joint QP 15 | */ 16 | 17 | class QP_tree_problem_JointQP : public QP_tree_joint_solver_base 18 | { 19 | public: 20 | QP_tree_problem_JointQP(const MPC_model & mpc, 21 | double u_min, double u_max); 22 | 23 | private: 24 | VectorXd call_solver() override; 25 | 26 | DecOptConfig options; 27 | }; 28 | 29 | /* 30 | * Solve QP in decentralized fashion 31 | */ 32 | 33 | class QP_tree_problem_DecQP : public QP_tree_solver_base 34 | { 35 | public: 36 | QP_tree_problem_DecQP(const MPC_model & mpc, 37 | double u_min, double u_max); 38 | 39 | VectorXd solve(const Vector2d & x0, const Vector2d & xd, const Constraints & k, 40 | int n_steps, 41 | const std::vector & varss, 42 | const std::vector & scaless); 43 | 44 | private: 45 | /** 46 | * @brief build_qp 47 | * @param i branch id 48 | * @param n_steps number of steps on branch 49 | * @param varss local vars of the branch 50 | * @param scaless local scales of the branch 51 | * @param constraints mapping branch id -> constraints 52 | * @param x0 initial state 53 | * @param xd desired state 54 | * @return 55 | */ 56 | std::shared_ptr build_qp(int i, 57 | int n_steps, 58 | const std::vector& varss, 59 | const std::vector& scaless, 60 | const std::unordered_map & constraints, 61 | const Vector2d & x0, 62 | const Vector2d & xd 63 | ) const; 64 | 65 | DecOptConfig options; 66 | }; 67 | 68 | std::vector get_compressed_masks(int n_steps, int dim, const std::vector& varss, IntA& var, IntA& global_to_branch); 69 | std::vector get_compressed_scales(const std::vector& scaless); 70 | std::unordered_map get_compressed_constraints(const Constraints & k, const IntA& var, const IntA& global_to_branch); // branch to constraints 71 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_problem_DecQP_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | arr convert(const MatrixXd& M) 4 | { 5 | arr m(M.rows(), M.cols()); 6 | 7 | for(uint i = 0; i < M.rows(); ++i) 8 | { 9 | for(uint j = 0; j < M.cols(); ++j) 10 | { 11 | m(i, j) = M(i,j); 12 | } 13 | } 14 | 15 | return m; 16 | } 17 | 18 | VectorXd convert(const arr& v) 19 | { 20 | VectorXd V(v.d0); 21 | 22 | for(uint i = 0; i < V.rows(); ++i) 23 | { 24 | V(i) = v(i); 25 | } 26 | 27 | return V; 28 | } 29 | 30 | arr createK(const MatrixXd& KA) 31 | { 32 | arr K(2 * KA.rows(), KA.cols()); 33 | 34 | for(uint i = 0; i < KA.rows(); ++i) 35 | { 36 | for(uint j = 0; j < KA.cols(); ++j) 37 | { 38 | K(i, j) = KA(i,j); 39 | K(i + KA.rows(), j) = -KA(i,j); 40 | } 41 | } 42 | 43 | return K; 44 | } 45 | 46 | arr createU(const VectorXd& Up, const VectorXd& Lo) 47 | { 48 | arr u(2 * Up.rows()); 49 | 50 | for(uint i = 0; i < Up.rows(); ++i) 51 | { 52 | u(i) = Up[i]; 53 | u(i + Up.rows()) = -Lo[i]; 54 | } 55 | 56 | return u; 57 | } 58 | 59 | IntA to_local(const IntA& global_indices, const IntA& global_to_branch) 60 | { 61 | IntA local_indices(global_indices.size()); 62 | 63 | for(uint j = 0; j < global_indices.size(); ++j) 64 | { 65 | local_indices[j] = global_to_branch[global_indices[j]]; 66 | } 67 | 68 | return local_indices; 69 | } 70 | 71 | std::vector get_compressed_masks(int n_steps, int dim, const std::vector& varss, IntA& var, IntA& global_to_branch) 72 | { 73 | CHECK(!varss.empty() && var.empty(), "Preconditions not met"); 74 | 75 | var.resize(varss.front().size()); 76 | 77 | for(uint i = 0; i < var.size(); ++i) 78 | { 79 | var[i] = i; 80 | } 81 | 82 | std::vector masks(varss.size(), zeros(n_steps * dim)); 83 | global_to_branch = IntA(n_steps); 84 | 85 | for(uint i = 0; i < varss.size(); ++i) 86 | { 87 | const auto& vars = varss[i]; 88 | auto& mask = masks[i]; 89 | 90 | for(uint j = 0; j < vars.size(); ++j) 91 | { 92 | for(uint k = 0; k < dim; ++k) 93 | { 94 | mask(dim * vars[j] + k) = 1.0; 95 | global_to_branch[vars[j]] = j; 96 | } 97 | } 98 | } 99 | 100 | return masks; 101 | } 102 | 103 | std::vector get_compressed_scales(const std::vector& joint_scaless) 104 | { 105 | std::vector scaless(joint_scaless.size()); 106 | 107 | for(uint i = 0; i < joint_scaless.size(); ++i) 108 | { 109 | scaless[i] = Arr(joint_scaless[i].size(), joint_scaless[i].back()); 110 | } 111 | 112 | return scaless; 113 | } 114 | 115 | Arr get_one_scale(const std::vector& joint_scaless) 116 | { 117 | return Arr(joint_scaless.front().size(), 1.0); 118 | } 119 | 120 | arr get_belief_state(const std::vector& joint_scaless) 121 | { 122 | arr bs = zeros(joint_scaless.size()); 123 | 124 | for(uint i = 0; i < joint_scaless.size(); ++i) 125 | { 126 | bs(i) = joint_scaless[i].back(); 127 | } 128 | 129 | return bs; 130 | } 131 | 132 | std::unordered_map get_compressed_constraints(const Constraints & k, const IntA& var, const IntA& global_to_branch) 133 | { 134 | std::unordered_map constraints; 135 | constraints.reserve(k.xmaxs.size()); 136 | 137 | for(uint i = 0; i < k.xmaxs.size(); ++i) 138 | { 139 | const auto& c = k.xmaxs[i]; 140 | const auto& global_indices = std::get<3>(c); 141 | 142 | int branch = std::get<0>(c); 143 | 144 | // global to local 145 | const auto local_indices = to_local(global_indices, global_to_branch); 146 | 147 | auto cxmax = std::make_tuple( 148 | std::get<0>(c), 149 | std::get<1>(c), 150 | std::get<2>(c), 151 | local_indices); 152 | 153 | auto kit = constraints.find(branch); 154 | if(kit!=constraints.end()) 155 | { 156 | kit->second.xmaxs.push_back(cxmax); 157 | } 158 | else 159 | { 160 | Constraints constraint(var.size(), std::vector({var})); 161 | constraint.xmaxs = {cxmax}; 162 | constraints.insert(std::make_pair(branch, constraint)); 163 | } 164 | } 165 | 166 | return constraints; 167 | } 168 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_problem_DecQP_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | arr convert(const MatrixXd& M); 8 | VectorXd convert(const arr& v); 9 | arr createK(const MatrixXd& KA); 10 | arr createU(const VectorXd& Up, const VectorXd& Lo); 11 | IntA to_local(const IntA& global_indices, const IntA& global_to_branch); 12 | 13 | std::vector get_compressed_scales(const std::vector& scaless); 14 | Arr get_one_scale(const std::vector& scaless); 15 | std::vector get_compressed_masks(int n_steps, int dim, const std::vector& varss, IntA& var, IntA& global_to_branch); 16 | 17 | arr get_belief_state(const std::vector& scaless); 18 | std::unordered_map get_compressed_constraints(const Constraints & k, const IntA& var, const IntA& global_to_branch); // branch to constraints 19 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_problem_OSQP.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | csc * create_csc_matrix(const MatrixXd & M) 4 | { 5 | std::list> nz; 6 | std::list col_starts; 7 | 8 | int k = 0; 9 | for(uint J = 0; J < M.cols(); ++J) 10 | { 11 | col_starts.push_back(k); 12 | for(auto I = 0; I < M.rows(); ++I) 13 | { 14 | if(M(I, J)!=0) 15 | { 16 | nz.push_back(std::make_pair(I, J)); 17 | ++k; 18 | } 19 | } 20 | } 21 | 22 | c_int m = M.rows(); 23 | c_int n = M.cols(); 24 | c_int nzmax = nz.size(); 25 | 26 | c_float * x = new c_float[nzmax]; 27 | c_int * i = new c_int[nzmax]; 28 | c_int * p = new c_int[col_starts.size()+1]; 29 | 30 | k = 0; 31 | for(auto IJ : nz) 32 | { 33 | auto & I = IJ.first; 34 | auto & J = IJ.second; 35 | x[k] = M(I, J); 36 | i[k] = I; 37 | ++k; 38 | } 39 | 40 | int l = 0; 41 | for(auto cs : col_starts) 42 | { 43 | p[l] = cs; 44 | ++l; 45 | } 46 | 47 | p[col_starts.size()] = nzmax; 48 | 49 | return csc_matrix(m, n, nzmax, x, i, p); 50 | } 51 | 52 | static c_float * create_c_float_array(const VectorXd & C) 53 | { 54 | c_float * q = new c_float[C.rows()]; 55 | 56 | for(uint I = 0; I < C.rows(); ++I) 57 | { 58 | q[I] = C(I); 59 | } 60 | 61 | return q; 62 | } 63 | 64 | 65 | static c_float * create_default_c_float_array(int rows, double value) 66 | { 67 | c_float * q = new c_float[rows]; 68 | 69 | for(uint I = 0; I < rows; ++I) 70 | { 71 | q[I] = value; 72 | } 73 | 74 | return q; 75 | } 76 | 77 | QP_tree_problem_OSQP::QP_tree_problem_OSQP(const MPC_model & mpc, double u_min, double u_max) 78 | : QP_tree_joint_solver_base(mpc, u_min, u_max) 79 | { 80 | // Workspace structures 81 | settings = (OSQPSettings *)c_malloc(sizeof(OSQPSettings)); 82 | data = (OSQPData *)c_malloc(sizeof(OSQPData)); 83 | 84 | // Define solver settings 85 | if (settings) osqp_set_default_settings(settings); 86 | settings->verbose = false; 87 | // 88 | //settings->polish = true; 89 | //settings->eps_abs *= 0.01; 90 | //settings->eps_rel *= 0.01; 91 | } 92 | 93 | QP_tree_problem_OSQP::~QP_tree_problem_OSQP() 94 | { 95 | if (data) { 96 | if (data->P) c_free(data->P); 97 | if (data->q) c_free(data->q); 98 | if (data->A) c_free(data->A); 99 | if (data->l) c_free(data->l); 100 | if (data->u) c_free(data->u); 101 | c_free(data); 102 | } 103 | if (settings) c_free(settings); 104 | } 105 | 106 | VectorXd QP_tree_problem_OSQP::call_solver() 107 | { 108 | // P-H 109 | auto P = create_csc_matrix(H.triangularView()); 110 | 111 | // Q-C 112 | c_float * q = create_c_float_array(C); 113 | 114 | // A 115 | auto A = create_csc_matrix(KA); 116 | 117 | // l 118 | auto l = create_c_float_array(Lo); 119 | auto u = create_c_float_array(Up); 120 | 121 | // Exitflag 122 | c_int exitflag = 0; 123 | 124 | // Populate data 125 | if (data) 126 | { 127 | data->n = H.rows(); 128 | data->m = KA.rows(); 129 | data->P = P; 130 | data->q = q; 131 | data->A = A; 132 | data->l = l; 133 | data->u = u; 134 | } 135 | 136 | // Setup workspace 137 | exitflag = osqp_setup(&work, data, settings); 138 | 139 | // Solve Problem 140 | osqp_solve(work); 141 | 142 | // Get solution 143 | auto U_sol = Eigen::VectorXd(); 144 | if(strcmp(work->info->status, "solved")==0) 145 | { 146 | U_sol = Eigen::VectorXd(H.rows()); 147 | for(auto I = 0; I < H.rows();++I) 148 | U_sol[I] = work->solution->x[I]; 149 | } 150 | 151 | // Clean workspace 152 | osqp_cleanup(work); 153 | 154 | // c_free(data->q); 155 | // c_free(data->l); 156 | // c_free(data->u); 157 | 158 | return U_sol; 159 | } 160 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_problem_OSQP.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "osqp.h" 8 | 9 | csc * create_csc_matrix(const MatrixXd & M); 10 | 11 | class QP_tree_problem_OSQP : public QP_tree_joint_solver_base 12 | { 13 | public: 14 | QP_tree_problem_OSQP(const MPC_model & mpc, 15 | double u_min, double u_max); 16 | 17 | ~QP_tree_problem_OSQP(); 18 | 19 | private: 20 | VectorXd call_solver() override; 21 | 22 | private: 23 | OSQPWorkspace *work; 24 | OSQPSettings *settings; 25 | OSQPData *data; 26 | }; 27 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_solver_base.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | QP_tree_solver_base::QP_tree_solver_base(const MPC_model & mpc, double u_min, double u_max) 4 | : mpc(mpc) 5 | , u_min_(u_min) 6 | , u_max_(u_max) 7 | { 8 | 9 | } 10 | 11 | QP_tree_joint_solver_base::QP_tree_joint_solver_base(const MPC_model & mpc, double u_min, double u_max) 12 | : QP_tree_solver_base(mpc, u_min, u_max) 13 | , control_bounds_in_A_(true) 14 | { 15 | 16 | } 17 | 18 | VectorXd QP_tree_joint_solver_base::solve(const Vector2d & x0, const Vector2d & xd, const Constraints & k, 19 | int n_steps, 20 | const std::vector & varss, 21 | const std::vector & scaless) 22 | { 23 | // build matrices 24 | S = mpc.get_S(n_steps, varss); 25 | T = mpc.get_T(n_steps, varss); 26 | 27 | Q_bar = mpc.get_Q_bar(n_steps, varss, scaless); 28 | R_bar = mpc.get_R_bar(n_steps, varss, scaless); 29 | 30 | H = 2 * (R_bar + S.transpose() * Q_bar * S); 31 | F = 2 * (T.transpose() * Q_bar * S); 32 | G = 2 * Q_bar * S; 33 | 34 | const Eigen::VectorXd & Xd = mpc.get_Xd(xd, n_steps); 35 | C = (x0.transpose() * F).transpose() - (Xd.transpose() * G).transpose(); 36 | 37 | /// Constraints 38 | const Eigen::VectorXd Xmax = k.getXmax(); 39 | const Eigen::MatrixXd Sextract = k.getSextract(); 40 | 41 | if(!KA.rows()) 42 | { 43 | if(control_bounds_in_A_) 44 | { 45 | KA.resize(Sextract.rows() + H.rows(), H.rows()); 46 | Up.resize(Sextract.rows() + H.rows()); 47 | Lo.resize(Sextract.rows() + H.rows()); 48 | } 49 | else 50 | { 51 | KA.resize(Sextract.rows(), H.rows()); 52 | Up.resize(Sextract.rows()); 53 | Lo.resize(Sextract.rows()); 54 | } 55 | } 56 | 57 | // traj constraints 58 | KA.block(0, 0, Sextract.rows(), H.rows()) = Sextract * S; 59 | Up.head(Sextract.rows()) = Sextract * (Xmax - T * x0); 60 | Lo.head(Up.rows()) = VectorXd::Constant(Up.rows(), std::numeric_limits::lowest()); 61 | 62 | // control bounds constraints 63 | if(control_bounds_in_A_) 64 | { 65 | KA.block(Sextract.rows(), 0, H.rows(), H.rows()) = MatrixXd::Identity(H.rows(), H.rows()); 66 | Up.tail(H.rows()) = VectorXd::Constant(H.rows(), u_max_); 67 | Lo.tail(H.rows()) = VectorXd::Constant(H.rows(), u_min_); 68 | } 69 | 70 | // call solver 71 | return call_solver(); 72 | } 73 | 74 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/QP_tree_solver_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class QP_tree_solver_base 7 | { 8 | public: 9 | QP_tree_solver_base(const MPC_model & mpc, 10 | double u_min, double u_max); 11 | 12 | virtual VectorXd solve(const Vector2d & x0, const Vector2d & xd, const Constraints & k, 13 | int n_steps, 14 | const std::vector & varss, 15 | const std::vector & scaless) = 0; 16 | 17 | protected: 18 | const MPC_model & mpc; 19 | 20 | const double u_min_; 21 | const double u_max_; 22 | }; 23 | 24 | 25 | class QP_tree_joint_solver_base : public QP_tree_solver_base 26 | { 27 | public: 28 | QP_tree_joint_solver_base(const MPC_model & mpc, 29 | double u_min, double u_max); 30 | 31 | VectorXd solve(const Vector2d & x0, const Vector2d & xd, const Constraints & k, 32 | int n_steps, 33 | const std::vector & varss, 34 | const std::vector & scaless); 35 | 36 | MatrixXd get_H() const {return H;} 37 | MatrixXd get_KA() const {return KA;} 38 | 39 | private: 40 | virtual VectorXd call_solver() = 0; 41 | // VectorXd solve_unconstrained(const Vector2d x0, const Vector2d xd) 42 | // { 43 | // return -H.inverse() * F.transpose() * x0; 44 | // } 45 | 46 | protected: 47 | bool control_bounds_in_A_; 48 | 49 | MatrixXd Q_bar; // state cost 50 | MatrixXd R_bar; // control cost 51 | 52 | MatrixXd S; 53 | MatrixXd T; 54 | 55 | MatrixXd F; 56 | MatrixXd G; 57 | 58 | // final matrices used by the solver 59 | MatrixXd H; 60 | VectorXd C; 61 | MatrixXd KA; 62 | VectorXd Up; 63 | VectorXd Lo; 64 | }; 65 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/control_tree_plot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void plot(const std::function & value_provider, 4 | const std::vector & varss, 5 | const std::vector & scaless, 6 | const PlotAxis & axis) 7 | { 8 | TreePlot plotter(varss.size(), axis.name, axis.range); 9 | plotter.update(varss, scaless, value_provider); 10 | } 11 | 12 | void save(const std::function & value_provider, 13 | double dt, 14 | const std::vector & varss, 15 | const std::vector & scales, 16 | std::ostream & file) 17 | { 18 | for(const auto & vars : varss) 19 | { 20 | save(value_provider, dt, vars, file); 21 | } 22 | 23 | file << std::endl; 24 | } 25 | 26 | 27 | void save(const std::function & value_provider, 28 | double dt, 29 | const IntA & vars, std::ostream & file) 30 | { 31 | for(uint i=0; i & varss, 43 | const std::vector & scales, 44 | const std::function & value_provider) 45 | { 46 | auto color_code = [](int i, double p)->std::string 47 | { 48 | std::stringstream ss; 49 | ss << "#"; 50 | ss << std::hex << int((1.0 - p) * 255); 51 | ss << "00"; 52 | ss << "00"; 53 | ss << "FF"; 54 | 55 | return ss.str(); 56 | }; 57 | 58 | auto & gp = gp_; 59 | 60 | gp << "set title '" << name_ << "'\n"; 61 | gp << "set xrange [0:16]\nset yrange " << yrange_ << "\n"; 62 | gp << "plot "; 63 | 64 | for(uint i = 0; i < varss.size(); ++i) 65 | { 66 | gp << "'-' with lines title '" << name_ << "-" << i << "'"; 67 | gp << " lc rgb '"<< color_code(i, scales[i].back()) <<"'"; 68 | 69 | if(i < varss.size() - 1) 70 | gp << ","; 71 | } 72 | 73 | gp << "\n"; 74 | 75 | 76 | // data 77 | std::vector > > xy_pts_all; 78 | 79 | for(const auto& vars : varss) 80 | { 81 | std::vector > xy_pts; 82 | for(uint i=0; i 4 | 5 | #include "gnuplot-iostream.h" 6 | 7 | struct PlotAxis 8 | { 9 | std::string name; 10 | std::string range; 11 | }; 12 | 13 | void plot(const std::function & value_provider, 14 | const std::vector & varss, 15 | const std::vector & scales, 16 | const PlotAxis & axis); 17 | 18 | void save(const std::function & value_provider, 19 | double dt, 20 | const std::vector & varss, 21 | const std::vector & scales, std::ostream & file); 22 | 23 | void save(const std::function & value_provider, 24 | double dt, 25 | const IntA & vars, 26 | std::ostream & file); 27 | 28 | 29 | class TreePlot 30 | { 31 | public: 32 | TreePlot(int n_branches, const std::string & name, const std::string & yrange) 33 | : n_branches_(n_branches) 34 | , name_(name) 35 | , yrange_(yrange) 36 | { 37 | } 38 | 39 | void update(const std::vector & varss, 40 | const std::vector & scales, 41 | const std::function & value_provider); 42 | 43 | private: 44 | Gnuplot gp_; 45 | int n_branches_; 46 | std::string name_; 47 | std::string yrange_; 48 | }; 49 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/qp/qp/stopline_qp_tree.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include "ros/ros.h" 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | //typedef QP_tree_problem_OSQP QP_tree_problem_solver_type; 27 | //typedef QP_tree_problem_JointQP QP_tree_problem_solver_type; 28 | typedef QP_tree_problem_DecQP QP_tree_problem_solver_type; 29 | 30 | class OdometryState; 31 | 32 | struct Stopline 33 | { 34 | double x; 35 | double p; 36 | }; 37 | 38 | class StopLineQPTree : public BehaviorBase 39 | { 40 | public: 41 | StopLineQPTree(BehaviorManager&, int n_pedestrians, int steps_per_phase); 42 | 43 | void desired_speed_callback(const std_msgs::Float32::ConstPtr& msg); 44 | 45 | void stopline_callback(const visualization_msgs::MarkerArray::ConstPtr& msg); 46 | 47 | TimeCostPair plan(); 48 | 49 | bool validate_and_save_solution(const VectorXd & U, const OdometryState & o); 50 | 51 | std::vector get_trajectories(); 52 | 53 | private: 54 | void create_tree(); 55 | bool valid(const VectorXd & U, const VectorXd & X) const; 56 | void debug(const VectorXd & U, const OdometryState & o) const; 57 | 58 | private: 59 | // params 60 | const int n_branches_; 61 | const uint steps_; 62 | double u_min_{-8.0}; 63 | double u_max_{2.0}; 64 | 65 | // target: params than can be adapted 66 | double v_desired_; 67 | std::vector stoplines_; 68 | 69 | // tree 70 | std::shared_ptr tree_; 71 | MPC_model model_; 72 | QP_tree_problem_solver_type solver_; 73 | 74 | // results 75 | Vector2d x0_; 76 | VectorXd U_sol_; 77 | VectorXd X_sol_; 78 | 79 | // state; 80 | bool optimization_run_; 81 | bool optimization_error_; 82 | 83 | // plot 84 | TreePlot acc_plotter_; 85 | TreePlot vel_plotter_; 86 | }; 87 | 88 | VectorXd emergency_brake(const double v, const TreePb &, int steps_per_phase, double u); 89 | std::vector fuse_probabilities(const std::vector &, int n); // n = n_branches -1 90 | VectorXd emergency_brake(double v, int n_phases, int steps_per_phase, double u); 91 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.8) 2 | 3 | get_filename_component(project_name ${CMAKE_CURRENT_SOURCE_DIR} NAME) 4 | string(REPLACE " " "_" project_name ${project_name}) 5 | 6 | include(CTest) 7 | 8 | ################################## 9 | # Locate depedencies 10 | ################################## 11 | find_package(GTest REQUIRED) 12 | 13 | include_directories(${GTEST_INCLUDE_DIRS}) 14 | 15 | ################################## 16 | # Define executable 17 | ################################## 18 | # flags 19 | add_definitions(-O3 -Wall -DRAI_LAPACK -DRAI_EIGEN -DRAI_PTHREAD -Wno-terminate -fPIC -std=c++14) 20 | 21 | # utility test lib 22 | file(GLOB_RECURSE utility_lib_src common/*.cpp) 23 | add_library(utility_lib STATIC ${utility_lib_src}) 24 | target_link_libraries(utility_lib 25 | qp 26 | common 27 | pthread 28 | ) 29 | 30 | # list all tests 31 | file(GLOB_RECURSE tests test_*.cpp) 32 | 33 | # specify target for each test 34 | foreach(test ${tests}) 35 | get_filename_component(test_name ${test} NAME_WE) 36 | add_executable(${test_name} ${test}) 37 | target_link_libraries(${test_name} 38 | qp 39 | common 40 | utility_lib 41 | #komo_lib 42 | ros_common_lib 43 | gtest 44 | pthread 45 | ) 46 | add_test(NAME ${test_name} COMMAND ${test_name}) 47 | endforeach() 48 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/common/komo_problems_cpp: -------------------------------------------------------------------------------- 1 | #include "komo_problems.h" 2 | 3 | nav_msgs::Odometry::Ptr create_odo(double x, double y, double vx) 4 | { 5 | nav_msgs::Odometry::Ptr odo(new nav_msgs::Odometry()); 6 | odo->pose.pose.position.x = x; 7 | odo->pose.pose.position.y = y; 8 | odo->pose.pose.position.z = 0; 9 | 10 | odo->pose.pose.orientation.w = 1; 11 | odo->pose.pose.orientation.x = 0; 12 | odo->pose.pose.orientation.y = 0; 13 | odo->pose.pose.orientation.z = 0; 14 | 15 | odo->twist.twist.linear.x = vx; 16 | odo->twist.twist.linear.y = 0; 17 | odo->twist.twist.linear.z = 0; 18 | 19 | return odo; 20 | } 21 | 22 | std_msgs::Float32::Ptr create_desired_speed(double v) 23 | { 24 | std_msgs::Float32::Ptr desired_speed(new std_msgs::Float32()); 25 | desired_speed->data = 10; 26 | 27 | return desired_speed; 28 | } 29 | 30 | struct ObstaclesBuilder 31 | { 32 | ObstaclesBuilder() 33 | : obstacles(new visualization_msgs::MarkerArray()) 34 | { 35 | 36 | } 37 | 38 | ObstaclesBuilder& add(double x, double y, double alpha) 39 | { 40 | visualization_msgs::Marker obstacle; 41 | 42 | obstacle.pose.position.x = x; 43 | obstacle.pose.position.y = y; 44 | obstacle.scale.x = 2; 45 | obstacle.color.a = alpha; 46 | 47 | obstacles->markers.push_back(obstacle); // obstacle 48 | obstacles->markers.push_back(obstacle); // obstacle belief 49 | 50 | return *this; 51 | } 52 | 53 | visualization_msgs::MarkerArray::Ptr build() const { return obstacles;} 54 | 55 | visualization_msgs::MarkerArray::Ptr obstacles; 56 | }; 57 | 58 | Scenario create_scenario_1(double p) 59 | { 60 | const auto odo = create_odo(0, 0, 10); 61 | const auto desired_velocity = create_desired_speed(10); 62 | const auto obstacles = ObstaclesBuilder().add(20, 1, p).build(); 63 | 64 | return {odo, desired_velocity, obstacles}; 65 | } 66 | 67 | Scenario create_scenario_2() 68 | { 69 | const auto odo = create_odo(0, 0, 10); 70 | const auto desired_velocity = create_desired_speed(10); 71 | const auto obstacles = ObstaclesBuilder().add(15, 1, 0.02).add(22.5, 0.1, 0.02).build(); 72 | 73 | return {odo, desired_velocity, obstacles}; 74 | } 75 | 76 | Scenario create_scenario_3() 77 | { 78 | const auto odo = create_odo(0, 0, 10); 79 | const auto desired_velocity = create_desired_speed(10); 80 | const auto obstacles = ObstaclesBuilder().add(20, 1, 0.5).add(25, -1, 0.3).add(100, 1.0, 0.1).build(); 81 | 82 | return {odo, desired_velocity, obstacles}; 83 | } 84 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/common/komo_problems_h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | nav_msgs::Odometry::Ptr create_odo(double x, double y, double vx); 11 | std_msgs::Float32::Ptr create_desired_speed(double v); 12 | visualization_msgs::MarkerArray::Ptr create_obstacles(double x, double y, double alpha); 13 | 14 | struct Scenario 15 | { 16 | nav_msgs::Odometry::Ptr odo; 17 | std_msgs::Float32::Ptr desired_velocity; 18 | visualization_msgs::MarkerArray::Ptr obstacles; 19 | }; 20 | 21 | Scenario create_scenario_1(double p); 22 | Scenario create_scenario_2(); 23 | Scenario create_scenario_3(); 24 | 25 | template 26 | void plan_impl(const Scenario & scenario, BehaviorManager& manager, T& behavior, bool _plot, bool _plot_debug) 27 | { 28 | manager.odometry_callback(scenario.odo); 29 | behavior->desired_speed_callback(scenario.desired_velocity); 30 | behavior->obstacle_callback(scenario.obstacles); 31 | 32 | auto plotter = [&]() 33 | { 34 | static int n = 0; 35 | 36 | PlotAxis axis{std::to_string(n) + " x/y", "[-4:3]"}; 37 | plot(manager.get_trajectories(), scenario.obstacles, axis); 38 | 39 | ++n; 40 | }; 41 | 42 | if(_plot_debug) 43 | { 44 | behavior->set_optim_callback(plotter); 45 | } 46 | 47 | manager.plan(); 48 | 49 | if(_plot) 50 | { 51 | plotter(); 52 | } 53 | } 54 | 55 | class KomoJointTest : public ::testing::Test 56 | { 57 | public: 58 | void SetUp() 59 | { 60 | ros::Time::init(); 61 | 62 | behavior = std::make_shared(manager, 4); 63 | 64 | manager.register_behavior("collision_avoidance", behavior); 65 | manager.set_current_behavior("collision_avoidance"); 66 | } 67 | 68 | void plan(const Scenario & scenario, bool plot, bool plot_debug = false) 69 | { 70 | plan_impl(scenario, manager, behavior, plot, plot_debug); 71 | } 72 | 73 | BehaviorManager manager; 74 | std::shared_ptr behavior; 75 | }; 76 | 77 | class KomoDecTest : public ::testing::Test 78 | { 79 | public: 80 | void SetUp() 81 | { 82 | ros::Time::init(); 83 | 84 | behavior = std::make_shared(manager, n_obstacles, tree, 3.5, 10, 4); 85 | 86 | manager.register_behavior("collision_avoidance", behavior); 87 | manager.set_current_behavior("collision_avoidance"); 88 | } 89 | 90 | void plan(const Scenario & scenario, bool plot, bool plot_debug = false) 91 | { 92 | plan_impl(scenario, manager, behavior, plot, plot_debug); 93 | } 94 | 95 | BehaviorManager manager; 96 | std::shared_ptr behavior; 97 | int n_obstacles = 2; 98 | bool tree = true; 99 | }; 100 | 101 | class KomoDecTest1Obstacle : public KomoDecTest 102 | { 103 | public: 104 | KomoDecTest1Obstacle() 105 | { 106 | n_obstacles = 1; 107 | } 108 | }; 109 | 110 | class KomoDecTest2Obstacle : public KomoDecTest 111 | { 112 | public: 113 | KomoDecTest2Obstacle() 114 | { 115 | n_obstacles = 2; 116 | } 117 | }; 118 | 119 | class KomoDecTest3Obstacle : public KomoDecTest 120 | { 121 | public: 122 | KomoDecTest3Obstacle() 123 | { 124 | n_obstacles = 3; 125 | } 126 | }; 127 | 128 | class KomoDecTestLinear1Obstacle : public KomoDecTest 129 | { 130 | public: 131 | KomoDecTestLinear1Obstacle() 132 | { 133 | n_obstacles = 1; 134 | tree = false; 135 | } 136 | }; 137 | 138 | class KomoDecTestLinear2Obstacle : public KomoDecTest 139 | { 140 | public: 141 | KomoDecTestLinear2Obstacle() 142 | { 143 | n_obstacles = 2; 144 | tree = false; 145 | } 146 | }; 147 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/common/qp_problems.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | struct QP_problem 12 | { 13 | MPC_model model; 14 | TreePb tree; 15 | Constraints k; 16 | Vector2d x0; 17 | Vector2d xd; 18 | }; 19 | 20 | QP_problem create_2_branches_2_steps_constrained(double p=0.6); 21 | QP_problem create_2_branches_4_steps_constrained(double p=0.6); 22 | QP_problem create_3_branches_4_steps_constrained(double p=0.6); 23 | QP_problem create_4_branches_4_steps_constrained(double p=0.6); 24 | QP_problem create_10_branches_4_steps_constrained(); 25 | QP_problem create_20_branches_4_steps_constrained(); 26 | QP_problem create_N_branches_4_steps_constrained(int N); 27 | QP_problem create_2_stages_branching(double p=0.6); 28 | // paper plots 29 | QP_problem create_paper_1_branch_4_steps_constrained(double p=0.6); 30 | QP_problem create_paper_4_branches_4_steps_constrained(double p=0.6); 31 | QP_problem replicate_simulation_1(); 32 | 33 | class QPTest : public ::testing::Test 34 | { 35 | public: 36 | double execution_time_ms{0}; 37 | 38 | protected: 39 | VectorXd plan_OSQP(const QP_problem &pb, bool plot = false, const std::string & filename = ""); 40 | VectorXd plan_JointQP(const QP_problem &pb, bool plot = false, const std::string & filename = ""); 41 | VectorXd plan_DecQP(const QP_problem &pb, bool plot = false, const std::string & filename = ""); 42 | 43 | void plot_XU(const VectorXd& X, const VectorXd& U, const QP_problem &pb) const; 44 | void save_XU(const VectorXd& X, const VectorXd& U, const QP_problem &pb, const std::string & filename) const; 45 | 46 | PlotAxis acc_axis{"acceleration", "[-4:3]"}; 47 | PlotAxis vel_axis{"velocity", "[0:10]"}; 48 | PlotAxis x_axis{"x", "[0:50]"}; 49 | 50 | double u_min{-6.0}; 51 | double u_max{2.0}; 52 | }; 53 | 54 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_KOMO_collision_avoidance_cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "common/komo_problems.h" 4 | 5 | #include 6 | 7 | 8 | TEST(ProbabilityFusion, DISABLED_OneObstacle2Branches) 9 | { 10 | std::vector obstacles; 11 | obstacles.push_back(Obstacle{arr(), 0.1}); 12 | 13 | std::vector> activities; 14 | auto ps = fuse_probabilities(obstacles, true, activities); 15 | 16 | EXPECT_EQ(std::vector({0.1, 0.9}), ps); 17 | } 18 | 19 | TEST(ProbabilityFusion, DISABLED_TwoObstacle4Branches) 20 | { 21 | std::vector obstacles; 22 | obstacles.push_back(Obstacle{arr(), 0.1}); 23 | obstacles.push_back(Obstacle{arr(), 0.5}); 24 | 25 | std::vector> activities; 26 | const auto ps = fuse_probabilities(obstacles, true, activities); 27 | 28 | EXPECT_EQ(std::vector({0.05, 0.5*(1.0 - 0.1), 0.05, 1.0 - 0.05 * 2 - 0.5*(1.0 - 0.1)}), ps); 29 | } 30 | 31 | TEST_F(KomoJointTest, DISABLED_scenario_1) 32 | { 33 | auto pb = create_scenario_1(0.5); 34 | 35 | plan(pb, true); 36 | plan(pb, false); 37 | } 38 | 39 | TEST_F(KomoDecTest1Obstacle, DISABLED_scenario_1) 40 | { 41 | auto pb = create_scenario_1(0.02); 42 | 43 | plan(pb, true); 44 | plan(pb, false); 45 | } 46 | 47 | TEST_F(KomoDecTest1Obstacle, DISABLED_scenario_1_bis) 48 | { 49 | auto pb = create_scenario_1(0.8); 50 | 51 | plan(pb, false, true); 52 | 53 | std::cout << "----------" << std::endl; 54 | 55 | //pb.odo->pose.pose.position.x += 3; 56 | 57 | //plan(pb, false, true); 58 | 59 | // std::cout << "----------" << std::endl; 60 | 61 | // pb.odo->pose.pose.position.x += 3; 62 | 63 | // plan(pb, true); 64 | } 65 | 66 | TEST_F(KomoDecTest2Obstacle, scenario_2) 67 | { 68 | auto pb = create_scenario_2(); 69 | 70 | plan(pb, true); 71 | 72 | pb.odo->pose.pose.position.x += 2; 73 | 74 | plan(pb, true); 75 | 76 | pb.odo->pose.pose.position.x += 2; 77 | 78 | plan(pb, true); 79 | 80 | pb.odo->pose.pose.position.x += 2; 81 | 82 | plan(pb, true); 83 | 84 | // pb.odo->pose.pose.position.x += 2; 85 | 86 | // plan(pb, true); 87 | } 88 | 89 | TEST_F(KomoDecTest3Obstacle, DISABLED_scenario_3) 90 | { 91 | auto pb = create_scenario_3(); 92 | 93 | plan(pb, true); 94 | //plan(pb, false); 95 | } 96 | 97 | TEST_F(KomoDecTestLinear1Obstacle, DISABLED_scenario_1) 98 | { 99 | auto pb = create_scenario_1(0.02); 100 | 101 | plan(pb, true); 102 | plan(pb, false); 103 | } 104 | 105 | TEST_F(KomoDecTestLinear2Obstacle, DISABLED_scenario_2) 106 | { 107 | auto pb = create_scenario_2(); 108 | 109 | plan(pb, true); 110 | plan(pb, false); 111 | } 112 | 113 | //////////////////////////////// 114 | int main(int argc, char **argv) 115 | { 116 | testing::InitGoogleTest(&argc, argv); 117 | int ret = RUN_ALL_TESTS(); 118 | return ret; 119 | } 120 | 121 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_KOMO_utility_cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | TEST(ProjectOnTrajectory, NominalCase) 8 | { 9 | std::vector trajectory; 10 | trajectory.push_back({0, 0, 0}); 11 | trajectory.push_back({1, 0, 0}); 12 | trajectory.push_back({2, 0, 0}); 13 | 14 | int index; double mu; 15 | auto proj = project_on_trajectory({0.5, 1, 0}, trajectory, index, mu); 16 | 17 | EXPECT_TRUE(near(proj,Pose2D({0.5, 0, 0}))); 18 | EXPECT_EQ(index, 0); 19 | EXPECT_NEAR(mu, 0.5, 0.01); 20 | } 21 | 22 | TEST(ProjectOnTrajectory, BeforeStartPoint) 23 | { 24 | std::vector trajectory; 25 | trajectory.push_back({0, 0, 0}); 26 | trajectory.push_back({1, 0, 0}); 27 | trajectory.push_back({2, 0, 0}); 28 | 29 | int index; double mu; 30 | auto proj = project_on_trajectory({-0.5, 1, 0}, trajectory, index, mu); 31 | 32 | EXPECT_TRUE(near(proj,Pose2D({-0.5, 0, 0}))); 33 | EXPECT_EQ(index, 0); 34 | EXPECT_NEAR(mu, -0.5, 0.01); 35 | } 36 | 37 | TEST(TranslateTrajectory, TrajectorySlightlyAdvanced) 38 | { 39 | std::vector trajectory; 40 | // prefix 41 | trajectory.push_back({-2, 0, 0}); 42 | trajectory.push_back({-1, 0, 0}); 43 | // start 44 | trajectory.push_back({0, 0, 0}); 45 | trajectory.push_back({1, 0, 0}); 46 | trajectory.push_back({2, 0, 0}); 47 | 48 | OdometryState projected_odo; 49 | projected_odo.x = 0.5; 50 | projected_odo.y = 0; 51 | projected_odo.yaw = 0; 52 | projected_odo.v = 1.0; 53 | projected_odo.omega = 0.0; 54 | 55 | translate_trajectory(projected_odo, 1, trajectory); 56 | 57 | EXPECT_TRUE(near(trajectory[0], Pose2D({-1.5, 0, 0}))); 58 | EXPECT_TRUE(near(trajectory[1], Pose2D({-0.5, 0, 0}))); 59 | EXPECT_TRUE(near(trajectory[2], Pose2D({0.5, 0, 0}))); 60 | EXPECT_TRUE(near(trajectory[3], Pose2D({1.5, 0, 0}))); 61 | EXPECT_TRUE(near(trajectory[4], Pose2D({2.5, 0, 0}))); 62 | } 63 | 64 | TEST(TranslateTrajectorySecondStrategy, Straight) 65 | { 66 | std::vector trajectory; 67 | // prefix 68 | trajectory.push_back({-2, 0, 0}); 69 | trajectory.push_back({-1, 0, 0}); 70 | // start 71 | trajectory.push_back({0, 0, 0}); 72 | trajectory.push_back({1, 0, 0}); 73 | trajectory.push_back({2, 0, 0}); 74 | 75 | slide_trajectory(1, 1, trajectory); 76 | 77 | EXPECT_TRUE(near(trajectory[0], Pose2D({-1, 0, 0}))); 78 | EXPECT_TRUE(near(trajectory[1], Pose2D({0, 0, 0}))); 79 | EXPECT_TRUE(near(trajectory[2], Pose2D({1, 0, 0}))); 80 | EXPECT_TRUE(near(trajectory[3], Pose2D({2, 0, 0}))); 81 | EXPECT_TRUE(near(trajectory[4], Pose2D({3, 0, 0}))); 82 | } 83 | 84 | TEST(SlideTrajectorySecondStrategy, Curve) 85 | { 86 | std::vector trajectory; 87 | // prefix 88 | trajectory.push_back({-2, 0, 0}); 89 | trajectory.push_back({-1, 0, 0}); 90 | // start 91 | trajectory.push_back({0, 0, 0}); 92 | trajectory.push_back({1, 1, 0}); 93 | trajectory.push_back({2, 2, 0.01}); 94 | 95 | slide_trajectory(2, 1, trajectory); 96 | 97 | EXPECT_TRUE(near(trajectory[0], Pose2D({0, 0, 0}))); 98 | EXPECT_TRUE(near(trajectory[1], Pose2D({1, 1, 0}))); 99 | EXPECT_TRUE(near(trajectory[2], Pose2D({2, 2, 0.01}))); 100 | EXPECT_TRUE(near(trajectory[3], Pose2D({3, 3, 0.01}))); 101 | EXPECT_TRUE(near(trajectory[4], Pose2D({4, 4, 0.01}))); 102 | } 103 | 104 | TEST(SlideTrajectorySecondStrategySmoothMu, Curve) 105 | { 106 | std::vector trajectory; 107 | // prefix 108 | trajectory.push_back({-2, 0, 0}); 109 | trajectory.push_back({-1, 0, 0}); 110 | // start 111 | trajectory.push_back({0, 0, 0}); 112 | trajectory.push_back({1, 1, 0}); 113 | trajectory.push_back({2, 2, 0.01}); 114 | 115 | slide_trajectory(0, 0.5, 1, trajectory); 116 | 117 | EXPECT_TRUE(near(trajectory[0], Pose2D({-1.5, 0, 0}))); 118 | EXPECT_TRUE(near(trajectory[1], Pose2D({-0.5, 0, 0}))); 119 | EXPECT_TRUE(near(trajectory[2], Pose2D({0.5, 0.5, 0}))); 120 | EXPECT_TRUE(near(trajectory[3], Pose2D({1.5, 1.5, 0.005}))); 121 | EXPECT_TRUE(near(trajectory[4], Pose2D({2.5, 2.5, 0.01}))); 122 | } 123 | 124 | TEST(SlideTrajectorySecondStrategySmoothShift, Curve) 125 | { 126 | std::vector trajectory; 127 | // prefix 128 | trajectory.push_back({-2, 0, 0}); 129 | trajectory.push_back({-1, 0, 0}); 130 | // start 131 | trajectory.push_back({0, 0, 0}); 132 | trajectory.push_back({1, 1, 0}); 133 | trajectory.push_back({2, 2, 0.01}); 134 | 135 | slide_trajectory(1, 0.0, 1, trajectory); 136 | 137 | EXPECT_TRUE(near(trajectory[0], Pose2D({-1, 0, 0}))); 138 | EXPECT_TRUE(near(trajectory[1], Pose2D({0, 0, 0}))); 139 | EXPECT_TRUE(near(trajectory[2], Pose2D({1, 1, 0}))); 140 | EXPECT_TRUE(near(trajectory[3], Pose2D({2, 2, 0.01}))); 141 | EXPECT_TRUE(near(trajectory[4], Pose2D({3, 3, 0.01}))); 142 | } 143 | 144 | TEST(SlideTrajectorySecondStrategySmoothMu1EqualsShift, Curve) 145 | { 146 | std::vector trajectory; 147 | // prefix 148 | trajectory.push_back({-2, 0, 0}); 149 | trajectory.push_back({-1, 0, 0}); 150 | // start 151 | trajectory.push_back({0, 0, 0}); 152 | trajectory.push_back({1, 1, 0}); 153 | trajectory.push_back({2, 2, 0.01}); 154 | 155 | slide_trajectory(0, 1.0, 1, trajectory); 156 | 157 | EXPECT_TRUE(near(trajectory[0], Pose2D({-1, 0, 0}))); 158 | EXPECT_TRUE(near(trajectory[1], Pose2D({0, 0, 0}))); 159 | EXPECT_TRUE(near(trajectory[2], Pose2D({1, 1, 0}))); 160 | EXPECT_TRUE(near(trajectory[3], Pose2D({2, 2, 0.01}))); 161 | EXPECT_TRUE(near(trajectory[4], Pose2D({3, 3, 0.01}))); 162 | } 163 | 164 | //////////////////////////////// 165 | int main(int argc, char **argv) 166 | { 167 | testing::InitGoogleTest(&argc, argv); 168 | int ret = RUN_ALL_TESTS(); 169 | return ret; 170 | } 171 | 172 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_MPC_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | using namespace std; 11 | 12 | static double p = 0.6; 13 | 14 | TEST(MPC_model_tests, cost_zero) 15 | { 16 | MPC_model model_(0.1, 1.0, 3.0); 17 | 18 | auto cost = model_.cost({0, 0}, 0, {0, 0}); 19 | 20 | EXPECT_EQ(0.0, cost); 21 | } 22 | 23 | 24 | TEST(MPC_model_tests, cost_control) 25 | { 26 | MPC_model model_(0.1, 1.0, 3.0); 27 | 28 | auto cost = model_.cost({0, 0}, 1.0, {0, 0}); 29 | 30 | EXPECT_EQ(3.0, cost); 31 | } 32 | 33 | TEST(MPC_model_tests, cost_state) 34 | { 35 | MPC_model model_(0.1, 1.0, 3.0); 36 | 37 | auto cost = model_.cost({0, 0}, 0.0, {0, 1}); 38 | 39 | EXPECT_EQ(1.0, cost); 40 | } 41 | 42 | TEST(MPC_model_tests, test_R_simple_tree) 43 | { 44 | Tree2Branches tree(p); 45 | 46 | MPC_model model_(0.1, 1.0, 3.0); 47 | 48 | auto R_bar = model_.get_R_bar(tree.n_steps, tree.varss, tree.scaless); 49 | 50 | std::cout << "R_bar:\n" << R_bar << std::endl; 51 | 52 | EXPECT_EQ(R_bar(0,0), model_.R(0)); 53 | EXPECT_EQ(R_bar(1,1), p * model_.R(0)); 54 | EXPECT_EQ(R_bar(5,5), (1-p) * model_.R(0)); 55 | } 56 | 57 | TEST(MPC_model_tests, test_Q_simple_tree) 58 | { 59 | Tree2Branches tree(p); 60 | 61 | MPC_model model_(0.1, 1.0, 3.0); 62 | 63 | auto Q_bar = model_.get_Q_bar(tree.n_steps, tree.varss, tree.scaless); 64 | 65 | std::cout << "Q_bar:\n" << Q_bar << std::endl; 66 | 67 | EXPECT_EQ(Q_bar(1,1), model_.Q(1, 1)); 68 | EXPECT_EQ(Q_bar(2*1+1,2*1+1), p * model_.Q(1, 1)); 69 | EXPECT_EQ(Q_bar(2*5+1,2*5+1), (1-p) * model_.Q(1, 1)); 70 | } 71 | 72 | TEST(MPC_model_tests, test_T_simple_tree) 73 | { 74 | Tree2Branches tree(p); 75 | 76 | MPC_model model_(0.1, 1.0, 3.0); 77 | 78 | auto T = model_.get_T(tree.n_steps, tree.varss); 79 | 80 | std::cout << "T:\n" << T << std::endl; 81 | 82 | EXPECT_EQ(T.block(0, 0, 2, 2), model_.A); 83 | EXPECT_EQ(T.block(2, 0, 2, 2), model_.A.pow(2)); // 1->2 84 | EXPECT_EQ(T.block(2*2, 0, 2, 2), model_.A.pow(3)); // 2->3 85 | EXPECT_EQ(T.block(2*3, 0, 2, 2), model_.A.pow(4)); // 3->4 86 | EXPECT_EQ(T.block(2*4, 0, 2, 2), model_.A.pow(5)); // 4->5 87 | 88 | EXPECT_EQ(T.block(2*4+2*1, 0, 2, 2), model_.A.pow(2)); // 1->5 89 | EXPECT_EQ(T.block(2*4+2*2, 0, 2, 2), model_.A.pow(3)); // 5->6 90 | EXPECT_EQ(T.block(2*4+2*3, 0, 2, 2), model_.A.pow(4)); // 6->7 91 | EXPECT_EQ(T.block(2*4+2*4, 0, 2, 2), model_.A.pow(5)); // 7->8 92 | } 93 | 94 | TEST(MPC_model_tests, test_T_simple_tree_N_steps_per_phase) 95 | { 96 | //auto tb = build_simple_path_builder(); 97 | 98 | Tree2Branches2Steps tree(p); 99 | 100 | MPC_model model_(0.1, 1.0, 3.0); 101 | 102 | auto T = model_.get_T(tree.n_steps, tree.varss); 103 | 104 | std::cout << "T:\n" << T << std::endl; 105 | 106 | EXPECT_EQ(T.block(0, 0, 2, 2), model_.A); 107 | EXPECT_EQ(T.block(2, 0, 2, 2), model_.A.pow(2)); // 1->2 108 | EXPECT_EQ(T.block(2*2, 0, 2, 2), model_.A.pow(3)); // 2->3 109 | EXPECT_EQ(T.block(2*3, 0, 2, 2), model_.A.pow(4)); // 3->4 110 | 111 | // EXPECT_EQ(T.block(2*3+2*1, 0, 2, 2), model_.A.pow(2)); // 1->5 112 | // EXPECT_EQ(T.block(2*3+2*2, 0, 2, 2), model_.A.pow(3)); // 5->6 113 | // EXPECT_EQ(T.block(2*3+2*3, 0, 2, 2), model_.A.pow(4)); // 6->7 114 | } 115 | 116 | TEST(MPC_model_tests, test_S_simple_tree) 117 | { 118 | //auto tb = build_simple_path_builder(); 119 | 120 | Tree2Branches tree(p); 121 | 122 | MPC_model model_(0.1, 1.0, 3.0); 123 | 124 | auto S = model_.get_S(tree.n_steps, tree.varss); 125 | 126 | std::cout << "S:\n" << S << std::endl; 127 | 128 | EXPECT_EQ(S.block(0, 0, 2, 1), model_.B); // 0->1 129 | 130 | EXPECT_EQ(S.block(2, 0, 2, 1), model_.A.pow(1) * model_.B); // 1->2 131 | EXPECT_EQ(S.block(2, 1, 2, 1), model_.B); // 1->2 132 | 133 | EXPECT_EQ(S.block(2*2, 0, 2, 1), model_.A.pow(2) * model_.B); // 2->3 134 | EXPECT_EQ(S.block(2*2, 1, 2, 1), model_.A.pow(1) * model_.B); // 2->3 135 | EXPECT_EQ(S.block(2*2, 2, 2, 1), model_.A.pow(0) * model_.B); // 2->3 136 | 137 | EXPECT_EQ(S.block(2*3, 0, 2, 1), model_.A.pow(3) * model_.B); // 3->4 138 | EXPECT_EQ(S.block(2*3, 1, 2, 1), model_.A.pow(2) * model_.B); // 3->4 139 | EXPECT_EQ(S.block(2*3, 2, 2, 1), model_.A.pow(1) * model_.B); // 3->4 140 | EXPECT_EQ(S.block(2*3, 3, 2, 1), model_.A.pow(0) * model_.B); // 3->4 141 | 142 | EXPECT_EQ(S.block(2*4, 0, 2, 1), model_.A.pow(4) * model_.B); // 4->5 143 | EXPECT_EQ(S.block(2*4, 1, 2, 1), model_.A.pow(3) * model_.B); // 4->5 144 | EXPECT_EQ(S.block(2*4, 2, 2, 1), model_.A.pow(2) * model_.B); // 4->5 145 | EXPECT_EQ(S.block(2*4, 3, 2, 1), model_.A.pow(1) * model_.B); // 4->5 146 | EXPECT_EQ(S.block(2*4, 4, 2, 1), model_.A.pow(0) * model_.B); // 4->5 147 | 148 | EXPECT_EQ(S.block(2*5, 0, 2, 1), model_.A.pow(1) * model_.B); // 1->6 149 | EXPECT_EQ(S.block(2*5, 5, 2, 1), model_.B); // 1->6 150 | } 151 | 152 | TEST(TreeBb, refinement) 153 | { 154 | auto tree = TreePb::refined(Tree2Branches(p), 2); 155 | 156 | auto tree_2 = Tree2Branches2Steps(p); 157 | 158 | EXPECT_EQ(tree.varss, tree_2.varss); 159 | EXPECT_EQ(tree.scaless, tree_2.scaless); 160 | EXPECT_EQ(tree.n_steps, tree_2.n_steps); 161 | } 162 | 163 | //////////////////////////////// 164 | int main(int argc, char **argv) 165 | { 166 | testing::InitGoogleTest(&argc, argv); 167 | int ret = RUN_ALL_TESTS(); 168 | return ret; 169 | } 170 | 171 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_QP_DecQP.cpp: -------------------------------------------------------------------------------- 1 | //#include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include "common/qp_problems.h" 8 | 9 | #include 10 | 11 | #include 12 | 13 | TEST_F(QPTest, JOINT_test_2_branches_4_steps_constrained) 14 | { 15 | auto pb = create_2_branches_4_steps_constrained(); 16 | 17 | plan_JointQP(pb, false); 18 | } 19 | 20 | TEST_F(QPTest, JOINT_test_paper_1_branch_4_steps) 21 | { 22 | auto pb = create_paper_1_branch_4_steps_constrained(); 23 | 24 | vel_axis.range = "[0:15]"; 25 | acc_axis.range = "[-8:3]"; 26 | 27 | auto U = plan_JointQP(pb, false, "/tmp/1_branch.dat");//, "/tmp/1_branch.dat"); 28 | 29 | //auto U = plan_JointQP(pb, false, "/home/camille/Phd/Paper/ICRA-2021/plots/1_branch.dat");//, "/tmp/1_branch.dat"); 30 | } 31 | 32 | TEST_F(QPTest, DEC_paper_4_branches_4_steps_constrained) 33 | { 34 | auto pb = create_paper_4_branches_4_steps_constrained(0.1); 35 | 36 | vel_axis.range = "[0:15]"; 37 | acc_axis.range = "[-8:3]"; 38 | 39 | plan_DecQP(pb, false, "/tmp/4_branches.dat"); 40 | 41 | //plan_DecQP(pb, false, "/home/camille/Phd/Paper/ICRA-2021/plots/4_branches.dat"); 42 | } 43 | 44 | TEST_F(QPTest, DEC_test_paper_1_branch_4_steps) 45 | { 46 | auto pb = create_paper_1_branch_4_steps_constrained(); 47 | 48 | vel_axis.range = "[0:15]"; 49 | acc_axis.range = "[-8:3]"; 50 | 51 | plan_DecQP(pb, false);//, "/tmp/1_branch.dat"); 52 | // plan_DecQP(pb, false);//, "/home/camille/Phd/Paper/ICRA-2021/plots/1_branch.dat"); 53 | } 54 | 55 | TEST_F(QPTest, DEC_test_paper_multi_probabilities) 56 | { 57 | std::vector ps{0.025, 0.05, 0.1, 0.25, 0.5, 0.75, 1.0}; 58 | 59 | for(const auto p: ps) 60 | { 61 | auto pb = create_paper_4_branches_4_steps_constrained(p); 62 | 63 | vel_axis.range = "[0:15]"; 64 | acc_axis.range = "[-8:3]"; 65 | 66 | std::stringstream ss; 67 | //ss << "/home/camille/Phd/Paper/ICRA-2021/plots/4_branches_" << p << ".dat"; 68 | ss << "/tmp/4_branches_" << p << ".dat"; 69 | 70 | plan_DecQP(pb, false, ss.str()); 71 | } 72 | } 73 | 74 | TEST_F(QPTest, DEC_test_2_branches_4_steps_constrained) 75 | { 76 | auto pb = create_2_branches_4_steps_constrained(0.1); 77 | 78 | plan_DecQP(pb, false); 79 | } 80 | 81 | TEST_F(QPTest, test_3_branches_4_steps_constrained) 82 | { 83 | auto pb = create_3_branches_4_steps_constrained(); 84 | 85 | plan_DecQP(pb, false); 86 | } 87 | 88 | TEST_F(QPTest, test_4_branches_4_steps_constrained) 89 | { 90 | auto pb = create_4_branches_4_steps_constrained(); 91 | 92 | plan_DecQP(pb, false); 93 | } 94 | 95 | TEST_F(QPTest, test_replicate_simulation_1) 96 | { 97 | auto pb = replicate_simulation_1(); 98 | 99 | plan_DecQP(pb, true); 100 | } 101 | 102 | TEST_F(QPTest, test_20_branches_) 103 | { 104 | std::ofstream file("/tmp/execution_time_dec_qp_20.dat"); 105 | 106 | //std::ofstream file("/home/camille/Phd/Paper/ICRA-2021/plots/execution_time_dec_qp_20.dat"); 107 | file << "#" << " " << "n" << " " << "execution time (ms)" << std::endl; 108 | for(auto i = 1; i <= 20; ++i) 109 | { 110 | auto pb = create_N_branches_4_steps_constrained(i); 111 | plan_DecQP(pb, false); 112 | file << " " << i << " " << execution_time_ms << std::endl; 113 | } 114 | } 115 | 116 | //TEST_F(QPTest, test_N_branches) 117 | //{ 118 | // std::ofstream file("/home/camille/Phd/Paper/ICRA-2021/plots/execution_time_dec_qp_100_.dat"); 119 | // file << "#" << " " << "n" << " " << "execution time (ms)" << std::endl; 120 | // for(auto i = 1; i <= 100; ++i) 121 | // { 122 | // auto pb = create_N_branches_4_steps_constrained(i); 123 | // plan_DecQP(pb, false); 124 | // file << " " << i << " " << execution_time_ms << std::endl; 125 | // } 126 | ///} 127 | 128 | //////////////////////////////// 129 | int main(int argc, char **argv) 130 | { 131 | testing::InitGoogleTest(&argc, argv); 132 | int ret = RUN_ALL_TESTS(); 133 | return ret; 134 | } 135 | 136 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_QP_DecQP_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "common/qp_problems.h" 5 | 6 | #include 7 | #include 8 | 9 | namespace 10 | { 11 | bool close(const Arr& a, const Arr& b) 12 | { 13 | if(a.size() != b.size()) 14 | return false; 15 | 16 | bool ret = true; 17 | for(auto i = 0; i < a.size(); ++i) 18 | { 19 | ret = ret && fabs(a[i] - b[i]) < 0.0001; 20 | } 21 | 22 | return ret; 23 | } 24 | 25 | bool close(const std::vector & a, const std::vector & b) 26 | { 27 | if(a.size() != b.size()) 28 | return false; 29 | 30 | bool ret = true; 31 | for(auto i = 0; i < a.size(); ++i) 32 | { 33 | ret = ret && close(a[i], b[i]); 34 | } 35 | 36 | return ret; 37 | } 38 | } 39 | 40 | TEST(Tree, TreeNBranches) 41 | { 42 | { 43 | Tree2Branches tree(0.4); 44 | TreeNBranches treen(std::vector({0.4})); 45 | 46 | EXPECT_EQ(tree.varss, treen.varss); 47 | EXPECT_EQ(tree.scaless, treen.scaless); 48 | EXPECT_EQ(tree.n_steps, treen.n_steps); 49 | } 50 | 51 | { 52 | Tree4Branches tree(0.3, 0.35, 0.2); 53 | TreeNBranches treen(std::vector({0.3, 0.35, 0.2})); 54 | 55 | EXPECT_EQ(tree.varss, treen.varss); 56 | EXPECT_TRUE(close(tree.scaless, treen.scaless)); 57 | EXPECT_EQ(tree.n_steps, treen.n_steps); 58 | } 59 | } 60 | 61 | TEST(VarsMasks, Compress) 62 | { 63 | IntA var0 {0, 1, 2, 3}; 64 | IntA var1 {0, 4, 5, 6}; 65 | 66 | std::vector varss{var0, var1}; 67 | 68 | IntA var, global_to_branch; 69 | auto masks = get_compressed_masks(7, 2, varss, var, global_to_branch); 70 | 71 | EXPECT_EQ(var, var0); 72 | EXPECT_EQ(masks[0], arr({1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); 73 | EXPECT_EQ(masks[1], arr({1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0})); 74 | EXPECT_EQ(global_to_branch, IntA({0, 1, 2, 3, 1, 2, 3})); 75 | } 76 | 77 | TEST(Constraints, Compress) 78 | { 79 | Tree2Branches tree(0.2); 80 | 81 | Constraints k(tree.n_steps, tree.varss); 82 | 83 | k.add_constraint(0, Vector2d(10, 10), Vector2d(1, 1), {3}); 84 | k.add_constraint(1, Vector2d(10, 10), Vector2d(1, 1), {-1}); 85 | 86 | IntA var, global_to_branch; 87 | auto masks = get_compressed_masks(9, 1, tree.varss, var, global_to_branch); 88 | 89 | auto ks = get_compressed_constraints(k, var, global_to_branch); 90 | 91 | EXPECT_EQ(2, ks.size()); 92 | } 93 | 94 | //////////////////////////////// 95 | int main(int argc, char **argv) 96 | { 97 | testing::InitGoogleTest(&argc, argv); 98 | int ret = RUN_ALL_TESTS(); 99 | return ret; 100 | } 101 | 102 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_QP_OSQP.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include "common/qp_problems.h" 8 | 9 | #include 10 | 11 | 12 | TEST_F(QPTest, test_2_branches_2_steps_constrained) 13 | { 14 | auto pb = create_2_branches_2_steps_constrained(); 15 | 16 | plan_OSQP(pb, false); 17 | } 18 | 19 | TEST_F(QPTest, test_2_branches_4_steps_constrained) 20 | { 21 | auto pb = create_2_branches_4_steps_constrained(); 22 | 23 | plan_OSQP(pb, false); 24 | } 25 | 26 | TEST_F(QPTest, test_3_branches_4_steps_constrained) 27 | { 28 | auto pb = create_3_branches_4_steps_constrained(); 29 | 30 | plan_OSQP(pb, false); 31 | } 32 | 33 | TEST_F(QPTest, test_4_branches_4_steps_constrained) 34 | { 35 | auto pb = create_4_branches_4_steps_constrained(); 36 | 37 | plan_OSQP(pb, false); 38 | } 39 | 40 | TEST_F(QPTest, test_2_stages_branching) 41 | { 42 | auto pb = create_2_stages_branching(); 43 | 44 | plan_OSQP(pb, false); 45 | } 46 | 47 | // plots for paper 48 | TEST_F(QPTest, test_paper_4_branches_4_steps_constrained) 49 | { 50 | auto pb = create_paper_4_branches_4_steps_constrained(0.1); 51 | 52 | vel_axis.range = "[0:15]"; 53 | acc_axis.range = "[-8:3]"; 54 | 55 | plan_OSQP(pb, false, "/home/camille/Phd/Paper/ICRA-2021/plots/4_branches.dat"); 56 | } 57 | 58 | TEST_F(QPTest, test_paper_1_branch_4_steps) 59 | { 60 | auto pb = create_paper_1_branch_4_steps_constrained(); 61 | 62 | vel_axis.range = "[0:15]"; 63 | acc_axis.range = "[-8:3]"; 64 | 65 | auto U = plan_OSQP(pb, false, "/home/camille/Phd/Paper/RSS/plots/1_branch.dat"); 66 | } 67 | 68 | TEST_F(QPTest, test_paper_multi_probabilities) 69 | { 70 | std::vector ps{0.025, 0.05, 0.1, 0.25, 0.5, 0.75, 1.0}; 71 | 72 | for(const auto p: ps) 73 | { 74 | auto pb = create_paper_4_branches_4_steps_constrained(p); 75 | 76 | vel_axis.range = "[0:15]"; 77 | acc_axis.range = "[-8:3]"; 78 | 79 | std::stringstream ss; 80 | ss << "/home/camille/Phd/Paper/ICRA-2021/plots/4_branches_" << p << ".dat"; 81 | 82 | plan_OSQP(pb, false, ss.str()); 83 | } 84 | } 85 | 86 | 87 | TEST_F(QPTest, test_10_branches) 88 | { 89 | auto pb = create_10_branches_4_steps_constrained(); 90 | 91 | plan_OSQP(pb, false); 92 | } 93 | 94 | TEST_F(QPTest, test_20_branches) 95 | { 96 | auto pb = create_20_branches_4_steps_constrained(); 97 | 98 | plan_OSQP(pb, false); 99 | } 100 | 101 | TEST_F(QPTest, test_20_branches_) 102 | { 103 | std::ofstream file("/home/camille/Phd/Paper/ICRA-2021/plots/execution_time_osqp_20.dat"); 104 | file << "#" << " " << "n" << " " << "execution time (ms)" << std::endl; 105 | for(auto i = 1; i <= 20; ++i) 106 | { 107 | auto pb = create_N_branches_4_steps_constrained(i); 108 | plan_OSQP(pb, false); 109 | file << " " << i << " " << execution_time_ms << std::endl; 110 | } 111 | } 112 | 113 | //TEST_F(QPTest, test_N_branches) 114 | //{ 115 | // std::ofstream file("/home/camille/Phd/Paper/ICRA-2021/plots/execution_time_osqp_100.dat"); 116 | // file << "#" << " " << "n" << " " << "execution time (ms)" << std::endl; 117 | // for(auto i = 1; i <= 100; ++i) 118 | // { 119 | // auto pb = create_N_branches_4_steps_constrained(i); 120 | // plan_OSQP(pb, false); 121 | // file << " " << i << " " << execution_time_ms << std::endl; 122 | // } 123 | //} 124 | 125 | TEST_F(QPTest, test_paper_sparsity) 126 | { 127 | auto to_file = [](const MatrixXd & m, const std::string & filepath) 128 | { 129 | std::ofstream of(filepath); 130 | 131 | for(auto i = 0; i < m.rows(); ++i) 132 | { 133 | for(auto j = 0; j < m.cols(); ++j) 134 | { 135 | of << m(i, j) << " "; 136 | } 137 | 138 | of << std::endl; 139 | } 140 | }; 141 | 142 | { 143 | auto pb = create_N_branches_4_steps_constrained(3); 144 | 145 | QP_tree_problem_OSQP solver(pb.model, u_min, u_max); 146 | solver.solve(pb.x0, pb.xd, pb.k, pb.tree.n_steps, pb.tree.varss, pb.tree.scaless); 147 | 148 | to_file(solver.get_H(), "/home/camille/Phd/Paper/RSS/matrices/H_tree_3.txt"); 149 | to_file(solver.get_KA(), "/home/camille/Phd/Paper/RSS/matrices/K_tree_3.txt"); 150 | } 151 | 152 | { 153 | auto pb = create_N_branches_4_steps_constrained(1); 154 | 155 | QP_tree_problem_OSQP solver(pb.model, u_min, u_max); 156 | solver.solve(pb.x0, pb.xd, pb.k, pb.tree.n_steps, pb.tree.varss, pb.tree.scaless); 157 | 158 | to_file(solver.get_H(), "/home/camille/Phd/Paper/RSS/matrices/H_tree_1.txt"); 159 | to_file(solver.get_KA(), "/home/camille/Phd/Paper/RSS/matrices/K_tree_1.txt"); 160 | } 161 | } 162 | 163 | //////////////////////////////// 164 | int main(int argc, char **argv) 165 | { 166 | testing::InitGoogleTest(&argc, argv); 167 | int ret = RUN_ALL_TESTS(); 168 | return ret; 169 | } 170 | 171 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_QP_constraints.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | using namespace std; 9 | 10 | static double p = 0.6; 11 | 12 | TEST(QP_constraints, test_constraints_construction) 13 | { 14 | Tree2Branches tree(p); 15 | 16 | Constraints k(tree.n_steps, tree.varss); 17 | 18 | k.add_constraint(0, Vector2d(10, 0), Vector2d(1, 0)); 19 | k.add_constraint(1, Vector2d(0, 10), Vector2d(0, 1)); 20 | 21 | EXPECT_TRUE(k.validate()); 22 | } 23 | 24 | TEST(QP_constraints, test_constraints_with_elements_specification) 25 | { 26 | Tree2Branches tree(p); 27 | 28 | Constraints k(tree.n_steps, tree.varss); 29 | 30 | k.add_constraint(0, Vector2d(10, 10), Vector2d(1, 1), {4}); 31 | k.add_constraint(1, Vector2d(10, 10), Vector2d(1, 1), {-1}); 32 | 33 | EXPECT_TRUE(k.validate()); 34 | 35 | auto Sextract = k.getSextract(); 36 | 37 | // branch 1 38 | EXPECT_EQ(Sextract(0, 4*2+0), 1); 39 | EXPECT_EQ(Sextract(1, 4*2+1), 1); 40 | 41 | // branch 2 42 | EXPECT_EQ(Sextract(2, 8*2+0), 1); 43 | EXPECT_EQ(Sextract(3, 8*2+1), 1); 44 | 45 | std::cout << "Sextract:\n" << Sextract << std::endl; 46 | } 47 | 48 | TEST(QP_constraints, test_constraints_refinement) 49 | { 50 | Tree2Branches tree(p); 51 | 52 | Constraints k(tree.n_steps, tree.varss); 53 | 54 | k.add_constraint(0, Vector2d(10, 10), Vector2d(1, 1)); // all 55 | k.add_constraint(1, Vector2d(10, 10), Vector2d(1, 1), {-1}); // last 56 | 57 | EXPECT_TRUE(k.validate()); 58 | 59 | auto l = Constraints::refined(k, 2); 60 | 61 | EXPECT_TRUE(l.validate()); 62 | EXPECT_EQ(l.varss.size(), k.varss.size()); 63 | EXPECT_EQ(l.xmaxs.size(), k.xmaxs.size()); 64 | 65 | for(auto i = 0; i < l.varss.size(); ++i) 66 | { 67 | EXPECT_EQ(l.varss[i].size(), 2 * k.varss[i].size()); 68 | } 69 | 70 | for(auto i = 0; i < l.xmaxs.size(); ++i) 71 | { 72 | EXPECT_EQ(get<3>(l.xmaxs[i]).size(), 2 * get<3>(k.xmaxs[i]).size()); 73 | } 74 | 75 | EXPECT_EQ(std::get<3>(l.xmaxs[0]), IntA({0, 1, 2, 3, 4, 5, 6, 7, 8, 9})); 76 | EXPECT_EQ(std::get<3>(l.xmaxs[1]), IntA({16, 17})); 77 | } 78 | 79 | TEST(QP_constraints, test_Sextract_if_no_constraints) 80 | { 81 | Tree2Branches tree(p); 82 | 83 | Constraints k(tree.n_steps, tree.varss); 84 | 85 | k.n_steps = tree.n_steps; 86 | k.varss = tree.varss; 87 | k.add_constraint(0, Vector2d(10, 0), Vector2d(0, 0)); 88 | k.add_constraint(1, Vector2d(0, 10), Vector2d(0, 0)); 89 | 90 | auto Sextract = k.getSextract(); 91 | 92 | EXPECT_EQ(Sextract.rows(), 0); 93 | 94 | std::cout << "Sextract:\n" << Sextract << std::endl; 95 | } 96 | 97 | TEST(QP_constraints, test_Sextract_construction) 98 | { 99 | Tree2Branches tree(p); 100 | 101 | Constraints k(tree.n_steps, tree.varss); 102 | 103 | k.n_steps = tree.n_steps; 104 | k.varss = tree.varss; 105 | k.add_constraint(0, Vector2d(10, 0), Vector2d(1, 0)); 106 | k.add_constraint(1, Vector2d(0, 10), Vector2d(0, 1)); 107 | 108 | auto Sextract = k.getSextract(); 109 | 110 | EXPECT_EQ(Sextract.rows(), tree.n_steps + 1); // first step has 2 constraints 111 | EXPECT_EQ(Sextract.cols(), 2 * tree.n_steps); 112 | 113 | EXPECT_EQ(Sextract(0, 0), 1); // first branch 114 | EXPECT_EQ(Sextract(1, 2), 1); 115 | EXPECT_EQ(Sextract(2, 4), 1); 116 | EXPECT_EQ(Sextract(3, 6), 1); 117 | EXPECT_EQ(Sextract(4, 8), 1); 118 | 119 | EXPECT_EQ(Sextract(5, 1), 1); // second branch 120 | EXPECT_EQ(Sextract(6, 11), 1); 121 | EXPECT_EQ(Sextract(7, 13), 1); 122 | EXPECT_EQ(Sextract(8, 15), 1); 123 | EXPECT_EQ(Sextract(9, 17), 1); 124 | 125 | std::cout << "Sextract:\n" << Sextract << std::endl; 126 | } 127 | 128 | TEST(QP_constraints, test_Xmax_construction) 129 | { 130 | Tree2Branches tree(p); 131 | 132 | Constraints k(tree.n_steps, tree.varss); 133 | 134 | k.add_constraint(0, Vector2d(5, 0), Vector2d(1, 0)); 135 | k.add_constraint(1, Vector2d(0, 10), Vector2d(0, 1)); 136 | 137 | auto Xmax = k.getXmax(); 138 | 139 | //std::cout << "Xmax:\n" << Xmax << std::endl; 140 | 141 | EXPECT_EQ(Xmax.rows(), 2 * tree.n_steps); 142 | 143 | EXPECT_EQ(Xmax(0), 5); 144 | EXPECT_EQ(Xmax(1), 10); 145 | 146 | EXPECT_EQ(Xmax(2), 5); 147 | EXPECT_EQ(Xmax(3), 1.); 148 | 149 | EXPECT_EQ(Xmax(5*2), 1.); 150 | EXPECT_EQ(Xmax(5*2+1), 10); 151 | } 152 | 153 | //////////////////////////////// 154 | int main(int argc, char **argv) 155 | { 156 | testing::InitGoogleTest(&argc, argv); 157 | int ret = RUN_ALL_TESTS(); 158 | return ret; 159 | } 160 | 161 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_emergency_braking.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | 7 | using namespace std; 8 | 9 | TEST(Emergency_brake_trajectory, control_vector_size) 10 | { 11 | auto U = emergency_brake(15.0, 4, 4, -6.0); 12 | 13 | EXPECT_EQ(U.rows(), 16); 14 | } 15 | 16 | TEST(Emergency_brake_trajectory, null_speed) 17 | { 18 | std::cout << "HERE" << std::endl; 19 | 20 | auto U = emergency_brake(0.0, 4, 4, -6.0); 21 | 22 | EXPECT_EQ(U, VectorXd::Zero(4 * 4)); 23 | } 24 | 25 | TEST(Emergency_brake_trajectory, normal_speed) 26 | { 27 | auto U = emergency_brake(10.0, 4, 4, -6.0); 28 | 29 | EXPECT_EQ(U[0], -6.0); 30 | 31 | //std::cout << U << std::endl; 32 | } 33 | 34 | TEST(Emergency_brake_trajectory_tree, normal_speed) 35 | { 36 | auto tree = TreePb::refined(Tree2Branches(0.5), 2); 37 | 38 | auto U = emergency_brake(10.0, tree, 4, -6.0); 39 | 40 | EXPECT_EQ(U[0], -6.0); 41 | 42 | //std::cout << U << std::endl; 43 | } 44 | 45 | TEST(Emergency_brake_trajectory_tree, foo) 46 | { 47 | auto tree = TreePb::refined(Tree2Branches(1.0), 4); 48 | 49 | auto U = emergency_brake(0.166056, tree, 4, -6.0); 50 | 51 | EXPECT_LE(U[0], 0); 52 | 53 | //std::cout << U << std::endl; 54 | } 55 | 56 | //////////////////////////////// 57 | int main(int argc, char **argv) 58 | { 59 | testing::InitGoogleTest(&argc, argv); 60 | int ret = RUN_ALL_TESTS(); 61 | return ret; 62 | } 63 | 64 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_obstacle_markers_cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | TEST(ObstacleBoundingBox, RoadLeftSide) 6 | { 7 | const double y = 1.5; 8 | auto marker = create_collision_marker(0, y, 2, 1, 1, 0.5, 0); 9 | 10 | EXPECT_GE(marker.pose.position.y, y); 11 | } 12 | 13 | 14 | //////////////////////////////// 15 | int main(int argc, char **argv) 16 | { 17 | testing::InitGoogleTest(&argc, argv); 18 | int ret = RUN_ALL_TESTS(); 19 | return ret; 20 | } 21 | 22 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_probability_fusion.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace std; 6 | 7 | TEST(Probability_fusion, one_to_one) 8 | { 9 | std::vector stoplines{{0, 0.5}}; 10 | 11 | const auto probabilities = fuse_probabilities(stoplines, 1); 12 | 13 | EXPECT_EQ(probabilities[0], 0.5); 14 | } 15 | 16 | TEST(Probability_fusion, two_to_two) 17 | { 18 | std::vector stoplines{{0, 0.5}, {1, 0.5}}; 19 | 20 | const auto probabilities = fuse_probabilities(stoplines, 2); 21 | 22 | EXPECT_EQ(probabilities[0], 0.5); 23 | EXPECT_EQ(probabilities[1], 0.25); 24 | } 25 | 26 | 27 | TEST(Probability_fusion, two_to_one) 28 | { 29 | std::vector stoplines{{0, 0.5}, {1, 0.5}}; 30 | 31 | const auto probabilities = fuse_probabilities(stoplines, 1); 32 | 33 | EXPECT_EQ(probabilities[0], 0.75); 34 | } 35 | 36 | TEST(Probability_fusion, two_to_one_edge_case_1) 37 | { 38 | std::vector stoplines{{0, 0.5}, {1, 1.0}}; 39 | 40 | const auto probabilities = fuse_probabilities(stoplines, 1); 41 | 42 | EXPECT_EQ(probabilities[0], 1.0); 43 | } 44 | 45 | TEST(Probability_fusion, two_to_one_edge_case_2) 46 | { 47 | std::vector stoplines{{0, 0.5}, {1, 0.0}}; 48 | 49 | const auto probabilities = fuse_probabilities(stoplines, 1); 50 | 51 | EXPECT_EQ(probabilities[0], 0.5); 52 | } 53 | 54 | //////////////////////////////// 55 | int main(int argc, char **argv) 56 | { 57 | testing::InitGoogleTest(&argc, argv); 58 | int ret = RUN_ALL_TESTS(); 59 | return ret; 60 | } 61 | 62 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_sparsification.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace std; 6 | 7 | TEST(QP_tree_problem, test_matrix_sparsification) 8 | { 9 | MatrixXd M(3, 3); 10 | M << 1, 0, 2, 11 | 0, 0, 3, 12 | 4, 5, 6; 13 | 14 | auto MS = create_csc_matrix(M); 15 | 16 | EXPECT_EQ(MS->nzmax, 6); 17 | 18 | c_float expected_x[6] = {1, 4, 5, 2, 3, 6}; 19 | for(auto I = 0; I < 6; ++I) 20 | EXPECT_EQ(MS->x[I],expected_x[I]); 21 | 22 | c_float expected_i[6] = {0, 2, 2, 0, 1, 2}; 23 | for(auto I = 0; I < 6; ++I) 24 | EXPECT_EQ(MS->i[I],expected_i[I]); 25 | 26 | c_float expected_p[4] = {0, 2, 3, 6}; 27 | for(auto I = 0; I < 4; ++I) 28 | EXPECT_EQ(MS->p[I],expected_p[I]); 29 | } 30 | 31 | //////////////////////////////// 32 | int main(int argc, char **argv) 33 | { 34 | testing::InitGoogleTest(&argc, argv); 35 | int ret = RUN_ALL_TESTS(); 36 | return ret; 37 | } 38 | 39 | -------------------------------------------------------------------------------- /control_tree_car/applications/pedestrians/test/test_utility_cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | using namespace std; 7 | 8 | static constexpr uint N = 10000; 9 | 10 | static uint n_above_threshold(double median_p) 11 | { 12 | double sum = 0; 13 | uint n = 0; 14 | 15 | for(auto i = 0; i < N; ++i) 16 | { 17 | double p = draw_p(median_p); 18 | 19 | if(p > median_p) 20 | { 21 | ++n; 22 | } 23 | } 24 | 25 | return n; 26 | } 27 | 28 | static uint n_true(double average_p) 29 | { 30 | uint n = 0; 31 | 32 | for(auto i = 0; i < N; ++i) 33 | { 34 | if(draw_bool(average_p)) 35 | { 36 | ++n; 37 | } 38 | } 39 | 40 | return n; 41 | } 42 | 43 | TEST(DrawProbability, probability_drawing_0p5) 44 | { 45 | auto n = n_above_threshold(0.5); 46 | 47 | EXPECT_GE(n, 0.4 * N); 48 | EXPECT_LE(n, 0.6 * N); 49 | } 50 | 51 | TEST(DrawProbability, probability_drawing_0p15) 52 | { 53 | auto n = n_above_threshold(0.15); 54 | 55 | EXPECT_GE(n, 0.4 * N); 56 | EXPECT_LE(n, 0.6 * N); 57 | } 58 | 59 | TEST(DrawProbability, probability_drawing_0p05) 60 | { 61 | auto n = n_above_threshold(0.05); 62 | 63 | EXPECT_GE(n, 0.4 * N); 64 | EXPECT_LE(n, 0.6 * N); 65 | } 66 | 67 | TEST(DrawProbability, probability_drawing_0p85) 68 | { 69 | auto n = n_above_threshold(0.85); 70 | 71 | EXPECT_GE(n, 0.4 * N); 72 | EXPECT_LE(n, 0.6 * N); 73 | } 74 | 75 | TEST(DrawBinary, probability_drawing_0p15) 76 | { 77 | auto n = n_true(0.15); 78 | 79 | EXPECT_GE(n, 0.05 * N); 80 | EXPECT_LE(n, 0.25 * N); 81 | } 82 | 83 | TEST(DrawBinary, probability_drawing_0p85) 84 | { 85 | auto n = n_true(0.85); 86 | 87 | EXPECT_GE(n, 0.75 * N); 88 | EXPECT_LE(n, 0.95 * N); 89 | } 90 | 91 | //////////////////////////////// 92 | int main(int argc, char **argv) 93 | { 94 | testing::InitGoogleTest(&argc, argv); 95 | int ret = RUN_ALL_TESTS(); 96 | return ret; 97 | } 98 | 99 | -------------------------------------------------------------------------------- /control_tree_car/data/LGP-real-time-1d.g: -------------------------------------------------------------------------------- 1 | ### objs 2 | 3 | #vehicles 4 | body car_ego { type=9 rel= size=[.2 .1 .1 .02] color=[0 1 1] contact } 5 | 6 | #road 7 | body ground{ type=9, X=, size=[5. 1.0 .04 .01], color=[.3 .3 .3] } 8 | 9 | ## Joints 10 | joint (ground car_ego) { from= to= type=3 agent=true } 11 | -------------------------------------------------------------------------------- /control_tree_car/data/LGP-real-time.g: -------------------------------------------------------------------------------- 1 | ### objs 2 | 3 | #vehicles 4 | body car_ego { type=9 rel= size=[.2 .1 .1 .02] color=[0 1 1] contact } 5 | 6 | #road 7 | body ground{ type=9, X=, size=[5. 1.0 .04 .01], color=[.3 .3 .3] } 8 | 9 | ## Joints 10 | joint (ground car_ego) { from= to= type=8 agent=true } 11 | -------------------------------------------------------------------------------- /control_tree_car/data/doc/obstacles.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:5472403dd74dcafb76314282f7bd197858faf3b08b523bc37897baf6eaa07d91 3 | size 254948 4 | -------------------------------------------------------------------------------- /control_tree_car/data/doc/pedestrians.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:823701b674cfdf5632342bb9a5983d8871be135a54cb36ebad86dec33819d937 3 | size 244941 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/BMW.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:18638229f2a4b7967a97e689773b29df73de17d22f53b0aa302726e8243fd7a6 3 | size 28350 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/G12Pos8.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:0290bb699c946fd0b4d705db54ad1c16210c67b6d830d164e482b05f2621ece7 3 | size 23863480 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/G12Pos8simple.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:202aa5e97bdbf546887e3b76bee283ba2d77f09ccb621449bf92c7cd4c0b545c 3 | size 14721204 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/_6a00d83451db4269e200e55380b5f68834-800wi.jpg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:54b0224281b7d978ed6d9b003df1ee3cc06cef090a1853fe6021ff3974739cad 3 | size 68209 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/black_walnut.jpg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:bb28db2135b2be6950c2b16d9df9d83e51044e0572a1f0f54908041714e53d45 3 | size 1949696 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/bleather.png.001.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:f45d40f58cbf1f23243f72ecaeb7a9efd9dc070735e9ec3e9a1c25cb9223dd41 3 | size 179659 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/bmw.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:5911b1f60fe699b3b5b01b75adc818e3d1e8f22a289ded7d1ba0602939e85ac9 3 | size 12756469 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/bmw_.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:6adeadbe145e7121e435113c526fb898cf1b17da798f3aa9db78a53e72f96f42 3 | size 12755669 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/bmw__.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:403643236713f5b033d208bcb05ea8ba0bdb2285e773bc8adf8704c27ef0f807 3 | size 12754705 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/buton.png.002.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:825af87eee7d0c3c6cde6e0392743f7ae8094582b51979bba4462ed15ae3c062 3 | size 392533 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/carpet.jpg: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:beacca8af0195e37ad6481670496d20723e3fbfa6258afc9da14dd36139d0c59 3 | size 481703 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/disp.png.001.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:1bdc163b2de954a12227a5cdcfb5aa1a8907a19312bccea793e24c34b8b2efb4 3 | size 335589 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/grill.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:3d66a09daa182f41974df8e85a88e78abc1ca2d953afdaae350c65b967fe7b7e 3 | size 7846 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/tleather.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:9d49665bb6b078d0d2c63c53f00397d481eb04083fe1841d082bb361132d3b29 3 | size 231108 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/bmw/wheel.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:ebdabdf2ca24463788aa8bcb15816e37c1c847f60aa13bfa5ba789b4e04e0a08 3 | size 208212 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/eyebrow001-unmodified.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:bd9d94033a85c16af38fb135f5fed4ab2e1a26982db952826e58a2672c8c9a9c 3 | size 80589 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/eyebrow001.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:b2b15dd52470d6d073d18ebf56bcb1f3b45567cd42a2c75557c0ed2adc0f1ec1 3 | size 238358 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/green_eye.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:97b79fb62dde7211c142ce398b9506d42a74bdc478230e90ec69377ffdce4e22 3 | size 682300 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/jeans01_normals.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:2aa953834fa0bcf240fe069dc1ef283d801573e077bc5f9da04847370e9837d1 3 | size 4017244 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/jeans_basic_diffuse.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:c4b2f9126873078ecf8a3678e28622196fa6cb47e131b6067d6ffe81919b0ca6 3 | size 4501616 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/male02_diffuse_black-unmodified.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:d3ef0f625df4d5cea6c9cd89e00b9e86c3b488d63672f9ed5b1327a17abd60dc 3 | size 4390775 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/male02_diffuse_black.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:37c166c53575369f57cf233e775f87ef225b3fe9f66ff7ba34b3abb12dcd46be 3 | size 4040719 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/teeth.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:d0afb57869c6fbb56b98f5efc4aec629ee7593e70d8dd2bfb9de923acfb65f43 3 | size 1799930 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/tshirt02_normals.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:f31407a3f8ae20736f8f9693906f6ad64569032bc4afd0798c305526fa51d33d 3 | size 319746 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/tshirt02_texture.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:932e21f76307fda21692a3b4db03fcddc68127d410b4ccbf9fb5cf95ecfbc1b1 3 | size 72189 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/materials/textures/young_lightskinned_male_diffuse.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:4233a4e1e2d423daf8e5aa25521df93253be54b0ab6490147afae39b076f4fcb 3 | size 4448702 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/meshes/standing.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:bc20dd2cd005d70a35627e38cb52d4c84f3d66034b196efa28e11b10b3295c15 3 | size 4514010 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Standing person 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Marina Kollmitz 9 | kollmitz@cs.uni-freiburg.de 10 | 11 | 12 | 13 | A standing person. Model created with Makehuman (http://www.makehuman.org/) 14 | 15 | 16 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_standing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 -0.1 0.95 0 0 0 7 | 80.0 8 | 9 | 24.88 10 | 0 11 | 0 12 | 25.73 13 | 0 14 | 2.48 15 | 16 | 17 | 18 | 19 | 0 -0.1 0.01 0 0 0 20 | 21 | 22 | 0.5 0.35 0.02 23 | 24 | 25 | 26 | 27 | 28 | 0 0 0.02 0.04 0 0 29 | 30 | 31 | model://person_standing/meshes/standing.dae 32 | 33 | 34 | 35 | 36 | 37 | 0 0 0.02 0.04 0 0 38 | 39 | 40 | model://person_standing/meshes/standing.dae 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/eyebrow001-unmodified.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:bd9d94033a85c16af38fb135f5fed4ab2e1a26982db952826e58a2672c8c9a9c 3 | size 80589 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/eyebrow001.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:b2b15dd52470d6d073d18ebf56bcb1f3b45567cd42a2c75557c0ed2adc0f1ec1 3 | size 238358 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/green_eye.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:97b79fb62dde7211c142ce398b9506d42a74bdc478230e90ec69377ffdce4e22 3 | size 682300 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/jeans01_normals.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:2aa953834fa0bcf240fe069dc1ef283d801573e077bc5f9da04847370e9837d1 3 | size 4017244 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/jeans_basic_diffuse.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:c4b2f9126873078ecf8a3678e28622196fa6cb47e131b6067d6ffe81919b0ca6 3 | size 4501616 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/male02_diffuse_black-unmodified.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:d3ef0f625df4d5cea6c9cd89e00b9e86c3b488d63672f9ed5b1327a17abd60dc 3 | size 4390775 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/male02_diffuse_black.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:37c166c53575369f57cf233e775f87ef225b3fe9f66ff7ba34b3abb12dcd46be 3 | size 4040719 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/teeth.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:d0afb57869c6fbb56b98f5efc4aec629ee7593e70d8dd2bfb9de923acfb65f43 3 | size 1799930 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/tshirt02_normals.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:f31407a3f8ae20736f8f9693906f6ad64569032bc4afd0798c305526fa51d33d 3 | size 319746 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/tshirt02_texture.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:932e21f76307fda21692a3b4db03fcddc68127d410b4ccbf9fb5cf95ecfbc1b1 3 | size 72189 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/materials/textures/young_lightskinned_male_diffuse.png: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:4233a4e1e2d423daf8e5aa25521df93253be54b0ab6490147afae39b076f4fcb 3 | size 4448702 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/meshes/walking.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:e2ae109264af6d8a625523a72e30a5ffa1a4ec34278f9bf1e10e614fa074c379 3 | size 4483436 4 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | Walking person 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Marina Kollmitz 9 | kollmitz@cs.uni-freiburg.de 10 | 11 | 12 | 13 | A walking person. Model created with Makehuman (http://www.makehuman.org/) 14 | 15 | 16 | -------------------------------------------------------------------------------- /control_tree_car/data/meshes/person_walking/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0.95 0 0 0 7 | 80.0 8 | 9 | 27.82 10 | 0 11 | 0 12 | 24.88 13 | 0 14 | 4.57 15 | 16 | 17 | 18 | 19 | 0 0 0.01 0 0 0 20 | 21 | 22 | 0.35 0.75 0.02 23 | 24 | 25 | 26 | 27 | 28 | 0 0 -0.02 0 0 0 29 | 30 | 31 | model://person_walking/meshes/walking.dae 32 | 33 | 34 | 35 | 36 | 37 | 0 0 -0.02 0 0 0 38 | 39 | 40 | model://person_walking/meshes/walking.dae 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /control_tree_car/data/urdf/car.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /control_tree_car/externals/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | add_subdirectory(tamp) 4 | -------------------------------------------------------------------------------- /control_tree_car/launch/display_obstacle_avoidance.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /control_tree_car/launch/display_pedestrian.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /control_tree_car/launch/display_single_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /control_tree_car/launch/display_three_cars.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /control_tree_car/launch/display_two_cars.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /control_tree_car/launch/obstacle_avoidance.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 | -------------------------------------------------------------------------------- /control_tree_car/launch/obstacle_avoidance_.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /control_tree_car/launch/pedestrian.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 | -------------------------------------------------------------------------------- /control_tree_car/launch/pedestrian_komo.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 | -------------------------------------------------------------------------------- /control_tree_car/launch/single_car.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /control_tree_car/launch/three_cars.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /control_tree_car/launch/two_cars.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /control_tree_car/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control_tree_car 4 | 0.0.0 5 | The control_tree_car package 6 | 7 | 8 | 9 | 10 | control tree developper 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | roscpp 53 | rospy 54 | roslib 55 | std_msgs 56 | nav_msgs 57 | tf 58 | roscpp 59 | rospy 60 | std_msgs 61 | nav_msgs 62 | roscpp 63 | rospy 64 | std_msgs 65 | nav_msgs 66 | tf 67 | roslib 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /control_tree_car/rviz/car.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorR=0.0941176 2 | Background\ ColorG=0 3 | Background\ ColorB=0.466667 4 | Fixed\ Frame=/base_link 5 | Target\ Frame= 6 | Grid.Alpha=0.5 7 | Grid.Cell\ Size=0.5 8 | Grid.ColorR=0.898039 9 | Grid.ColorG=0.898039 10 | Grid.ColorB=0.898039 11 | Grid.Enabled=1 12 | Grid.Line\ Style=0 13 | Grid.Line\ Width=0.03 14 | Grid.Normal\ Cell\ Count=0 15 | Grid.OffsetX=0 16 | Grid.OffsetY=0 17 | Grid.OffsetZ=0 18 | Grid.Plane=0 19 | Grid.Plane\ Cell\ Count=10 20 | Grid.Reference\ Frame= 21 | Robot\ Model.Alpha=1 22 | Robot\ Model.Collision\ Enabled=0 23 | Robot\ Model.Enabled=1 24 | Robot\ Model.Robot\ Description=robot_description 25 | Robot\ Model.TF\ Prefix= 26 | Robot\ Model.Update\ Interval=0 27 | Robot\ Model.Visual\ Enabled=1 28 | Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0 29 | Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0 30 | Robot\:\ Robot\ Model\ Link\ legShow\ Axes=0 31 | Robot\:\ Robot\ Model\ Link\ legShow\ Trail=0 32 | Tool\ 2D\ Nav\ GoalTopic=goal 33 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 34 | Camera\ Type=rviz::OrbitViewController 35 | Camera\ Config=1.15779 3.76081 2.16475 0 0 0 36 | Property\ Grid\ State=selection=.Global StatusTopStatus;expanded=.Global Options,Grid.Enabled,Robot Model.Enabled;scrollpos=0,0;splitterpos=150,285;ispageselected=1 37 | [Display0] 38 | Name=Grid 39 | Package=rviz 40 | ClassName=rviz::GridDisplay 41 | [Display1] 42 | Name=Robot Model 43 | Package=rviz 44 | ClassName=rviz::RobotModelDisplay 45 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | project(lgp_car_gazebo_plugin) 3 | 4 | find_package(gazebo REQUIRED) 5 | 6 | find_package(catkin REQUIRED COMPONENTS gazebo_ros 7 | gazebo_plugins 8 | geometry_msgs 9 | nav_msgs 10 | roscpp 11 | sensor_msgs 12 | std_msgs 13 | tf) 14 | 15 | include_directories(${GAZEBO_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 16 | link_directories(${GAZEBO_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS}) 17 | list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") 18 | 19 | add_library(lgp_car_gazebo_plugin SHARED src/lgp_car.cpp) 20 | target_link_libraries(lgp_car_gazebo_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES}) 21 | 22 | add_library(lgp_pedestrian_gazebo_plugin SHARED src/lgp_pedestrian.cpp) 23 | target_link_libraries(lgp_pedestrian_gazebo_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES}) 24 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/meshes/car.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ControlTrees/icra2021/9e05ac6ffb10256b9931958343cc1209dc1fac8f/lgp_car_gazebo_plugin/meshes/car.blend -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/meshes/car.blend1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ControlTrees/icra2021/9e05ac6ffb10256b9931958343cc1209dc1fac8f/lgp_car_gazebo_plugin/meshes/car.blend1 -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/meshes/car.dae: -------------------------------------------------------------------------------- 1 | version https://git-lfs.github.com/spec/v1 2 | oid sha256:463ca74d0f947c241f84a2f6eb559e4fd75c2c5040137b3a608ff77e9b8e2cd2 3 | size 7978 4 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | lgp_car_gazebo_plugin 3 | 0.5.7 4 | ROS plugins for LGP cars 5 | Camille Phiquepal 6 | BSD 7 | Camille Phiquepal 8 | 9 | catkin 10 | 11 | boost 12 | gazebo_ros 13 | gazebo_plugins 14 | geometry_msgs 15 | kobuki_msgs 16 | nav_msgs 17 | roscpp 18 | sensor_msgs 19 | std_msgs 20 | tf 21 | 22 | boost 23 | gazebo_ros 24 | gazebo_plugins 25 | geometry_msgs 26 | kobuki_msgs 27 | nav_msgs 28 | roscpp 29 | sensor_msgs 30 | std_msgs 31 | tf 32 | 33 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/src/lgp_car.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | namespace gazebo 19 | { 20 | class LGPCarPlugIn : public ModelPlugin 21 | { 22 | public: 23 | LGPCarPlugIn(); 24 | 25 | // Called when starting 26 | void Load(physics::ModelPtr parent, sdf::ElementPtr sdf); 27 | 28 | // Called by the world update start event 29 | void OnUpdate(); 30 | 31 | // Callback for incoming velocity commands 32 | void CmdVelCB(const geometry_msgs::TwistConstPtr &msg); 33 | 34 | // Callback for pose reset 35 | void ResetPoseCB(const geometry_msgs::Pose2DConstPtr &msg); 36 | 37 | private: 38 | void PublishMsgs(); 39 | visualization_msgs::Marker CreateMarker(nav_msgs::Odometry msg) const; 40 | visualization_msgs::Marker CreateCar(nav_msgs::Odometry msg) const; 41 | visualization_msgs::Marker CreateObstacle(nav_msgs::Odometry msg) const; 42 | 43 | void ApplyControl(); 44 | 45 | void InitRos(); 46 | void QueueThread(); 47 | 48 | private: 49 | // Pointer to the model 50 | physics::ModelPtr model_; 51 | 52 | // Pointer to the update event connection 53 | event::ConnectionPtr updateConnection_; 54 | 55 | // Model parameters 56 | double wheel_radius_; 57 | double wheel_distance_; 58 | 59 | // Model joints 60 | physics::JointPtr left_wheel_joint_; 61 | physics::JointPtr right_wheel_joint_; 62 | 63 | // Targets 64 | double target_v_; 65 | double target_omega_; 66 | 67 | // ROS 68 | // node use for ROS transport 69 | std::unique_ptr ros_node_; 70 | 71 | // ROS vel subscriber 72 | ros::Subscriber cmd_vel_sub_; 73 | 74 | // ROS reset position subscriber 75 | ros::Subscriber reset_pos_sub_; 76 | 77 | // ROS odo publisher 78 | ros::Publisher odometry_pub_; 79 | 80 | // ROS marker array publisher 81 | ros::Publisher marker_pub_; 82 | 83 | // Tf boradcaster 84 | std::shared_ptr tf_broadcaster_; 85 | 86 | // ROS callbackqueue that helps process messages 87 | ros::CallbackQueue ros_queue_; 88 | 89 | // thread the keeps running the rosQueue 90 | std::thread ros_queue_thread_; 91 | }; 92 | 93 | GZ_REGISTER_MODEL_PLUGIN(LGPCarPlugIn) 94 | } 95 | 96 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/src/lgp_pedestrian.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | namespace gazebo 19 | { 20 | class LGPPedestrianPlugIn : public ModelPlugin 21 | { 22 | public: 23 | LGPPedestrianPlugIn(); 24 | 25 | // Called when starting 26 | void Load(physics::ModelPtr parent, sdf::ElementPtr sdf); 27 | 28 | // Called by the world update start event 29 | void OnUpdate(); 30 | 31 | // Callback for incoming velocity commands 32 | void cmdVelCB(const geometry_msgs::TwistConstPtr &msg); 33 | 34 | // Callback for pose reset 35 | void resetPoseCB(const geometry_msgs::Pose2DConstPtr &msg); 36 | 37 | private: 38 | void publishMsgs(); 39 | visualization_msgs::Marker create_marker(nav_msgs::Odometry msg) const; 40 | visualization_msgs::Marker create_pedestrian(nav_msgs::Odometry msg) const; 41 | 42 | void applyControl(); 43 | 44 | void initRos(); 45 | void queueThread(); 46 | 47 | private: 48 | // Pointer to the model 49 | physics::ModelPtr model_; 50 | 51 | // Pointer to the update event connection 52 | event::ConnectionPtr updateConnection_; 53 | 54 | // Targets 55 | double target_vx_; 56 | double target_vy_; 57 | double target_omega_; 58 | 59 | // ROS 60 | // node use for ROS transport 61 | std::unique_ptr ros_node_; 62 | 63 | // ROS vel subscriber 64 | ros::Subscriber cmd_vel_sub_; 65 | 66 | // ROS reset position subscriber 67 | ros::Subscriber reset_pos_sub_; 68 | 69 | // ROS odo publisher 70 | ros::Publisher odometry_pub_; 71 | 72 | // ROS marker array publisher 73 | ros::Publisher marker_pub_; 74 | 75 | // Tf boradcaster 76 | std::shared_ptr tf_broadcaster_; 77 | 78 | // ROS callbackqueue that helps process messages 79 | ros::CallbackQueue ros_queue_; 80 | 81 | // thread the keeps running the rosQueue 82 | std::thread ros_queue_thread_; 83 | }; 84 | 85 | GZ_REGISTER_MODEL_PLUGIN(LGPPedestrianPlugIn) 86 | } 87 | 88 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/obstacle_avoidance.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_obstacle_0 21 | -10 0 0 0 0 3.1415 22 | model://lgp_obstacle 23 | 24 | 25 | 26 | 27 | lgp_car 28 | model://lgp_car 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/obstacle_avoidance_2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_obstacle_0 21 | -20 0 0 0 0 3.1415 22 | model://lgp_obstacle 23 | 24 | 25 | 26 | 27 | lgp_obstacle_1 28 | -5 0 0 0 0 3.1415 29 | model://lgp_obstacle 30 | 31 | 32 | 33 | 34 | lgp_car 35 | model://lgp_car 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/pedestrian.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_pedestrian_0 21 | 50 0 0 0 0 3.1415 22 | model://lgp_pedestrian 23 | 24 | 25 | 26 | 27 | lgp_car 28 | model://lgp_car 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/pedestrian_1.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_pedestrian_0 21 | 50 0 0 0 0 3.1415 22 | model://lgp_pedestrian 23 | 24 | 25 | 26 | 27 | lgp_car 28 | model://lgp_car 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/pedestrian_2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_pedestrian_0 21 | 50 0 0 0 0 3.1415 22 | model://lgp_pedestrian 23 | 24 | 25 | 26 | 27 | lgp_pedestrian_1 28 | 60 0 0 0 0 3.1415 29 | model://lgp_pedestrian 30 | 31 | 32 | 33 | 34 | lgp_car 35 | model://lgp_car 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/pedestrian_3.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_pedestrian_0 21 | 50 0 0 0 0 3.1415 22 | model://lgp_pedestrian 23 | 24 | 25 | 26 | 27 | lgp_pedestrian_1 28 | 60 0 0 0 0 3.1415 29 | model://lgp_pedestrian 30 | 31 | 32 | 33 | 34 | lgp_pedestrian_2 35 | 70 0 0 0 0 3.1415 36 | model://lgp_pedestrian 37 | 38 | 39 | 40 | 41 | lgp_car 42 | model://lgp_car 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/pedestrian_4.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_pedestrian_0 21 | 50 0 0 0 0 3.1415 22 | model://lgp_pedestrian 23 | 24 | 25 | 26 | 27 | lgp_pedestrian_1 28 | 60 0 0 0 0 3.1415 29 | model://lgp_pedestrian 30 | 31 | 32 | 33 | 34 | lgp_pedestrian_2 35 | 70 0 0 0 0 3.1415 36 | model://lgp_pedestrian 37 | 38 | 39 | 40 | 41 | lgp_pedestrian_3 42 | 80 0 0 0 0 3.1415 43 | model://lgp_pedestrian 44 | 45 | 46 | 47 | 48 | lgp_car 49 | model://lgp_car 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/pedestrian_9.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_pedestrian_0 21 | 50 0 0 0 0 3.1415 22 | model://lgp_pedestrian 23 | 24 | 25 | 26 | 27 | lgp_pedestrian_1 28 | 60 0 0 0 0 3.1415 29 | model://lgp_pedestrian 30 | 31 | 32 | 33 | 34 | lgp_pedestrian_2 35 | 70 0 0 0 0 3.1415 36 | model://lgp_pedestrian 37 | 38 | 39 | 40 | 41 | lgp_pedestrian_3 42 | 80 0 0 0 0 3.1415 43 | model://lgp_pedestrian 44 | 45 | 46 | 47 | 48 | lgp_pedestrian_4 49 | 90 0 0 0 0 3.1415 50 | model://lgp_pedestrian 51 | 52 | 53 | 54 | 55 | lgp_pedestrian_5 56 | 100 0 0 0 0 3.1415 57 | model://lgp_pedestrian 58 | 59 | 60 | 61 | 62 | lgp_pedestrian_6 63 | 110 0 0 0 0 3.1415 64 | model://lgp_pedestrian 65 | 66 | 67 | 68 | 69 | lgp_pedestrian_7 70 | 120 0 0 0 0 3.1415 71 | model://lgp_pedestrian 72 | 73 | 74 | 75 | 76 | lgp_pedestrian_8 77 | 130 0 0 0 0 3.1415 78 | model://lgp_pedestrian 79 | 80 | 81 | 82 | 83 | lgp_car 84 | model://lgp_car 85 | 86 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/single_car.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | lgp_car 21 | model://lgp_car 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/three_cars.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | oncoming_car 21 | 60 3 0 0 0 3.1415 22 | model://lgp_car 23 | 24 | 25 | 26 | 27 | front_car 28 | 10 0 0 0 0 0 29 | model://lgp_car 30 | 31 | 32 | 33 | 34 | lgp_car 35 | model://lgp_car 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /lgp_car_gazebo_plugin/world/two_cars.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -7.35 -3.59 39.73 0 1.417 0.124 7 | 8 | 9 | 10 | 11 | 12 | model://ground_plane 13 | 14 | 15 | 16 | model://sun 17 | 18 | 19 | 20 | 21 | front_car 22 | 10 0 0 0 0 0 23 | model://lgp_car 24 | 25 | 26 | 27 | 28 | 29 | lgp_car 30 | model://lgp_car 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /lgp_car_models/lgp_car/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | LGP Car 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Camille Phiquepal 9 | camille.phiquepal@gmail.com 10 | 11 | 12 | 13 | Simple Car for LGP KOMO based Planning 14 | 15 | 16 | -------------------------------------------------------------------------------- /lgp_car_models/lgp_car/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 1.25 0 0.69 0 0 0 8 | 9 | 10 | 100.0 11 | 12 | 13 | 14 | 15 | 16 | 4.3 1.7 1.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 0 24 | 0 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 4.3 1.7 1.0 36 | 37 | 38 | 39 | 40 | 41 | 1.4 0.9 -0.5 0 0 0 42 | 43 | 44 | .19 45 | 46 | 47 | 48 | 49 | 50 | 51 | 0 52 | 0 53 | 0.0 54 | 0.0 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 1.4 0.9 -0.5 0 0 0 63 | 64 | 65 | .19 66 | 67 | 68 | 69 | 70 | 71 | 72 | 1.4 -0.9 -0.5 0 0 0 73 | 74 | 75 | .19 76 | 77 | 78 | 79 | 80 | 81 | 82 | 0 83 | 0 84 | 0.0 85 | 0.0 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 1.4 -0.9 -0.5 0 0 0 94 | 95 | 96 | .19 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 0.0 0.9 0.19 0 1.5707 1.5707 105 | 106 | 107 | 108 | 109 | .19 110 | .05 111 | 112 | 113 | 114 | 115 | 116 | 117 | 1000.00 118 | 1000.00 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | .19 128 | .05 129 | 130 | 131 | 132 | 133 | 134 | 135 | 0.0 -0.9 0.19 0 1.5707 1.5707 136 | 137 | 138 | 139 | .19 140 | .05 141 | 142 | 143 | 144 | 145 | 146 | 1000.0 147 | 1000.0 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | .19 157 | .05 158 | 159 | 160 | 161 | 162 | 163 | 164 | 0 0 -0.03 0 0 0 165 | left_wheel 166 | chassis 167 | 168 | 0 1 0 169 | 170 | 171 | 172 | 173 | 0 0 0.03 0 0 0 174 | right_wheel 175 | chassis 176 | 177 | 0 1 0 178 | 179 | 180 | 181 | 182 | 183 | -------------------------------------------------------------------------------- /lgp_car_models/lgp_obstacle/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | LGP Car 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Camille Phiquepal 9 | camille.phiquepal@gmail.com 10 | 11 | 12 | 13 | Simple Obstacle for LGP KOMO based Planning 14 | 15 | 16 | -------------------------------------------------------------------------------- /lgp_car_models/lgp_obstacle/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 0.0 0 0.69 0 0 0 8 | 9 | 10 | 300.0 11 | 12 | 13 | 14 | 15 | 16 | 0.5 17 | 1.39 18 | 19 | 20 | 21 | 22 | 23 | 24 | 1.0 25 | 0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 0.5 36 | 1.39 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /lgp_car_models/lgp_pedestrian/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | LGP Car 4 | 1.0 5 | model.sdf 6 | 7 | 8 | Camille Phiquepal 9 | camille.phiquepal@gmail.com 10 | 11 | 12 | 13 | Simple Pedestrian for LGP KOMO based Planning 14 | 15 | 16 | -------------------------------------------------------------------------------- /lgp_car_models/lgp_pedestrian/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 0.0 0 0.69 0 0 0 8 | 9 | 10 | 70.0 11 | 12 | 13 | 14 | 15 | 16 | 0.25 17 | 1.8 18 | 19 | 20 | 21 | 22 | 23 | 24 | 0.0 25 | 0 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 0.25 37 | 1.8 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | --------------------------------------------------------------------------------