├── .dockerignore ├── gerona_examples ├── CATKIN_IGNORE ├── CMakeLists.txt ├── launch │ ├── include │ │ ├── gazebo2tf.launch │ │ ├── stage.launch │ │ ├── amcl.launch │ │ ├── local_elevationmap.launch │ │ └── gerona_stage_params.launch │ ├── stage_example_amcl.launch │ ├── stage_example_odometry.launch │ ├── gazebo_example_summit.launch │ ├── gazebo_example_summit_mbc.launch │ └── subt_tunnel_odom.launch ├── costmap │ └── minimal.yaml ├── package.xml └── nodes │ └── gazebo2tf.py ├── docker ├── kinetic-perception.sh ├── melodic-perception.sh ├── noetic-perception.sh ├── docker-build-generic.sh ├── melodic-perception.docker ├── noetic-perception.docker └── kinetic-perception.docker ├── path_planner ├── src │ └── course_planner │ │ ├── course │ │ ├── node.cpp │ │ ├── segment.cpp │ │ ├── transition.cpp │ │ ├── segment.h │ │ ├── transition.h │ │ ├── node.h │ │ ├── cost_calculator.h │ │ ├── path_builder.h │ │ ├── analyzer.h │ │ └── cost_calculator.cpp │ │ └── course_planner_node.cpp ├── launch │ ├── goal_planner.launch │ ├── planner_cost.launch │ ├── path_planner.launch │ └── planner.launch ├── package.xml └── cmake │ └── FindSBPL.cmake ├── path_msgs ├── msg │ ├── PathSequence.msg │ ├── DirectionalPath.msg │ ├── Obstacle.msg │ ├── FollowerOptions.msg │ ├── Goal.msg │ └── PlannerOptions.msg ├── action │ ├── PlanPath.action │ ├── FollowPath.action │ └── NavigateToGoal.action ├── CMakeLists.txt └── package.xml ├── gerona ├── CMakeLists.txt └── package.xml ├── tools ├── model_based_planner │ ├── config │ │ ├── husky.png │ │ ├── ramaxx.png │ │ ├── Rollator.png │ │ ├── summit_16b_512.png │ │ ├── summit_wl_16b_512.png │ │ ├── ramaxx.yaml │ │ ├── rollator.yaml │ │ ├── rollator_c.yaml │ │ ├── summit.yaml │ │ ├── summit_c.yaml │ │ └── husky_c.yaml │ ├── include │ │ └── model_based_planner │ │ │ ├── wheeldescriptor.h │ │ │ ├── utils_diff.h │ │ │ ├── robotdescriptor.h │ │ │ ├── chassisdescriptor.h │ │ │ ├── pose_writer.h │ │ │ ├── scaleddrawproc.h │ │ │ └── chassismodel.h │ ├── package.xml │ └── src │ │ └── config_modelbasedplanner.cpp ├── scan2cloud │ ├── src │ │ └── scan2cloud_node.cpp │ ├── package.xml │ └── include │ │ └── scan2cloud │ │ └── scan2cloud.h ├── localmap │ ├── config │ │ ├── dem_local_640_10.yaml │ │ ├── dem_local_640_8.yaml │ │ ├── dem_local_1024_10.yaml │ │ └── dem_local_768_8.yaml │ └── launch │ │ ├── localmap.launch │ │ ├── localmap_mf_640_6.launch │ │ ├── localmap_mf_768_8.launch │ │ ├── localmap_mf_1024_10.launch │ │ ├── localmap_rs_mf_768_8.launch │ │ └── sim_summit_localmap.launch ├── path_rviz │ ├── plugin_description.xml │ ├── package.xml │ └── src │ │ └── path_display.h ├── roimap │ ├── launch │ │ └── roimap_default.launch │ ├── CMakeLists.txt │ └── package.xml ├── odom2tf │ ├── package.xml │ ├── CMakeLists.txt │ └── src │ │ └── odom2tf_node.cpp └── tf2odom │ ├── package.xml │ └── CMakeLists.txt ├── path_follower ├── scripts │ ├── record.bash │ └── plot.py ├── test │ └── test_obstacletracker.test ├── src │ ├── local_planner │ │ ├── constraint.cpp │ │ ├── high_speed │ │ │ ├── local_planner_star_g.cpp │ │ │ ├── local_planner_star_n.cpp │ │ │ ├── local_planner_astar_n_static.cpp │ │ │ ├── local_planner_astar_g_static.cpp │ │ │ ├── local_planner_astar_g_reconf.cpp │ │ │ ├── local_planner_astar_n_reconf.cpp │ │ │ ├── local_planner_star_reconf.cpp │ │ │ ├── local_planner_star_static.cpp │ │ │ ├── local_planner_bfs_static.cpp │ │ │ ├── local_planner_bfs_reconf.cpp │ │ │ ├── local_planner_astar.cpp │ │ │ ├── local_planner_static.cpp │ │ │ └── local_planner_bfs.cpp │ │ ├── scorers │ │ │ ├── dis2pathp_scorer.cpp │ │ │ ├── level_scorer.cpp │ │ │ ├── dis2pathd_scorer.cpp │ │ │ ├── curvature_scorer.cpp │ │ │ └── curvatured_scorer.cpp │ │ ├── scorer.cpp │ │ ├── constraints │ │ │ ├── dis2obst_constraint.cpp │ │ │ └── dis2path_constraint.cpp │ │ └── local_planner_null.cpp │ ├── factory │ │ ├── abstract_factory.cpp │ │ └── collision_avoider_factory.cpp │ ├── supervisor │ │ ├── waypointtimeout.cpp │ │ └── supervisorchain.cpp │ ├── utils │ │ ├── elevation_map.cpp │ │ ├── pose_goal_remapper.cpp │ │ └── obstacle_cloud.cpp │ ├── collision_avoidance │ │ ├── collision_detector_omnidrive.cpp │ │ └── collision_avoider.cpp │ └── controller │ │ ├── robotcontroller_ackermann_modelbased.cpp │ │ ├── robotcontroller_ackermann_orthexp.cpp │ │ └── robotcontroller_omnidrive_orthexp.cpp ├── launch │ ├── filter_ramaxx.launch │ ├── follower_potential_field_TT.launch │ ├── follower_ackermann_pid.launch │ ├── follower_potential_field.launch │ ├── follower_unicycle_inputscaling.launch │ ├── follower_differential_orthexp.launch │ ├── follower_ackermann_orthexp.launch │ ├── follower_OFC.launch │ ├── follower_PBR.launch │ ├── filter_sick.launch │ ├── follower_omnidrive_orthexp.launch │ ├── follower_2steer_purepursuit.launch │ ├── follower_2steer_stanley.launch │ ├── follower_kinematic_SLP.launch │ ├── follower_ackermann_purepursuit.launch │ ├── follower_2steer_inputscaling.launch │ ├── follower_dynamic_window.launch │ └── follower_ackermann_stanley.launch ├── include │ └── path_follower │ │ ├── collision_avoidance │ │ ├── collision_detector_omnidrive.h │ │ ├── none_avoider.hpp │ │ ├── collision_detector_ackermann.h │ │ └── collision_detector_polygon.h │ │ ├── local_planner │ │ ├── high_speed │ │ │ ├── local_planner_star_g.h │ │ │ ├── local_planner_star_n.h │ │ │ ├── local_planner_astar.h │ │ │ ├── local_planner_bfs_reconf.h │ │ │ ├── local_planner_bfs_static.h │ │ │ ├── local_planner_star_reconf.h │ │ │ ├── local_planner_star_static.h │ │ │ ├── local_planner_astar_g_reconf.h │ │ │ ├── local_planner_astar_g_static.h │ │ │ ├── local_planner_astar_n_reconf.h │ │ │ ├── local_planner_astar_n_static.h │ │ │ ├── local_planner_static.h │ │ │ ├── local_planner_reconf.h │ │ │ ├── local_planner_bfs.h │ │ │ └── local_planner_star.h │ │ ├── scorers │ │ │ ├── level_scorer.h │ │ │ ├── dis2obst_scorer.h │ │ │ ├── curvature_scorer.h │ │ │ ├── dis2pathd_scorer.h │ │ │ ├── dis2pathp_scorer.h │ │ │ └── curvatured_scorer.h │ │ ├── constraint.h │ │ ├── constraints │ │ │ ├── dis2obst_constraint.h │ │ │ └── dis2path_constraint.h │ │ ├── local_planner_transformer.h │ │ ├── local_planner_null.h │ │ ├── scorer.h │ │ └── high_speed_local_planner.h │ │ ├── supervisor │ │ ├── supervisorchain.h │ │ ├── distancetopathsupervisor.h │ │ └── waypointtimeout.h │ │ ├── factory │ │ ├── abstract_factory.h │ │ └── collision_avoider_factory.h │ │ ├── utils │ │ ├── extended_kalman_filter.h │ │ ├── path_exceptions.h │ │ ├── path_follower_config.h │ │ ├── elevation_map.h │ │ └── maptransformer.h │ │ ├── path_follower_server.h │ │ └── controller │ │ ├── robotcontroller_potential_field_TT.h │ │ ├── robotcontroller_ackermann_modelbased.h │ │ └── robotcontroller_ackermann_orthexp.h └── cmake │ └── FindALGLIB.cmake ├── .gitmodules ├── navigation_launch ├── config │ ├── laser_config.yaml │ ├── laser_config_kobuki.yaml │ ├── laser_config_scitos.yaml │ ├── laser_config_back.yaml │ ├── laser_config_front.yaml │ └── laser_config_leia_front.yaml ├── launch │ ├── karto.launch │ ├── laser_filter_kobuki.launch │ ├── karto_segmented.launch │ ├── scitos.launch │ ├── laser_filter.launch │ ├── laser_filter_scitos.launch │ ├── mapping.launch │ ├── laser_filter_summit.launch │ ├── model_params │ │ ├── model_summit_dwa.launch │ │ ├── model_summit_tree.launch │ │ ├── model_rollator_star.launch │ │ ├── model_rollator_tree.launch │ │ ├── model_summit_star.launch │ │ └── model_rollator_dwa.launch │ └── navigation_static_course.launch ├── CMakeLists.txt └── package.xml ├── path_control ├── launch │ ├── path_control.launch │ └── direct_path_control.launch ├── src │ └── controller_node.cpp └── package.xml ├── .gitignore ├── CONTRIBUTING.md └── LICENSE /.dockerignore: -------------------------------------------------------------------------------- 1 | .git/* -------------------------------------------------------------------------------- /gerona_examples/CATKIN_IGNORE: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docker/kinetic-perception.sh: -------------------------------------------------------------------------------- 1 | docker-build-generic.sh -------------------------------------------------------------------------------- /docker/melodic-perception.sh: -------------------------------------------------------------------------------- 1 | docker-build-generic.sh -------------------------------------------------------------------------------- /docker/noetic-perception.sh: -------------------------------------------------------------------------------- 1 | docker-build-generic.sh -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/node.cpp: -------------------------------------------------------------------------------- 1 | #include "node.h" 2 | 3 | -------------------------------------------------------------------------------- /path_msgs/msg/PathSequence.msg: -------------------------------------------------------------------------------- 1 | ## A sequence of directional paths 2 | 3 | Header header 4 | path_msgs/DirectionalPath[] paths 5 | -------------------------------------------------------------------------------- /gerona/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gerona) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/husky.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cogsys-tuebingen/gerona/HEAD/tools/model_based_planner/config/husky.png -------------------------------------------------------------------------------- /tools/model_based_planner/config/ramaxx.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cogsys-tuebingen/gerona/HEAD/tools/model_based_planner/config/ramaxx.png -------------------------------------------------------------------------------- /tools/model_based_planner/config/Rollator.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cogsys-tuebingen/gerona/HEAD/tools/model_based_planner/config/Rollator.png -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/segment.cpp: -------------------------------------------------------------------------------- 1 | #include "segment.h" 2 | 3 | Segment::Segment(const path_geom::Line& line) 4 | : line(line) 5 | { 6 | } 7 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/summit_16b_512.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cogsys-tuebingen/gerona/HEAD/tools/model_based_planner/config/summit_16b_512.png -------------------------------------------------------------------------------- /gerona_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(gerona_examples) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/summit_wl_16b_512.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cogsys-tuebingen/gerona/HEAD/tools/model_based_planner/config/summit_wl_16b_512.png -------------------------------------------------------------------------------- /path_follower/scripts/record.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | file=$1_$(date +%Y-%m-%d-%H-%M-%S) 3 | rosparam dump /path_follower ${file}.txt 4 | rosbag record -O ${file}.bag /topics 5 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/transition.cpp: -------------------------------------------------------------------------------- 1 | #include "transition.h" 2 | 3 | double Transition::arc_length() const 4 | { 5 | return std::abs(dtheta * r); 6 | } 7 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "utils_path"] 2 | path = utils_path 3 | url = gitlab@gitlab.cs.uni-tuebingen.de:utils/path 4 | [submodule "utils_generic"] 5 | path = utils_generic 6 | url = gitlab@gitlab.cs.uni-tuebingen.de:utils/generic 7 | -------------------------------------------------------------------------------- /path_follower/test/test_obstacletracker.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /path_msgs/msg/DirectionalPath.msg: -------------------------------------------------------------------------------- 1 | ## A path with an associated direction 2 | Header header 3 | 4 | # poses: The waypoints on this path 5 | geometry_msgs/PoseStamped[] poses 6 | 7 | # forward: Determines if the robot should drive forwards or in reverse on this path 8 | bool forward 9 | -------------------------------------------------------------------------------- /path_msgs/msg/Obstacle.msg: -------------------------------------------------------------------------------- 1 | # An obstacle is defined by it's enclosing circle 2 | # position is the center of the circle 3 | 4 | geometry_msgs/Point position 5 | float32 radius 6 | 7 | # Result of the obstacle weighting in path lookout. If this is > 1, the robot will stop. 8 | float32 weight 9 | -------------------------------------------------------------------------------- /tools/scan2cloud/src/scan2cloud_node.cpp: -------------------------------------------------------------------------------- 1 | #include "../include/scan2cloud/scan2cloud.h" 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ros::init(argc, argv, "scan2cloud", ros::init_options::NoSigintHandler); 6 | 7 | ScanConverter filter; 8 | 9 | filter.spin(); 10 | 11 | return 0; 12 | } 13 | -------------------------------------------------------------------------------- /docker/docker-build-generic.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh -e 2 | 3 | SCRIPT_PATH=$(readlink -f $0) 4 | SCRIPT_DIR=$(dirname $SCRIPT_PATH) 5 | 6 | TYPE=$(basename $0 .sh) 7 | DOCKERFILE=${SCRIPT_DIR}/${TYPE}.docker 8 | 9 | set -x 10 | cd $SCRIPT_DIR/../.. 11 | pwd 12 | docker build -f ${DOCKERFILE} -t gerona-${TYPE} --pull --network=host . -------------------------------------------------------------------------------- /navigation_launch/config/laser_config.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: shadows 3 | type: ScanShadowsFilter 4 | params: 5 | min_angle: 10 6 | max_angle: 170 7 | neighbors: 20 8 | window: 1 9 | - name: footprint_filter 10 | type: LaserScanFootprintFilter 11 | params: 12 | inscribed_radius: 0.42 13 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/constraint.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | Constraint::Constraint() 5 | { 6 | sw.resetStopped(); 7 | } 8 | 9 | Constraint::~Constraint() 10 | { 11 | 12 | } 13 | 14 | long Constraint::nsUsed(){ 15 | return sw.nsElapsedStatic(); 16 | } 17 | -------------------------------------------------------------------------------- /navigation_launch/config/laser_config_kobuki.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: angular_low 3 | type: LaserScanAngularBoundsFilter 4 | params: 5 | lower_angle: -3.1415 6 | upper_angle: -1.7 7 | - name: angular_up 8 | type: LaserScanAngularBoundsFilter 9 | params: 10 | lower_angle: 1.7 11 | upper_angle: 3.1415 12 | 13 | -------------------------------------------------------------------------------- /navigation_launch/config/laser_config_scitos.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: angular_low 3 | type: LaserScanAngularBoundsFilter 4 | params: 5 | lower_angle: -3.1415 6 | upper_angle: -1.4 7 | - name: angular_up 8 | type: LaserScanAngularBoundsFilter 9 | params: 10 | lower_angle: 1.4 11 | upper_angle: 3.1415 12 | 13 | -------------------------------------------------------------------------------- /gerona_examples/launch/include/gazebo2tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /path_control/launch/path_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /navigation_launch/launch/karto.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /tools/localmap/config/dem_local_640_10.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | Proc: 4 | numAngleStep: 360 5 | heightScale: 1000. 6 | mapBaseHeight: 10000 7 | wheelGroundLevel: 20000 8 | maxHeight: 30000 9 | pixelSize: 0.015625 10 | imagePosBLMinX: -5 11 | imagePosBLMinY: -5 12 | validThresholdFactor: 9.4999998807907104e-01 13 | convertImage: 0 14 | -------------------------------------------------------------------------------- /tools/localmap/config/dem_local_640_8.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | Proc: 4 | numAngleStep: 360 5 | heightScale: 1000. 6 | mapBaseHeight: 10000 7 | wheelGroundLevel: 20000 8 | maxHeight: 30000 9 | pixelSize: 0.0125 10 | imagePosBLMinX: -5 11 | imagePosBLMinY: -5 12 | validThresholdFactor: 9.4999998807907104e-01 13 | convertImage: 0 14 | -------------------------------------------------------------------------------- /path_follower/launch/filter_ramaxx.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | -------------------------------------------------------------------------------- /tools/localmap/config/dem_local_1024_10.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | Proc: 4 | numAngleStep: 360 5 | heightScale: 1000. 6 | mapBaseHeight: 10000 7 | wheelGroundLevel: 20000 8 | maxHeight: 30000 9 | pixelSize: 0.009765625 10 | imagePosBLMinX: -5 11 | imagePosBLMinY: -5 12 | validThresholdFactor: 9.4999998807907104e-01 13 | convertImage: 0 14 | -------------------------------------------------------------------------------- /gerona_examples/costmap/minimal.yaml: -------------------------------------------------------------------------------- 1 | plugins: 2 | - {name: static_map, type: "costmap_2d::StaticLayer"} 3 | - {name: obstacles, type: "costmap_2d::VoxelLayer"} 4 | publish_frequency: 1.0 5 | obstacles: 6 | observation_sources: base_scan 7 | base_scan: {data_type: LaserScan, sensor_frame: /base_laser_link, clearing: true, marking: true, topic: /hokuyo_scan} -------------------------------------------------------------------------------- /tools/path_rviz/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Displays a path sequence. 7 | 8 | path_msgs/PathSequence 9 | 10 | 11 | -------------------------------------------------------------------------------- /navigation_launch/launch/laser_filter_kobuki.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_star_g.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | 6 | 7 | LocalPlannerStarG::LocalPlannerStarG() 8 | { 9 | 10 | } 11 | 12 | double LocalPlannerStarG::f(double& g, double& score, double& heuristic){ 13 | (void) g; 14 | return score + heuristic; 15 | } 16 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_star_n.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | 6 | 7 | LocalPlannerStarN::LocalPlannerStarN() 8 | { 9 | 10 | } 11 | 12 | double LocalPlannerStarN::f(double& g, double& score, double& heuristic){ 13 | (void) score; 14 | return g + heuristic; 15 | } 16 | -------------------------------------------------------------------------------- /tools/localmap/config/dem_local_768_8.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | Proc: 4 | numAngleStep: 360 5 | heightScale: 1000. 6 | mapBaseHeight: 10000 7 | notVisibleLevel: 1000 8 | wheelGroundLevel: 20000 9 | maxHeight: 30000 10 | pixelSize: 0.010416667 11 | imagePosBLMinX: -4 12 | imagePosBLMinY: -4 13 | validThresholdFactor: 9.4999998807907104e-01 14 | convertImage: 0 15 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_astar_n_static.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | #include 6 | 7 | REGISTER_LOCAL_PLANNER(LocalPlannerAStarNStatic, HS_AStar); 8 | 9 | LocalPlannerAStarNStatic::LocalPlannerAStarNStatic() 10 | { 11 | 12 | } 13 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_astar_g_static.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | #include 6 | 7 | REGISTER_LOCAL_PLANNER(LocalPlannerAStarGStatic, HS_AStarG); 8 | 9 | LocalPlannerAStarGStatic::LocalPlannerAStarGStatic() 10 | { 11 | 12 | } 13 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_astar_g_reconf.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | #include 6 | 7 | REGISTER_LOCAL_PLANNER(LocalPlannerAStarGReconf, HS_AStarGR); 8 | 9 | 10 | LocalPlannerAStarGReconf::LocalPlannerAStarGReconf() 11 | { 12 | 13 | } 14 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_astar_n_reconf.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | #include 6 | 7 | REGISTER_LOCAL_PLANNER(LocalPlannerAStarNReconf, HS_AStarR); 8 | 9 | 10 | LocalPlannerAStarNReconf::LocalPlannerAStarNReconf() 11 | { 12 | 13 | } 14 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_star_reconf.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | 6 | 7 | LocalPlannerStarReconf::LocalPlannerStarReconf() 8 | { 9 | 10 | } 11 | 12 | void LocalPlannerStarReconf::evaluate(double& current_p, double& heuristic, double& score){ 13 | current_p = heuristic + score; 14 | } 15 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_star_static.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | 6 | 7 | LocalPlannerStarStatic::LocalPlannerStarStatic() 8 | { 9 | 10 | } 11 | 12 | void LocalPlannerStarStatic::evaluate(double& current_p, double& heuristic, double& score){ 13 | current_p = heuristic + score; 14 | } 15 | -------------------------------------------------------------------------------- /path_control/launch/direct_path_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /path_follower/src/factory/abstract_factory.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | #include 5 | 6 | AbstractFactory::~AbstractFactory() 7 | { 8 | 9 | } 10 | 11 | std::string AbstractFactory::toLower(const std::string& s) 12 | { 13 | std::string s_low = s; 14 | std::transform(s_low.begin(), s_low.end(), s_low.begin(), ::tolower); 15 | return s_low; 16 | } 17 | -------------------------------------------------------------------------------- /navigation_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(navigation_launch) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | ) 8 | 9 | # this is to list all launch files in qtcreator 10 | file(GLOB_RECURSE ${PROJECT_NAME}_launch_files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS launch/*.launch) 11 | add_custom_target(list_all_launch_files SOURCES ${${PROJECT_NAME}_launch_files}) 12 | -------------------------------------------------------------------------------- /path_control/src/controller_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "pathcontroller.h" 3 | 4 | int main(int argc, char** argv) { 5 | ros::init(argc, argv, "path_control_node", ros::init_options::NoSigintHandler); 6 | ros::NodeHandle nh; 7 | 8 | PathController pc(nh); 9 | 10 | ros::Rate r(60); 11 | while(ros::ok()) { 12 | ros::spinOnce(); 13 | r.sleep(); 14 | } 15 | return 0; 16 | } 17 | 18 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/segment.h: -------------------------------------------------------------------------------- 1 | #ifndef SEGMENT_H 2 | #define SEGMENT_H 3 | 4 | #include 5 | #include "transition.h" 6 | 7 | class Segment 8 | { 9 | public: 10 | path_geom::Line line; 11 | std::vector forward_transitions; 12 | std::vector backward_transitions; 13 | 14 | Segment(const path_geom::Line& line); 15 | }; 16 | 17 | #endif // SEGMENT_H 18 | -------------------------------------------------------------------------------- /tools/roimap/launch/roimap_default.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /navigation_launch/launch/karto_segmented.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /navigation_launch/config/laser_config_back.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: better_footprint_filter 3 | type: laser_filters/PolygonLaserFilter 4 | params: 5 | calibrate: 0 6 | polygon_path: "/localhome/widmaier/ws/misc/src/sick-robot-day-2014/laser_filter/laser_polygon_back.xml" 7 | polygon_marker_id: 1 8 | calibration_min_cycles: 100 9 | calibration_max_no_change: 10 10 | calibration_radius: 0.5 11 | calibration_growth_ratio: 1.3 12 | -------------------------------------------------------------------------------- /navigation_launch/config/laser_config_front.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: better_footprint_filter 3 | type: laser_filters/PolygonLaserFilter 4 | params: 5 | calibrate: 0 6 | polygon_path: "/localhome/widmaier/ws/misc/src/sick-robot-day-2014/laser_filter/laser_polygon_front.xml" 7 | polygon_marker_id: 1 8 | calibration_min_cycles: 100 9 | calibration_max_no_change: 10 10 | calibration_radius: 0.5 11 | calibration_growth_ratio: 1.3 12 | -------------------------------------------------------------------------------- /docker/melodic-perception.docker: -------------------------------------------------------------------------------- 1 | FROM ros:melodic-perception 2 | 3 | RUN apt-get update -y && \ 4 | apt-get install -y --no-install-recommends build-essential openssh-client sudo 5 | 6 | RUN mkdir -p /build/ws/src/ 7 | ADD ./ /build/ws/src/ 8 | 9 | SHELL [ "bash", "-c" ] 10 | 11 | RUN apt-get update -y && rosdep update 12 | RUN rosdep install --from-paths -r -i -y /build/ws/src 13 | RUN source /opt/ros/$(ls /opt/ros/ | head -n 1)/setup.bash && cd /build/ws/ && catkin_make_isolated 14 | -------------------------------------------------------------------------------- /docker/noetic-perception.docker: -------------------------------------------------------------------------------- 1 | FROM ros:noetic-perception 2 | 3 | RUN apt-get update -y && \ 4 | apt-get install -y --no-install-recommends build-essential openssh-client sudo 5 | 6 | RUN mkdir -p /build/ws/src/ 7 | ADD ./ /build/ws/src/ 8 | 9 | SHELL [ "bash", "-c" ] 10 | 11 | RUN apt-get update -y && rosdep update 12 | RUN rosdep install --from-paths -r -i -y /build/ws/src 13 | RUN source /opt/ros/$(ls /opt/ros/ | head -n 1)/setup.bash && cd /build/ws/ && catkin_make_isolated 14 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/collision_avoidance/collision_detector_omnidrive.h: -------------------------------------------------------------------------------- 1 | #ifndef COLLISION_DETECTOR_OMNIDRIVE_H 2 | #define COLLISION_DETECTOR_OMNIDRIVE_H 3 | 4 | #include "collision_detector_polygon.h" 5 | 6 | class CollisionDetectorOmnidrive : public CollisionDetectorPolygon 7 | { 8 | protected: 9 | virtual PolygonWithTfFrame getPolygon(float width, float length, float course_angle, float curve_enlarge_factor) const; 10 | }; 11 | 12 | #endif // COLLISION_DETECTOR_OMNIDRIVE_H 13 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_star_g.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_STAR_G_H 2 | #define LOCAL_PLANNER_STAR_G_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerStarG : virtual public LocalPlannerStar 8 | { 9 | public: 10 | LocalPlannerStarG(); 11 | private: 12 | virtual double f(double& g, double& score, double& heuristic) override; 13 | }; 14 | 15 | #endif // LOCAL_PLANNER_STAR_G_H 16 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_star_n.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_STAR_N_H 2 | #define LOCAL_PLANNER_STAR_N_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerStarN : virtual public LocalPlannerStar 8 | { 9 | public: 10 | LocalPlannerStarN(); 11 | private: 12 | virtual double f(double& g, double& score, double& heuristic) override; 13 | }; 14 | 15 | #endif // LOCAL_PLANNER_STAR_N_H 16 | -------------------------------------------------------------------------------- /path_planner/launch/goal_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /tools/model_based_planner/include/model_based_planner/wheeldescriptor.h: -------------------------------------------------------------------------------- 1 | #ifndef WHEELDESCRIPTOR_H 2 | #define WHEELDESCRIPTOR_H 3 | 4 | #include "cv_aligned_mat.h" 5 | 6 | 7 | /** 8 | * @brief Wheel descriptor for one wheel orientation 9 | */ 10 | struct WheelDescriptor 11 | { 12 | 13 | cv::Point2f centerImg_,jointPosImg_; 14 | 15 | cv::Point2f dirX_, dirY_; 16 | 17 | int numImagePixels_; 18 | float numImagePixelsInv_; 19 | 20 | CVAlignedMat::ptr image_; 21 | }; 22 | 23 | #endif // WHEELDESCRIPTOR_H 24 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/transition.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSITION_H 2 | #define TRANSITION_H 3 | 4 | #include 5 | #include 6 | 7 | class Segment; 8 | 9 | class Transition { 10 | public: 11 | double arc_length() const; 12 | 13 | const Segment* source; 14 | const Segment* target; 15 | 16 | Eigen::Vector2d intersection; 17 | Eigen::Vector2d icr; 18 | 19 | double r; 20 | double dtheta; 21 | 22 | std::vector path; 23 | }; 24 | 25 | #endif // TRANSITION_H 26 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/supervisor/supervisorchain.h: -------------------------------------------------------------------------------- 1 | #ifndef SUPERVISORCHAIN_H 2 | #define SUPERVISORCHAIN_H 3 | 4 | #include 5 | #include 6 | 7 | class SupervisorChain 8 | { 9 | public: 10 | void addSupervisor(Supervisor::Ptr supervisor); 11 | 12 | Supervisor::Result supervise(Supervisor::State &state); 13 | 14 | void notifyNewGoal(); 15 | void notifyNewWaypoint(); 16 | 17 | private: 18 | std::list supervisors_; 19 | }; 20 | 21 | #endif // SUPERVISORCHAIN_H 22 | -------------------------------------------------------------------------------- /path_follower/launch/follower_potential_field_TT.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_bfs_static.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | #include 6 | 7 | REGISTER_LOCAL_PLANNER(LocalPlannerBFSStatic, HS_BFS); 8 | 9 | 10 | LocalPlannerBFSStatic::LocalPlannerBFSStatic() 11 | { 12 | 13 | } 14 | 15 | void LocalPlannerBFSStatic::evaluate(double& current_p, LNode*& succ, double& dis2last) 16 | { 17 | current_p = Heuristic(*succ, dis2last) + Score(*succ); 18 | } 19 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_bfs_reconf.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | #include 6 | 7 | REGISTER_LOCAL_PLANNER(LocalPlannerBFSReconf, HS_BFSR); 8 | 9 | 10 | LocalPlannerBFSReconf::LocalPlannerBFSReconf() 11 | { 12 | 13 | } 14 | 15 | void LocalPlannerBFSReconf::evaluate(double& current_p, LNode*& succ, double& dis2last){ 16 | (void) current_p; 17 | (void) succ; 18 | (void) dis2last; 19 | } 20 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/scorers/level_scorer.h: -------------------------------------------------------------------------------- 1 | #ifndef LEVEL_SCORER_H 2 | #define LEVEL_SCORER_H 3 | 4 | #include 5 | 6 | class Level_Scorer : public Scorer 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | Level_Scorer(); 13 | virtual ~Level_Scorer(); 14 | 15 | static void setLevel(const int& m_level); 16 | 17 | virtual double score(const LNode& point) override; 18 | 19 | private: 20 | static int max_level; 21 | }; 22 | 23 | #endif // LEVEL_SCORER_H 24 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/scorers/dis2pathp_scorer.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | double Dis2PathP_Scorer::MAX_DIS = 0.3; 5 | 6 | Dis2PathP_Scorer::Dis2PathP_Scorer(): 7 | Scorer() 8 | { 9 | 10 | } 11 | 12 | Dis2PathP_Scorer::~Dis2PathP_Scorer() 13 | { 14 | 15 | } 16 | 17 | void Dis2PathP_Scorer::setMaxD(double& dis){ 18 | MAX_DIS = dis; 19 | } 20 | 21 | double Dis2PathP_Scorer::score(const LNode& point){ 22 | sw.resume(); 23 | double p = point.d2p; 24 | sw.stop(); 25 | return p; 26 | } 27 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/scorers/level_scorer.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | int Level_Scorer::max_level = 10; 5 | 6 | Level_Scorer::Level_Scorer(): 7 | Scorer() 8 | { 9 | 10 | } 11 | 12 | Level_Scorer::~Level_Scorer() 13 | { 14 | 15 | } 16 | 17 | void Level_Scorer::setLevel(const int& m_level){ 18 | max_level = m_level; 19 | } 20 | 21 | double Level_Scorer::score(const LNode& point){ 22 | sw.resume(); 23 | double ls = (double)(max_level - point.level_); 24 | sw.stop(); 25 | return ls; 26 | } 27 | -------------------------------------------------------------------------------- /path_follower/launch/follower_ackermann_pid.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/scorers/dis2obst_scorer.h: -------------------------------------------------------------------------------- 1 | #ifndef DIS2OBST_SCORER_H 2 | #define DIS2OBST_SCORER_H 3 | 4 | #include 5 | 6 | class Dis2Obst_Scorer : public Scorer 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | Dis2Obst_Scorer(); 13 | virtual ~Dis2Obst_Scorer(); 14 | static void setFactor(double factor); 15 | 16 | virtual double score(const LNode& point) override; 17 | 18 | private: 19 | static double factor_; 20 | }; 21 | 22 | #endif // DIS2PATH_SCORER_H 23 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/scorer.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | Scorer::Scorer() 5 | : weight_(1.0) 6 | { 7 | sw.resetStopped(); 8 | } 9 | 10 | Scorer::~Scorer() 11 | { 12 | 13 | } 14 | 15 | long Scorer::nsUsed(){ 16 | return sw.nsElapsedStatic(); 17 | } 18 | 19 | double Scorer::getWeight() const 20 | { 21 | return weight_; 22 | } 23 | void Scorer::setWeight(double weight) 24 | { 25 | weight_ = weight; 26 | } 27 | 28 | double Scorer::calculateScore(const LNode& point) 29 | { 30 | return weight_ * score(point); 31 | } 32 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/scorers/curvature_scorer.h: -------------------------------------------------------------------------------- 1 | #ifndef CURVATURE_SCORER_H 2 | #define CURVATURE_SCORER_H 3 | 4 | #include 5 | 6 | class Curvature_Scorer : public Scorer 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | Curvature_Scorer(); 13 | virtual ~Curvature_Scorer(); 14 | static void setMaxC(double& radius); 15 | 16 | virtual double score(const LNode& point) override; 17 | private: 18 | static double MAX_CURV; 19 | }; 20 | 21 | #endif // CURVATURE_SCORER_H 22 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/scorers/dis2pathd_scorer.h: -------------------------------------------------------------------------------- 1 | #ifndef DIS2PATHD_SCORER_H 2 | #define DIS2PATHD_SCORER_H 3 | 4 | #include 5 | 6 | class Dis2PathD_Scorer : public Scorer 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | Dis2PathD_Scorer(); 13 | virtual ~Dis2PathD_Scorer(); 14 | static void setMaxD(double& dis); 15 | 16 | virtual double score(const LNode& point) override; 17 | 18 | private: 19 | static double MAX_DIS; 20 | }; 21 | 22 | #endif // DIS2PATHD_SCORER_H 23 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/scorers/dis2pathp_scorer.h: -------------------------------------------------------------------------------- 1 | #ifndef DIS2PATHP_SCORER_H 2 | #define DIS2PATHP_SCORER_H 3 | 4 | #include 5 | 6 | class Dis2PathP_Scorer : public Scorer 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | Dis2PathP_Scorer(); 13 | virtual ~Dis2PathP_Scorer(); 14 | static void setMaxD(double& dis); 15 | 16 | virtual double score(const LNode& point) override; 17 | 18 | private: 19 | static double MAX_DIS; 20 | }; 21 | 22 | #endif // DIS2PATHP_SCORER_H 23 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/scorers/curvatured_scorer.h: -------------------------------------------------------------------------------- 1 | #ifndef CURVATURED_SCORER_H 2 | #define CURVATURED_SCORER_H 3 | 4 | #include 5 | 6 | class CurvatureD_Scorer : public Scorer 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | CurvatureD_Scorer(); 13 | virtual ~CurvatureD_Scorer(); 14 | static void setMaxC(double& radius); 15 | 16 | virtual double score(const LNode& point) override; 17 | private: 18 | static double MAX_CURV; 19 | }; 20 | 21 | #endif // CURVATURED_SCORER_H 22 | -------------------------------------------------------------------------------- /path_follower/launch/follower_potential_field.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef CONSTRAINT_H 2 | #define CONSTRAINT_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class Constraint 10 | { 11 | public: 12 | typedef std::shared_ptr Ptr; 13 | 14 | public: 15 | Constraint(); 16 | virtual ~Constraint(); 17 | 18 | virtual bool isSatisfied(const LNode& point) = 0; 19 | long nsUsed(); 20 | protected: 21 | Stopwatch sw; 22 | }; 23 | 24 | #endif // CONSTRAINT_H 25 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_astar.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_ASTAR_H 2 | #define LOCAL_PLANNER_ASTAR_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerAStar : virtual public LocalPlannerStar 8 | { 9 | public: 10 | LocalPlannerAStar(); 11 | private: 12 | virtual double G(LNode*& current, LNode*& succ, 13 | double& score) override; 14 | 15 | virtual void updateSucc(LNode*& current, LNode*& f_current, LNode& succ) override; 16 | }; 17 | 18 | #endif // LOCAL_PLANNER_ASTAR_H 19 | -------------------------------------------------------------------------------- /tools/roimap/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roimap) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | nav_msgs 6 | roscpp 7 | ) 8 | 9 | find_package(OpenCV REQUIRED) 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS nav_msgs 13 | ) 14 | 15 | include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) 16 | 17 | add_executable(${PROJECT_NAME}_node src/RoiMapNode.cpp) 18 | target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) 19 | 20 | 21 | install(TARGETS ${PROJECT_NAME}_node 22 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 23 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/constraints/dis2obst_constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef DIS2OBST_CONSTRAINT_H 2 | #define DIS2OBST_CONSTRAINT_H 3 | 4 | #include 5 | 6 | class Dis2Obst_Constraint : public Constraint 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | Dis2Obst_Constraint(); 13 | virtual ~Dis2Obst_Constraint(); 14 | void setParams(double threshold); 15 | 16 | virtual bool isSatisfied(const LNode& point) override; 17 | private: 18 | double threshold; 19 | }; 20 | 21 | #endif // DIS2OBST_CONSTRAINT_H 22 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_bfs_reconf.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_BFS_RECONF_H 2 | #define LOCAL_PLANNER_BFS_RECONF_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | 8 | class LocalPlannerBFSReconf : public LocalPlannerBFS, public LocalPlannerReconf 9 | { 10 | public: 11 | LocalPlannerBFSReconf(); 12 | private: 13 | virtual void evaluate(double& current_p, LNode*& succ, double& dis2last) override; 14 | }; 15 | 16 | #endif // LOCAL_PLANNER_BFS_RECONF_H 17 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_bfs_static.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_BFS_STATIC_H 2 | #define LOCAL_PLANNER_BFS_STATIC_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | 8 | class LocalPlannerBFSStatic : public LocalPlannerBFS, public LocalPlannerStatic 9 | { 10 | public: 11 | LocalPlannerBFSStatic(); 12 | private: 13 | virtual void evaluate(double& current_p, LNode*& succ, double& dis2last) override; 14 | }; 15 | 16 | #endif // LOCAL_PLANNER_BFS_STATIC_H 17 | -------------------------------------------------------------------------------- /tools/model_based_planner/include/model_based_planner/utils_diff.h: -------------------------------------------------------------------------------- 1 | #ifndef UTILS_DIFF_H 2 | #define UTILS_DIFF_H 3 | 4 | 5 | 6 | /** 7 | * @brief Include SIMD functions depending on requested CPU architecture 8 | */ 9 | #if (defined(USE_AVX2) && defined(__AVX2__)) 10 | #include "utils_diff_axv.h" 11 | #elif __SSE4_2__ 12 | #include "utils_diff_sse.h" 13 | #else 14 | #include "utils_diff_nosimd.h" 15 | #endif 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | #endif // UTILS_DIFF_H 29 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/node.h: -------------------------------------------------------------------------------- 1 | #ifndef NODE_H 2 | #define NODE_H 3 | 4 | #include 5 | 6 | class Transition; 7 | class Segment; 8 | 9 | struct Node { 10 | // associated transition 11 | const Transition* transition = nullptr; 12 | 13 | const Segment* next_segment = nullptr; 14 | 15 | bool curve_forward= true; 16 | 17 | // distance travelled until this transition is reached 18 | double cost = std::numeric_limits::infinity(); 19 | 20 | // node via which this transition is reached 21 | Node* prev = nullptr; 22 | Node* next = nullptr; 23 | }; 24 | 25 | #endif // NODE_H 26 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/factory/abstract_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef ABSTRACT_FACTORY_H 2 | #define ABSTRACT_FACTORY_H 3 | 4 | #include 5 | 6 | /** 7 | * @brief The AbstractFactory class is the base class for all factories. 8 | */ 9 | class AbstractFactory 10 | { 11 | public: 12 | virtual ~AbstractFactory(); 13 | 14 | protected: 15 | /** 16 | * @brief toLower Convenience function to transform a string to lower case. 17 | * @param s String in any case 18 | * @return s transformed to lower case 19 | */ 20 | static std::string toLower(const std::string& s); 21 | }; 22 | 23 | #endif // ABSTRACT_FACTORY_H 24 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_star_reconf.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_STAR_RECONF_H 2 | #define LOCAL_PLANNER_STAR_RECONF_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | 8 | class LocalPlannerStarReconf : virtual public LocalPlannerStar, virtual public LocalPlannerReconf 9 | { 10 | public: 11 | LocalPlannerStarReconf(); 12 | private: 13 | virtual void evaluate(double& current_p, double& heuristic, double& score) override; 14 | }; 15 | 16 | #endif // LOCAL_PLANNER_STAR_RECONF_H 17 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_star_static.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_STAR_STATIC_H 2 | #define LOCAL_PLANNER_STAR_STATIC_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | 8 | class LocalPlannerStarStatic : virtual public LocalPlannerStar, virtual public LocalPlannerStatic 9 | { 10 | public: 11 | LocalPlannerStarStatic(); 12 | private: 13 | virtual void evaluate(double& current_p, double& heuristic, double& score) override; 14 | }; 15 | 16 | #endif // LOCAL_PLANNER_STAR_STATIC_H 17 | -------------------------------------------------------------------------------- /tools/model_based_planner/include/model_based_planner/robotdescriptor.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTDESCRIPTOR_H 2 | #define ROBOTDESCRIPTOR_H 3 | 4 | #include 5 | 6 | /** 7 | * @brief Robot descriptor for one vehicle orientation 8 | */ 9 | struct RobotDescriptor 10 | { 11 | 12 | inline cv::Point2f GetPixelPos(const int &idx){ return wheelPositionsImage_[idx]; } 13 | 14 | 15 | std::vector wheelPositionsImage_; 16 | 17 | cv::Point2f chassisPosImage_; 18 | 19 | 20 | cv::Point2f baseLinkPosImage_; 21 | 22 | 23 | cv::Mat rotMat_; 24 | 25 | float sina_,cosa_; 26 | }; 27 | 28 | #endif // ROBOTDESCRIPTOR_H 29 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_astar_g_reconf.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_ASTAR_G_RECONF_H 2 | #define LOCAL_PLANNER_ASTAR_G_RECONF_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | #include 8 | 9 | class LocalPlannerAStarGReconf : public LocalPlannerAStar, public LocalPlannerStarG, public LocalPlannerStarReconf 10 | { 11 | public: 12 | LocalPlannerAStarGReconf(); 13 | }; 14 | 15 | #endif // LOCAL_PLANNER_ASTAR_G_RECONF_H 16 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_astar_g_static.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_ASTAR_G_STATIC_H 2 | #define LOCAL_PLANNER_ASTAR_G_STATIC_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | #include 8 | 9 | class LocalPlannerAStarGStatic : public LocalPlannerAStar, public LocalPlannerStarG, public LocalPlannerStarStatic 10 | { 11 | public: 12 | LocalPlannerAStarGStatic(); 13 | }; 14 | 15 | #endif // LOCAL_PLANNER_ASTAR_G_STATIC_H 16 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_astar_n_reconf.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_ASTAR_N_RECONF_H 2 | #define LOCAL_PLANNER_ASTAR_N_RECONF_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | #include 8 | 9 | class LocalPlannerAStarNReconf : public LocalPlannerAStar, public LocalPlannerStarN, public LocalPlannerStarReconf 10 | { 11 | public: 12 | LocalPlannerAStarNReconf(); 13 | }; 14 | 15 | #endif // LOCAL_PLANNER_ASTAR_N_RECONF_H 16 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_astar_n_static.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_ASTAR_N_STATIC_H 2 | #define LOCAL_PLANNER_ASTAR_N_STATIC_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | #include 8 | 9 | class LocalPlannerAStarNStatic : public LocalPlannerAStar, public LocalPlannerStarN, public LocalPlannerStarStatic 10 | { 11 | public: 12 | LocalPlannerAStarNStatic(); 13 | }; 14 | 15 | #endif // LOCAL_PLANNER_ASTAR_N_STATIC_H 16 | -------------------------------------------------------------------------------- /path_follower/launch/follower_unicycle_inputscaling.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/constraints/dis2obst_constraint.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | Dis2Obst_Constraint::Dis2Obst_Constraint(): 5 | Constraint() 6 | { 7 | 8 | } 9 | 10 | Dis2Obst_Constraint::~Dis2Obst_Constraint() 11 | { 12 | 13 | } 14 | 15 | void Dis2Obst_Constraint::setParams(double obstacle_threshold){ 16 | threshold = obstacle_threshold; 17 | } 18 | 19 | bool Dis2Obst_Constraint::isSatisfied(const LNode& point){ 20 | sw.resume(); 21 | if(point.d2o <= threshold){ 22 | sw.stop(); 23 | return false; 24 | } 25 | sw.stop(); 26 | return true; 27 | } 28 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/collision_avoidance/none_avoider.hpp: -------------------------------------------------------------------------------- 1 | #ifndef NONE_AVOIDER_H 2 | #define NONE_AVOIDER_H 3 | 4 | #include 5 | 6 | /** 7 | * @brief Dummy avoider that does nothing. Use this to deactivate obstacle avoidance. 8 | */ 9 | class NoneAvoider : public CollisionAvoider 10 | { 11 | public: 12 | virtual bool avoid(MoveCommand* const cmd, const State &) 13 | { 14 | if (externalError_ != 0) 15 | { 16 | cmd->setVelocity(0); 17 | return true; 18 | } 19 | // pass 20 | return false; 21 | } 22 | }; 23 | 24 | #endif // NONEAVOIDER_H 25 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/scorers/dis2pathd_scorer.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | double Dis2PathD_Scorer::MAX_DIS = 0.3; 5 | 6 | Dis2PathD_Scorer::Dis2PathD_Scorer(): 7 | Scorer() 8 | { 9 | 10 | } 11 | 12 | Dis2PathD_Scorer::~Dis2PathD_Scorer() 13 | { 14 | 15 | } 16 | 17 | void Dis2PathD_Scorer::setMaxD(double& dis){ 18 | MAX_DIS = dis; 19 | } 20 | 21 | double Dis2PathD_Scorer::score(const LNode& point){ 22 | sw.resume(); 23 | double diff = 0.0; 24 | if(point.parent_ != nullptr){ 25 | diff = (point.d2p - point.parent_->d2p); 26 | } 27 | sw.stop(); 28 | return diff; 29 | } 30 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course_planner_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | @year 2015 4 | @author Karsten Bohlmann 5 | @email bohlmann@gmail.com 6 | @file course_planner_node.cpp 7 | 8 | 9 | */ 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "course_planner.h" 15 | 16 | 17 | int main(int argc, char** argv) 18 | { 19 | ros::init(argc, argv, "course_planner", ros::init_options::NoSigintHandler); 20 | 21 | CoursePlanner planner; 22 | 23 | ros::WallRate r(2); 24 | while(ros::ok()){ 25 | ros::spinOnce(); 26 | r.sleep(); 27 | 28 | planner.tick(); 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /navigation_launch/launch/scitos.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/local_planner_transformer.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_TRANSFORMER_H 2 | #define LOCAL_PLANNER_TRANSFORMER_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerTransformer : public AbstractLocalPlanner 8 | { 9 | public: 10 | LocalPlannerTransformer(); 11 | 12 | virtual void setVelocity(geometry_msgs::Twist::_linear_type vector) override; 13 | virtual void setVelocity(double velocity) override; 14 | 15 | virtual Path::Ptr updateLocalPath() override; 16 | 17 | private: 18 | virtual void setParams(const LocalPlannerParameters& opt) override; 19 | }; 20 | 21 | #endif // LOCAL_PLANNER_TRANSFORMER_H 22 | -------------------------------------------------------------------------------- /path_follower/launch/follower_differential_orthexp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/scorers/curvature_scorer.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | double Curvature_Scorer::MAX_CURV = 0.0; 5 | 6 | Curvature_Scorer::Curvature_Scorer(): 7 | Scorer() 8 | { 9 | 10 | } 11 | 12 | Curvature_Scorer::~Curvature_Scorer() 13 | { 14 | 15 | } 16 | 17 | void Curvature_Scorer::setMaxC(double& radius){ 18 | MAX_CURV = 1.0/radius; 19 | } 20 | 21 | double Curvature_Scorer::score(const LNode& point){ 22 | sw.resume(); 23 | if(point.radius_ < std::numeric_limits::infinity()){ 24 | double div = std::abs(1.0/point.radius_); 25 | sw.stop(); 26 | return div; 27 | } 28 | sw.stop(); 29 | return 0.0; 30 | } 31 | -------------------------------------------------------------------------------- /tools/model_based_planner/include/model_based_planner/chassisdescriptor.h: -------------------------------------------------------------------------------- 1 | #ifndef CHASSISDESCRIPTOR_H 2 | #define CHASSISDESCRIPTOR_H 3 | 4 | #include "cv_aligned_mat.h" 5 | 6 | 7 | /** 8 | * @brief ChassisDescriptor for one vehicle orientation 9 | */ 10 | struct ChassisDescriptor 11 | { 12 | 13 | /** 14 | * @brief location of the chassis image 15 | */ 16 | cv::Point2f centerImg_; 17 | 18 | /** 19 | * @brief x and y direction on the chassis image (the vehicle is rotated, the image still has normal top right coordinates) 20 | */ 21 | cv::Point2f dirX_, dirY_; 22 | 23 | /** 24 | * @brief Chassis image 25 | */ 26 | CVAlignedMat::ptr image_; 27 | 28 | }; 29 | 30 | #endif // CHASSISDESCRIPTOR_H 31 | -------------------------------------------------------------------------------- /tools/model_based_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | model_based_planner 4 | 1.0.0 5 | The model_based_planner package 6 | 7 | Julian Jordan 8 | 9 | Goran Huskic 10 | Sebastian Buck 11 | Julian Jordan 12 | Adrian Zwiener 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /navigation_launch/config/laser_config_leia_front.yaml: -------------------------------------------------------------------------------- 1 | scan_filter_chain: 2 | - name: shadows 3 | type: ScanShadowsFilter 4 | params: 5 | min_angle: 20 6 | max_angle: 180 7 | neighbors: 5 8 | window: 1 9 | #- name: ray_filter 10 | # type: laser_filters/RayLaserFilter 11 | # params: 12 | # calibrate: 0 # bool if filter should be calibrated first 13 | # calib_file_path: "/home/robot/.ray_filter_leia_front_calibration.yaml" 14 | # calibration_cycles: 300 # the number of scans used for callibration 15 | # calibration_radius: 0.8 # all rays with range below this value are beeing filtered out 16 | # extra_rays: 2 # the number of valid rays around the invalid rays that are beeing filtered to be shure 17 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/local_planner_null.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_NULL_H 2 | #define LOCAL_PLANNER_NULL_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerNull : public AbstractLocalPlanner 8 | { 9 | public: 10 | LocalPlannerNull(); 11 | 12 | virtual void setGlobalPath(Path::Ptr path) override; 13 | 14 | virtual Path::Ptr updateLocalPath() override; 15 | virtual bool isNull() const override; 16 | virtual void setParams(const LocalPlannerParameters& opt) override; 17 | virtual void setVelocity(geometry_msgs::Twist::_linear_type vector) override; 18 | virtual void setVelocity(double velocity) override; 19 | }; 20 | 21 | #endif // LOCAL_PLANNER_NULL_H 22 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/utils/extended_kalman_filter.h: -------------------------------------------------------------------------------- 1 | #ifndef EXTENDED_KALMAN_FILTER 2 | #define EXTENDED_KALMAN_FILTER 3 | 4 | #include 5 | #include 6 | #include 7 | #include "std_msgs/Float64MultiArray.h" 8 | #include "nav_msgs/Odometry.h" 9 | 10 | struct EKF { 11 | EKF(); 12 | 13 | void reset(); 14 | 15 | void predict(const std_msgs::Float64MultiArray::ConstPtr& array, double dt); 16 | 17 | void correct(const Eigen::Vector3d &delta); 18 | 19 | Eigen::Matrix x_; 20 | Eigen::Matrix F; 21 | Eigen::Matrix P; 22 | 23 | Eigen::Matrix R; 24 | Eigen::Matrix Q; 25 | }; 26 | 27 | #endif // EXTENDED_KALMAN_FILTER 28 | 29 | -------------------------------------------------------------------------------- /path_msgs/msg/FollowerOptions.msg: -------------------------------------------------------------------------------- 1 | ## Options for path following algorithms 2 | 3 | # init_mode: Determines how the path follower reacts to a new path: 4 | # INIT_MODE_STOP will cause the robot to brake 5 | # INIT_MODE_CONTINUE will not send a brake command 6 | uint8 INIT_MODE_STOP = 0 7 | uint8 INIT_MODE_CONTINUE = 1 8 | uint8 init_mode 9 | 10 | # velocity: Driving speed of the robot (interpreted as recommended value) 11 | float32 velocity 12 | 13 | # robot_controller: [optional] name of the controller to use 14 | std_msgs/String robot_controller 15 | 16 | # local_planner: [optional] name of the local planner to use 17 | std_msgs/String local_planner 18 | 19 | # collision_avoider: [optional] name of the collision avoider to use 20 | std_msgs/String collision_avoider 21 | -------------------------------------------------------------------------------- /path_follower/launch/follower_ackermann_orthexp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_astar.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | LocalPlannerAStar::LocalPlannerAStar() 5 | { 6 | 7 | } 8 | 9 | double LocalPlannerAStar::G(LNode*& current, LNode*& succ, 10 | double& score){ 11 | double tentative_gScore = current->gScore_ ; 12 | if(succ->twin_ != nullptr){ 13 | tentative_gScore += Cost(*(succ->twin_), score); 14 | }else{ 15 | tentative_gScore += Cost(*(succ), score); 16 | } 17 | return tentative_gScore; 18 | } 19 | 20 | void LocalPlannerAStar::updateSucc(LNode *¤t, LNode *&f_current, LNode &succ){ 21 | (void) current; 22 | (void) f_current; 23 | (void) succ; 24 | } 25 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/scorer.h: -------------------------------------------------------------------------------- 1 | #ifndef SCORER_H 2 | #define SCORER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class Scorer 10 | { 11 | public: 12 | typedef std::shared_ptr Ptr; 13 | 14 | public: 15 | Scorer(); 16 | virtual ~Scorer(); 17 | 18 | double getWeight() const; 19 | void setWeight(double weight); 20 | 21 | double calculateScore(const LNode& point); 22 | 23 | long nsUsed(); 24 | 25 | protected: 26 | virtual double score(const LNode& point) = 0; 27 | 28 | protected: 29 | Stopwatch sw; 30 | 31 | double weight_; 32 | }; 33 | 34 | #endif // SCORER_H 35 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/constraints/dis2path_constraint.h: -------------------------------------------------------------------------------- 1 | #ifndef DIS2PATH_CONSTRAINT_H 2 | #define DIS2PATH_CONSTRAINT_H 3 | 4 | #include 5 | 6 | class Dis2Path_Constraint : public Constraint 7 | { 8 | public: 9 | typedef std::shared_ptr Ptr; 10 | 11 | public: 12 | Dis2Path_Constraint(); 13 | virtual ~Dis2Path_Constraint(); 14 | void setParams(double new_limit); 15 | static void setDRate(double d_rate); 16 | static void setLimit(double dis2p); 17 | double getLimit(); 18 | 19 | virtual bool isSatisfied(const LNode& point) override; 20 | private: 21 | static double D_RATE, DIS2P_; 22 | double limit; 23 | int level; 24 | }; 25 | 26 | #endif // DIS2PATH_CONSTRAINT_H 27 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/utils/path_exceptions.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_EXCEPTIONS_H 2 | #define PATH_EXCEPTIONS_H 3 | 4 | /// SYSTEM 5 | #include 6 | #include // uint8_t 7 | 8 | #include 9 | 10 | /** 11 | * @brief The EmergencyBreakException struct is thrown on unrecoverable errors and 12 | * should be caught up-stream to halt the robot. 13 | */ 14 | struct EmergencyBreakException : public std::runtime_error 15 | { 16 | EmergencyBreakException(const std::string& what, 17 | uint8_t status = path_msgs::FollowPathResult::RESULT_STATUS_INTERNAL_ERROR) 18 | : std::runtime_error(what), 19 | status_code(status) 20 | { 21 | } 22 | 23 | const uint8_t status_code; 24 | }; 25 | 26 | #endif // PATH_EXCEPTIONS_H 27 | -------------------------------------------------------------------------------- /navigation_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | navigation_launch 4 | 1.0.0 5 | The navigation_launch package contains different launch files 6 | 7 | Sebastian Buck 8 | Felix Widmaier 9 | 10 | Goran Huskic 11 | Sebastian Buck 12 | Julian Jordan 13 | Adrian Zwiener 14 | 15 | BSD 16 | 17 | catkin 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /path_msgs/action/PlanPath.action: -------------------------------------------------------------------------------- 1 | ## Request to plan a path 2 | 3 | # use_start: If true, start the search at the pose 4 | bool use_start 5 | # start: The start pose, only used when is true. 6 | # Otherwise, other means (tf, odometry) are used to determine the start pose. 7 | geometry_msgs/PoseStamped start 8 | 9 | # goal: Specification of the goal condiditions 10 | path_msgs/Goal goal 11 | 12 | # options: Planner options 13 | path_msgs/PlannerOptions options 14 | 15 | --- 16 | #result definition 17 | # path: Resulting path, empty if no path is found 18 | path_msgs/PathSequence path 19 | 20 | --- 21 | #feedback 22 | 23 | uint8 STATUS_DONE = 0 24 | uint8 STATUS_PLANNING = 1 25 | uint8 STATUS_POST_PROCESSING = 2 26 | uint8 STATUS_PRE_PROCESSING = 3 27 | uint8 STATUS_PLANNING_FAILED = 10 28 | uint8 status 29 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled source # 2 | ################### 3 | *.com 4 | *.class 5 | *.dll 6 | *.exe 7 | *.o 8 | *.so 9 | *.pyc 10 | 11 | # Packages # 12 | ############ 13 | # it's better to unpack these files and commit the raw source 14 | # git has its own built in compression methods 15 | *.7z 16 | *.dmg 17 | *.gz 18 | *.iso 19 | *.jar 20 | *.rar 21 | *.tar 22 | *.zip 23 | 24 | # Logs and databases # 25 | ###################### 26 | *.log 27 | *.sql 28 | *.sqlite 29 | 30 | # OS generated files # 31 | ###################### 32 | .DS_Store 33 | .DS_Store? 34 | ._* 35 | .Spotlight-V100 36 | .Trashes 37 | Icon? 38 | ehthumbs.db 39 | Thumbs.db 40 | *~ 41 | 42 | # CMake # 43 | ######### 44 | *.txt.user 45 | 46 | # Catkin # 47 | ########## 48 | 49 | build/ 50 | devel/ 51 | install/ 52 | 53 | # RANDOM # 54 | ########## 55 | .svn/ 56 | /.project 57 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/collision_avoidance/collision_detector_ackermann.h: -------------------------------------------------------------------------------- 1 | #ifndef COLLISION_DETECTOR_ACKERMANN_H 2 | #define COLLISION_DETECTOR_ACKERMANN_H 3 | 4 | #include "collision_detector_polygon.h" 5 | 6 | /** 7 | * @brief Rectangle obstacle box that is enlarged and bend in curves 8 | * 9 | * See comments in the code (*.cpp) for details of the behaviour of the 10 | * box in curves. 11 | */ 12 | class CollisionDetectorAckermann : public CollisionDetectorPolygon 13 | { 14 | public: 15 | virtual bool avoid(MoveCommand * const cmd, const CollisionAvoider::State &state) override; 16 | 17 | protected: 18 | PolygonWithTfFrame getPolygon(float width, float length, float course_angle, float curve_enlarge_factor) const override; 19 | 20 | float velocity_; 21 | }; 22 | 23 | #endif // COLLISION_DETECTOR_ACKERMANN_H 24 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_static.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_STATIC_H 2 | #define LOCAL_PLANNER_STATIC_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerStatic : virtual public LocalPlannerClassic 8 | { 9 | public: 10 | LocalPlannerStatic(); 11 | 12 | protected: 13 | virtual void initLeaves(LNode& root) override; 14 | 15 | virtual void updateLeaves(std::vector &successors, LNode*& current) override; 16 | 17 | virtual void updateBest(double& current_p, double& best_p, LNode*& obj, LNode*& succ) override; 18 | 19 | virtual void addLeaf(LNode*& node) override; 20 | 21 | virtual void reconfigureTree(LNode*& obj, std::vector& nodes, double& best_p) override; 22 | }; 23 | 24 | #endif // LOCAL_PLANNER_STATIC_H 25 | -------------------------------------------------------------------------------- /path_follower/launch/follower_OFC.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /path_follower/launch/follower_PBR.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /docker/kinetic-perception.docker: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic-perception 2 | 3 | RUN set -x; apt-get update -y && apt-get install -y software-properties-common wget gnupg apt-transport-https ca-certificates && \ 4 | ( wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null ) && \ 5 | apt-add-repository -y 'deb https://apt.kitware.com/ubuntu/ xenial main' && apt-get update -y && \ 6 | apt-get install -y --no-install-recommends build-essential openssh-client sudo cmake 7 | 8 | RUN mkdir -p /build/ws/src/ 9 | ADD ./ /build/ws/src/ 10 | 11 | SHELL [ "bash", "-c" ] 12 | 13 | RUN apt-get update -y && rosdep update --include-eol-distros 14 | RUN rosdep install --from-paths -r -i -y /build/ws/src 15 | RUN source /opt/ros/$(ls /opt/ros/ | head -n 1)/setup.bash && cd /build/ws/ && catkin_make_isolated -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/cost_calculator.h: -------------------------------------------------------------------------------- 1 | #ifndef COST_CALCULATOR_H 2 | #define COST_CALCULATOR_H 3 | 4 | #include "node.h" 5 | #include "analyzer.h" 6 | #include 7 | #include 8 | #include 9 | 10 | class Search; 11 | 12 | class CostCalculator : public Analyzer 13 | { 14 | public: 15 | friend class Search; 16 | 17 | CostCalculator(Search& search); 18 | 19 | double calculateStraightCost(Node* current_node, const Eigen::Vector2d &start_point_on_segment, const Eigen::Vector2d &end_point_on_segment) const; 20 | double calculateCurveCost(Node* current_node) const; 21 | 22 | private: 23 | ros::NodeHandle pnh_; 24 | 25 | double backward_penalty_factor; 26 | double turning_straight_segment; 27 | double turning_penalty; 28 | 29 | }; 30 | 31 | #endif // COST_CALCULATOR_H 32 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # How to contribute 2 | 3 | We are glad about contributions in any way, shape, or form. 4 | If you have patches or new features, please consider creating a merge request. 5 | We are also always happy to hear about projects in which you have implemented 6 | your own plug-ins. 7 | 8 | 9 | ## Changes to the Core Framework 10 | 11 | On a case-by-case basis we have to decide on whether to merge new branches 12 | or whether to create preliminary topic branches for them. 13 | Trivial changes are also always welcome. 14 | 15 | 16 | ## Plug-Ins 17 | 18 | New controllers or local planners should be implemented in the form of 19 | plug-ins that do not need extensions to the main framework. 20 | 21 | 22 | ## Reporting Issues 23 | 24 | We appreciate every posted issue for any kind of bug, failed assertion or 25 | usability issue, as well as in the form of ideas and suggestions. 26 | -------------------------------------------------------------------------------- /tools/odom2tf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odom2tf 4 | 1.0.0 5 | The odom2tf package converts nav_msgs/Odometry to tf information 6 | 7 | 8 | Sebastian Buck 9 | 10 | Goran Huskic 11 | Sebastian Buck 12 | Julian Jordan 13 | Adrian Zwiener 14 | 15 | BSD 16 | 17 | catkin 18 | 19 | roscpp 20 | nav_msgs 21 | tf2 22 | tf2_ros 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /gerona_examples/launch/include/stage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /gerona_examples/launch/stage_example_amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /path_follower/src/supervisor/waypointtimeout.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | WaypointTimeout::WaypointTimeout(ros::Duration max_duration): 5 | Timeout(max_duration) 6 | {} 7 | 8 | 9 | void WaypointTimeout::supervise(Supervisor::State &state, Supervisor::Result *out) 10 | { 11 | out->can_continue = !isExpired(); 12 | 13 | if (!out->can_continue) { 14 | ROS_WARN_NAMED("s_waypoint_timeout", "Waypoint Timeout! The robot did not reach the" 15 | " next waypoint within %g sec. Abort path execution.", 16 | duration_.toSec()); 17 | } 18 | 19 | out->status = path_msgs::FollowPathResult::RESULT_STATUS_TIMEOUT; 20 | } 21 | 22 | void WaypointTimeout::eventNewGoal() 23 | { 24 | reset(); 25 | } 26 | 27 | void WaypointTimeout::eventNewWaypoint() 28 | { 29 | reset(); 30 | } 31 | -------------------------------------------------------------------------------- /path_msgs/action/FollowPath.action: -------------------------------------------------------------------------------- 1 | ## Request to follow a path 2 | 3 | # follower_options: Options for the path follower 4 | path_msgs/FollowerOptions follower_options 5 | 6 | # path: The path to follow 7 | path_msgs/PathSequence path 8 | 9 | --- 10 | # RESULT DEFINITION 11 | 12 | uint8 RESULT_STATUS_UNKNOWN = 1 13 | uint8 RESULT_STATUS_OBSTACLE = 2 14 | uint8 RESULT_STATUS_SUCCESS = 3 15 | uint8 RESULT_STATUS_ABORTED = 4 16 | uint8 RESULT_STATUS_INTERNAL_ERROR = 5 17 | uint8 RESULT_STATUS_SLAM_FAIL = 6 18 | uint8 RESULT_STATUS_TF_FAIL = 7 19 | uint8 RESULT_STATUS_PATH_LOST = 8 20 | uint8 RESULT_STATUS_TIMEOUT = 9 21 | int8 status 22 | 23 | --- 24 | # FEEDBACK DEFINITION 25 | 26 | uint8 MOTION_STATUS_MOVING = 1 27 | uint8 MOTION_STATUS_OBSTACLE = 2 28 | uint8 MOTION_STATUS_NO_LOCAL_PATH = 3 29 | uint8 status 30 | 31 | # obstacles_on_path: a list of the currently relevant obstacles 32 | Obstacle[] obstacles_on_path 33 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_reconf.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_RECONF_H 2 | #define LOCAL_PLANNER_RECONF_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerReconf : virtual public LocalPlannerClassic 8 | { 9 | public: 10 | LocalPlannerReconf(); 11 | 12 | protected: 13 | virtual void initLeaves(LNode& root) override; 14 | 15 | virtual void updateLeaves(std::vector& successors, LNode*& current) override; 16 | 17 | virtual void updateBest(double& current_p, double& best_p, LNode*& obj, LNode*& succ) override; 18 | 19 | virtual void addLeaf(LNode*& node) override; 20 | 21 | virtual void reconfigureTree(LNode*& obj, std::vector& nodes, double& best_p) override; 22 | 23 | private: 24 | std::vector leaves; 25 | }; 26 | 27 | #endif // LOCAL_PLANNER_RECONF_H 28 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/scorers/curvatured_scorer.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | double CurvatureD_Scorer::MAX_CURV = 0.0; 5 | 6 | CurvatureD_Scorer::CurvatureD_Scorer(): 7 | Scorer() 8 | { 9 | 10 | } 11 | 12 | CurvatureD_Scorer::~CurvatureD_Scorer() 13 | { 14 | 15 | } 16 | 17 | void CurvatureD_Scorer::setMaxC(double& radius){ 18 | MAX_CURV = 2.0/radius; 19 | } 20 | 21 | double CurvatureD_Scorer::score(const LNode& point){ 22 | sw.resume(); 23 | double diff = 0.0; 24 | if(point.parent_ != nullptr){ 25 | double c_curv = point.radius_ < std::numeric_limits::infinity() ? 1.0/point.radius_:0.0; 26 | double p_curv = point.parent_->radius_ < std::numeric_limits::infinity() ? 1.0/point.parent_->radius_:0.0; 27 | diff = c_curv - p_curv ; 28 | } 29 | sw.stop(); 30 | return diff; 31 | } 32 | -------------------------------------------------------------------------------- /tools/tf2odom/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tf2odom 4 | 1.0.0 5 | The tf2odom package converts tf information to nav_msgs/Odometry 6 | 7 | 8 | Sebastian Buck 9 | 10 | Goran Huskic 11 | Sebastian Buck 12 | Julian Jordan 13 | Adrian Zwiener 14 | 15 | BSD 16 | 17 | catkin 18 | 19 | roscpp 20 | nav_msgs 21 | tf2 22 | tf2_ros 23 | tf2_eigen 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /gerona_examples/launch/include/amcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/factory/collision_avoider_factory.h: -------------------------------------------------------------------------------- 1 | #ifndef COLLISION_AVOIDER_FACTORY_H 2 | #define COLLISION_AVOIDER_FACTORY_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | class CollisionAvoider; 10 | 11 | /** 12 | * @brief The CollisionAvoiderFactory class is responsible for creating instance 13 | * of the CollisionAvoider class. 14 | * @see CollisionAvoider 15 | */ 16 | class CollisionAvoiderFactory : public AbstractFactory 17 | { 18 | public: 19 | /** 20 | * @brief makeObstacleAvoider creates an instance of the CollisionAvoider identified by . 21 | * @param name The name of the collision avoider to instanciate 22 | * @return A shared pointer to the avoider 23 | */ 24 | std::shared_ptr makeObstacleAvoider(const std::string &name); 25 | }; 26 | 27 | 28 | #endif // COLLISION_AVOIDER_FACTORY_H 29 | -------------------------------------------------------------------------------- /tools/roimap/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roimap 4 | 1.0.0 5 | 6 | 7 | Get a region of interest defined by the boundaries of a known area. 8 | 9 | 10 | Richard Hanten 11 | 12 | Goran Huskic 13 | Sebastian Buck 14 | Julian Jordan 15 | Adrian Zwiener 16 | 17 | BSD 18 | 19 | catkin 20 | 21 | nav_msgs 22 | roscpp 23 | 24 | nav_msgs 25 | roscpp 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /gerona_examples/launch/stage_example_odometry.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_static.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | 6 | 7 | LocalPlannerStatic::LocalPlannerStatic() 8 | { 9 | 10 | } 11 | 12 | void LocalPlannerStatic::initLeaves(LNode& root){ 13 | (void) root; 14 | } 15 | 16 | void LocalPlannerStatic::updateLeaves(std::vector& successors, LNode*& current){ 17 | (void) successors; 18 | (void) current; 19 | } 20 | 21 | void LocalPlannerStatic::updateBest(double& current_p, double& best_p, LNode*& obj, LNode*& succ){ 22 | if(current_p < best_p){ 23 | best_p = current_p; 24 | obj = succ; 25 | } 26 | } 27 | 28 | void LocalPlannerStatic::addLeaf(LNode*& node){ 29 | (void) node; 30 | } 31 | 32 | void LocalPlannerStatic::reconfigureTree(LNode*& obj, std::vector& nodes, double& best_p){ 33 | (void) obj; 34 | (void) nodes; 35 | (void) best_p; 36 | } 37 | -------------------------------------------------------------------------------- /tools/model_based_planner/src/config_modelbasedplanner.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "config_modelbasedplanner.h" 3 | 4 | template 5 | void splitT(const std::string &s, char delim, Out result) { 6 | std::stringstream ss(s); 7 | std::string item; 8 | while (std::getline(ss, item, delim)) { 9 | *(result++) = item; 10 | } 11 | } 12 | 13 | std::vector split(const std::string &s, char delim) { 14 | std::vector elems; 15 | splitT(s, delim, std::back_inserter(elems)); 16 | return elems; 17 | } 18 | std::string ModelBasedPlannerConfig::getFolderName(const std::string &s) 19 | { 20 | std::string res = ""; 21 | 22 | std::vector tokens = split(s, '/'); 23 | 24 | for (unsigned int tl = 0; tl < tokens.size()-1;tl++) 25 | { 26 | res += tokens[tl] + "/"; 27 | } 28 | 29 | 30 | return res; 31 | } 32 | -------------------------------------------------------------------------------- /path_follower/launch/filter_sick.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 19 | 20 | -------------------------------------------------------------------------------- /path_follower/src/utils/elevation_map.cpp: -------------------------------------------------------------------------------- 1 | 2 | /// HEADER 3 | #include 4 | #include 5 | //#include 6 | #include 7 | #include 8 | 9 | 10 | 11 | ElevationMap::ElevationMap() 12 | : elevationMap(nullptr) 13 | {} 14 | 15 | ElevationMap::ElevationMap(const EMap &c) 16 | : elevationMap(c) 17 | { 18 | } 19 | 20 | 21 | bool ElevationMap::empty() const 22 | { 23 | return elevationMap == nullptr; 24 | } 25 | 26 | void ElevationMap::clear() 27 | { 28 | elevationMap = nullptr; 29 | } 30 | 31 | cv::Mat ElevationMap::toCVMat() const 32 | { 33 | 34 | return empty()?cv::Mat() : cv_bridge::toCvShare(elevationMap,"")->image; 35 | 36 | } 37 | 38 | 39 | ros::Time ElevationMap::getStamp() const 40 | { 41 | return elevationMap->header.stamp; 42 | } 43 | 44 | std::string ElevationMap::getFrameId() const 45 | { 46 | return elevationMap->header.frame_id; 47 | } 48 | -------------------------------------------------------------------------------- /path_follower/launch/follower_omnidrive_orthexp.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /path_follower/launch/follower_2steer_purepursuit.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /path_follower/src/factory/collision_avoider_factory.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | std::shared_ptr CollisionAvoiderFactory::makeObstacleAvoider(const std::string &name) 9 | { 10 | if(name == "default_collision_avoider") { 11 | return std::make_shared(); 12 | } else { 13 | if (name == "omnidrive") { 14 | return std::make_shared(); 15 | } else if (name == "ackermann") { 16 | return std::make_shared(); 17 | } 18 | } 19 | ROS_WARN_STREAM("No collision_avoider defined with the name '" << name << "'. Defaulting to Omnidrive."); 20 | return std::make_shared(); 21 | } 22 | -------------------------------------------------------------------------------- /path_planner/launch/planner_cost.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /navigation_launch/launch/laser_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 23 | 24 | -------------------------------------------------------------------------------- /path_follower/src/utils/pose_goal_remapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | geometry_msgs::PoseStamped curPose; 5 | ros::Publisher pub; 6 | 7 | void callback(const geometry_msgs::PoseStamped::ConstPtr& msg) { 8 | curPose = *msg; 9 | } 10 | 11 | void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg) { 12 | ROS_INFO("Republishing goal..."); 13 | pub.publish(curPose); 14 | ROS_INFO_STREAM("Old goal: " << (*msg) << ", new goal: " << curPose); 15 | } 16 | 17 | int main(int argc, char **argv) { 18 | ros::init(argc, argv, "pose_goal_remapper"); 19 | 20 | ros::NodeHandle n; 21 | 22 | pub = n.advertise("move_base_simple/goal", 1); 23 | 24 | auto s1 = n.subscribe("/slam_out_pose", 10, callback); 25 | 26 | auto s2 = n.subscribe("/rviz_goal", 1, goalCallback); 27 | 28 | ROS_INFO("Started remapping. Waiting for goals..."); 29 | 30 | ros::Rate loop_rate(10); 31 | while (n.ok()) { 32 | ros::spinOnce(); 33 | loop_rate.sleep(); 34 | } 35 | return 0; 36 | } 37 | 38 | -------------------------------------------------------------------------------- /navigation_launch/launch/laser_filter_scitos.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 23 | 24 | -------------------------------------------------------------------------------- /navigation_launch/launch/mapping.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /tools/odom2tf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(odom2tf) 3 | 4 | ## Enforce that we use C++11 5 | include(CheckCXXCompilerFlag) 6 | check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11) 7 | check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 8 | if(COMPILER_SUPPORTS_CXX11) 9 | add_definitions(-std=c++11) 10 | elseif(COMPILER_SUPPORTS_CXX0X) 11 | add_definitions(-std=c++0x) 12 | else() 13 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 14 | endif() 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | roscpp 18 | nav_msgs 19 | tf2 20 | tf2_ros 21 | ) 22 | 23 | catkin_package() 24 | 25 | include_directories(SYSTEM 26 | ${catkin_INCLUDE_DIRS} 27 | ) 28 | add_executable(${PROJECT_NAME}_node 29 | src/odom2tf_node.cpp 30 | ) 31 | target_link_libraries(${PROJECT_NAME}_node 32 | ${catkin_LIBRARIES} 33 | ) 34 | 35 | install(TARGETS ${PROJECT_NAME}_node 36 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 37 | ) 38 | 39 | -------------------------------------------------------------------------------- /tools/tf2odom/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tf2odom) 3 | 4 | ## Enforce that we use C++11 5 | include(CheckCXXCompilerFlag) 6 | check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11) 7 | check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 8 | if(COMPILER_SUPPORTS_CXX11) 9 | add_definitions(-std=c++11) 10 | elseif(COMPILER_SUPPORTS_CXX0X) 11 | add_definitions(-std=c++0x) 12 | else() 13 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 14 | endif() 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | roscpp 18 | nav_msgs 19 | tf2 20 | tf2_ros 21 | tf2_eigen 22 | ) 23 | 24 | catkin_package() 25 | 26 | include_directories(SYSTEM 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | add_executable(${PROJECT_NAME}_node 30 | src/tf2odom_node.cpp 31 | ) 32 | target_link_libraries(${PROJECT_NAME}_node 33 | ${catkin_LIBRARIES} 34 | ) 35 | 36 | install(TARGETS ${PROJECT_NAME}_node 37 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 38 | ) 39 | 40 | -------------------------------------------------------------------------------- /navigation_launch/launch/laser_filter_summit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 23 | 24 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/supervisor/distancetopathsupervisor.h: -------------------------------------------------------------------------------- 1 | #ifndef DISTANCETOPATHSUPERVISOR_H 2 | #define DISTANCETOPATHSUPERVISOR_H 3 | 4 | #include 5 | #include 6 | 7 | /** 8 | * @brief Supervises the robot to stay near the path. 9 | * 10 | * This supervisor checks how far the robot is away from the current 11 | * path segment and tells it to stop, if the distance exceeds a defined 12 | * maximum. 13 | */ 14 | class DistanceToPathSupervisor : public Supervisor 15 | { 16 | public: 17 | DistanceToPathSupervisor(double max_distance_to_path); 18 | 19 | virtual std::string getName() const { 20 | return "DistanceToPath"; 21 | } 22 | 23 | virtual void supervise(State &state, Result *out); 24 | 25 | private: 26 | double max_dist_; 27 | Visualizer *visualizer_; 28 | 29 | //! Calculate the distance of the robot to the current path segment. 30 | double calculateDistanceToCurrentPathSegment(const State &state); 31 | }; 32 | 33 | #endif // DISTANCETOPATHSUPERVISOR_H 34 | -------------------------------------------------------------------------------- /gerona_examples/launch/include/local_elevationmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /path_follower/launch/follower_2steer_stanley.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/path_follower_server.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_FOLLOWER_SERVER_H 2 | #define PATH_FOLLOWER_SERVER_H 3 | 4 | /// SYSTEM 5 | #include 6 | #include 7 | 8 | class PathFollower; 9 | 10 | class PathFollowerServer 11 | { 12 | public: 13 | PathFollowerServer(PathFollower& follower); 14 | 15 | void spin(); 16 | void update(); 17 | 18 | //! Callback for new follow_path action goals. 19 | void followPathGoalCB(); 20 | //! Callback for follow_path action preemption. 21 | void followPathPreemptCB(); 22 | 23 | private: 24 | PathFollower& follower_; 25 | 26 | typedef actionlib::SimpleActionServer FollowPathServer; 27 | 28 | //! Action server that communicates with path_control (or who ever sends actions) 29 | FollowPathServer follow_path_server_; 30 | 31 | path_msgs::FollowPathGoalConstPtr latest_goal_; 32 | 33 | ros::Duration continue_mode_timeout_; 34 | boost::optional last_preempt_; 35 | }; 36 | 37 | #endif // PATH_FOLLOWER_SERVER_H 38 | -------------------------------------------------------------------------------- /path_follower/src/collision_avoidance/collision_detector_omnidrive.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace std; 5 | 6 | CollisionDetectorPolygon::PolygonWithTfFrame CollisionDetectorOmnidrive::getPolygon(float width, float length, float course_angle, float curve_enlarge_factor) const 7 | { 8 | PolygonWithTfFrame pwf; 9 | pwf.frame = robot_frame_; 10 | 11 | // rotate about course_angle (box should point in driving direction) 12 | tf::Transform rot(tf::Quaternion(tf::Vector3(0,0,1), course_angle)); 13 | 14 | vector corners; 15 | corners.push_back(rot * tf::Point(0.0, width/2, 0.0)); 16 | corners.push_back(rot * tf::Point(0.0, -width/2, 0.0)); 17 | corners.push_back(rot * tf::Point(length, -width/2, 0.0)); 18 | corners.push_back(rot * tf::Point(length, width/2, 0.0)); 19 | 20 | for (vector::iterator it = corners.begin(); it != corners.end(); ++it) { 21 | pwf.polygon.push_back( cv::Point2f(it->x(), it->y()) ); 22 | } 23 | 24 | return pwf; 25 | } 26 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/path_builder.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_BUILDER_H 2 | #define PATH_BUILDER_H 3 | 4 | #include 5 | #include 6 | #include "analyzer.h" 7 | #include "node.h" 8 | 9 | class PathBuilder : public Analyzer 10 | { 11 | public: 12 | PathBuilder(Search &search); 13 | 14 | operator path_msgs::PathSequence() { 15 | return build(); 16 | } 17 | 18 | path_msgs::PathSequence build() const; 19 | void addPath(const path_msgs::PathSequence& path); 20 | 21 | void insertTangentPoint(const Segment* start_segment, Eigen::Vector2d start_pt); 22 | 23 | void insertCurveSegment(const Node* node); 24 | void extendAlongSourceSegment(const Node *node, double length); 25 | void extendAlongTargetSegment(const Node *node, double length); 26 | void extendWithStraightTurningSegment(const Eigen::Vector2d &pt, double length); 27 | 28 | private: 29 | void insertPose(const Eigen::Vector2d& pt, double c_yaw); 30 | 31 | private: 32 | path_msgs::PathSequence res; 33 | }; 34 | 35 | #endif // PATH_BUILDER_H 36 | -------------------------------------------------------------------------------- /path_follower/launch/follower_kinematic_SLP.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 | -------------------------------------------------------------------------------- /tools/scan2cloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | scan2cloud 4 | 1.0.0 5 | The scan2cloud package 6 | 7 | yutschiin 8 | 9 | Goran Huskic 10 | Sebastian Buck 11 | Julian Jordan 12 | Adrian Zwiener 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | laser_geometry 19 | roscpp 20 | std_msgs 21 | tf 22 | pcl_ros 23 | 24 | laser_geometry 25 | roscpp 26 | std_msgs 27 | tf 28 | pcl_ros 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /path_follower/launch/follower_ackermann_purepursuit.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /path_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(path_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | actionlib_msgs 6 | geometry_msgs 7 | message_generation 8 | nav_msgs 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | Goal.msg 14 | Obstacle.msg 15 | DirectionalPath.msg 16 | PathSequence.msg 17 | PlannerOptions.msg 18 | FollowerOptions.msg 19 | ) 20 | 21 | # Generate actions in the 'action' folder 22 | add_action_files( 23 | FILES 24 | FollowPath.action 25 | NavigateToGoal.action 26 | PlanPath.action 27 | ) 28 | 29 | # Generate added messages and services with any dependencies listed here 30 | generate_messages( 31 | DEPENDENCIES 32 | actionlib_msgs 33 | geometry_msgs 34 | nav_msgs 35 | ) 36 | 37 | catkin_package( 38 | CATKIN_DEPENDS 39 | actionlib_msgs 40 | geometry_msgs 41 | message_runtime 42 | nav_msgs 43 | ) 44 | 45 | 46 | 47 | 48 | file(GLOB_RECURSE message_files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} FOLLOW_SYMLINKS msg/*.msg action/*.action) 49 | add_custom_target(${PROJECT_NAME}_show_messages SOURCES ${message_files}) 50 | -------------------------------------------------------------------------------- /gerona/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gerona 4 | 1.0.0 5 | 6 | The GeRoNa stack contains packages for path planning and following. 7 | 8 | 9 | Goran Huskic 10 | Sebastian Buck 11 | Felix Widmaier 12 | 13 | Goran Huskic 14 | Sebastian Buck 15 | Julian Jordan 16 | Adrian Zwiener 17 | 18 | BSD 19 | 20 | catkin 21 | 22 | path_msgs 23 | path_control 24 | path_planner 25 | path_follower 26 | path_rviz 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/ramaxx.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | Robot: 4 | baseLinkPosCoord: [ 0., 0. ] 5 | Chassis: 6 | chassisfileName: "ramaxx.png" 7 | chassisImageCenter: [ -1., -1. ] 8 | chassisImageValueOffset: 0. 9 | chassisImageValueScale: 1. 10 | chassisModelYSize: 0.417 11 | chassisPosRobot: [ 0., 0. ] 12 | testChassis: 0 13 | Wheels: 14 | wheelPosRobotFrontX: 0.23 15 | wheelPosRobotRearX: -0.23 16 | wheelPosRobotFrontY: 0.2 17 | wheelPosRobotRearY: 0.2 18 | wheelJointPosFront: [ 0., 0. ] 19 | wheelRadiusFront: 0.09652 20 | wheelWidthFront: 0.14 21 | wheelLatRadiusFront: 0.0 22 | wheelJointPosRear: [ 0., 0. ] 23 | wheelRadiusRear: 0.09652 24 | wheelWidthRear: 0.14 25 | wheelLatRadiusRear: 0.0 26 | frontWheelsTurnable: 1 27 | wheelRotTestSteps: 0 28 | wheelRotTestStepSize: 0 29 | Proc: 30 | numAngleStep: 360 31 | heightScale: 1000.0 32 | mapBaseHeight: 10000 33 | wheelGroundLevel: 20000 34 | maxHeight: 30000 35 | pixelSize: 0.00625 36 | imagePosBLMinX: 0.75 37 | imagePosBLMinY: -1.0 38 | validThresholdFactor: 0.95 39 | convertImage: 0 40 | 41 | 42 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/rollator.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | Robot: 4 | baseLinkPosCoord: [ 0., 0. ] 5 | Chassis: 6 | chassisfileName: "Rollator.png" 7 | chassisImageCenter: [ -1., -1. ] 8 | chassisImageValueOffset: 0. 9 | chassisImageValueScale: 1. 10 | chassisModelYSize: 0.55 11 | chassisPosRobot: [ 0.04, 0. ] 12 | testChassis: 0 13 | Wheels: 14 | wheelPosRobotFrontX: 0.574 15 | wheelPosRobotRearX: 0.0 16 | wheelPosRobotFrontY: 0.252 17 | wheelPosRobotRearY: 0.29 18 | wheelJointPosFront: [ 0., 0. ] 19 | wheelRadiusFront: 0.1025 20 | wheelWidthFront: 0.03 21 | wheelLatRadiusFront: 0.05 22 | wheelJointPosRear: [ 0., 0. ] 23 | wheelRadiusRear: 0.1025 24 | wheelWidthRear: 0.03 25 | wheelLatRadiusRear: 0.05 26 | frontWheelsTurnable: 1 27 | wheelRotTestSteps: 0 28 | wheelRotTestStepSize: 0 29 | Proc: 30 | numAngleStep: 360 31 | heightScale: 1000.0 32 | mapBaseHeight: 10000 33 | wheelGroundLevel: 20000 34 | maxHeight: 30000 35 | pixelSize: 0.00625 36 | imagePosBLMinX: -0.8 37 | imagePosBLMinY: -1.5 38 | validThresholdFactor: 0.95 39 | convertImage: 0 40 | 41 | 42 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/rollator_c.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | Robot: 4 | baseLinkPosCoord: [ 0., 0. ] 5 | Chassis: 6 | chassisfileName: "Rollator.png" 7 | chassisImageCenter: [ -1., -1. ] 8 | chassisImageValueOffset: 0. 9 | chassisImageValueScale: 2. 10 | chassisModelYSize: 0.55 11 | chassisPosRobot: [ 0.04, 0. ] 12 | testChassis: 1 13 | Wheels: 14 | wheelPosRobotFrontX: 0.574 15 | wheelPosRobotRearX: 0.0 16 | wheelPosRobotFrontY: 0.26 17 | wheelPosRobotRearY: 0.31 18 | wheelJointPosFront: [ 0., 0. ] 19 | wheelRadiusFront: 0.1025 20 | wheelWidthFront: 0.06 21 | wheelLatRadiusFront: 0.05 22 | wheelJointPosRear: [ 0., 0. ] 23 | wheelRadiusRear: 0.1025 24 | wheelWidthRear: 0.06 25 | wheelLatRadiusRear: 0.05 26 | frontWheelsTurnable: 1 27 | wheelRotTestSteps: 0 28 | wheelRotTestStepSize: 0 29 | Proc: 30 | numAngleStep: 360 31 | heightScale: 1000.0 32 | mapBaseHeight: 10000 33 | wheelGroundLevel: 20000 34 | maxHeight: 30000 35 | pixelSize: 0.00625 36 | imagePosBLMinX: -0.8 37 | imagePosBLMinY: -1.5 38 | validThresholdFactor: 0.95 39 | convertImage: 0 40 | 41 | 42 | -------------------------------------------------------------------------------- /path_planner/launch/path_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /tools/path_rviz/package.xml: -------------------------------------------------------------------------------- 1 | 2 | path_rviz 3 | 1.0.0 4 | 5 | Path plugins for RViz. 6 | 7 | 8 | Sebastian Buck 9 | 10 | Goran Huskic 11 | Sebastian Buck 12 | Julian Jordan 13 | Adrian Zwiener 14 | 15 | BSD 16 | 17 | catkin 18 | 19 | qtbase5-dev 20 | rviz 21 | path_msgs 22 | 23 | libqt5-core 24 | libqt5-gui 25 | libqt5-widgets 26 | rviz 27 | path_msgs 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed_local_planner.h: -------------------------------------------------------------------------------- 1 | #ifndef HIGH_SPEED_LOCAL_PLANNER_H 2 | #define HIGH_SPEED_LOCAL_PLANNER_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | /// SYSTEM 8 | #include 9 | 10 | class HighSpeedLocalPlanner : public AbstractLocalPlanner 11 | { 12 | public: 13 | HighSpeedLocalPlanner(); 14 | 15 | virtual Path::Ptr updateLocalPath() override; 16 | 17 | virtual void setGlobalPath(Path::Ptr path) override; 18 | private: 19 | bool transform2Odo(ros::Time& now); 20 | 21 | void printSCTimeUsage(); 22 | 23 | virtual void printNodeUsage(std::size_t& nnodes) const = 0; 24 | virtual void printVelocity() = 0; 25 | virtual void printLevelReached() const = 0; 26 | virtual bool algo(Eigen::Vector3d& pose, SubPath& local_wps, 27 | std::size_t& nnodes) = 0; 28 | protected: 29 | SubPath waypoints, wlp_; 30 | SubPath waypoints_map; 31 | 32 | bool close_to_goal; 33 | 34 | std::vector all_local_paths_; 35 | 36 | ros::Time last_update_; 37 | }; 38 | 39 | #endif // HIGH_SPEED_LOCAL_PLANNER_H 40 | -------------------------------------------------------------------------------- /path_follower/cmake/FindALGLIB.cmake: -------------------------------------------------------------------------------- 1 | # - Find ALGLIB 2 | # Find the native ALGLIB includes and library 3 | # 4 | # ALGLIB_INCLUDES - where to find fftw3.h 5 | # ALGLIB_LIBRARIES - List of libraries when using FFTW. 6 | # ALGLIB_FOUND - True if FFTW found. 7 | 8 | if (ALGLIB_INCLUDES) 9 | # Already in cache, be silent 10 | set (ALGLIB_FIND_QUIETLY TRUE) 11 | endif (ALGLIB_INCLUDES) 12 | 13 | find_path (ALGLIB_INCLUDES 14 | alglibinternal.h 15 | alglibmisc.h 16 | ap.h 17 | dataanalysis.h 18 | diffequations.h 19 | fasttransforms.h 20 | integration.h 21 | interpolation.h 22 | linalg.h 23 | optimization.h 24 | solvers.h 25 | specialfunctions.h 26 | statistics.h 27 | stdafx.h 28 | PATHS 29 | /usr/include/libalglib/ 30 | ) 31 | 32 | find_library (ALGLIB_LIBRARIES NAMES alglib) 33 | 34 | # handle the QUIETLY and REQUIRED arguments and set ALGLIB_FOUND to TRUE if 35 | # all listed variables are TRUE 36 | include (FindPackageHandleStandardArgs) 37 | find_package_handle_standard_args (ALGLIB DEFAULT_MSG ALGLIB_LIBRARIES ALGLIB_INCLUDES) 38 | 39 | mark_as_advanced (ALGLIB_LIBRARIES ALGLIB_INCLUDES) 40 | -------------------------------------------------------------------------------- /tools/model_based_planner/include/model_based_planner/pose_writer.h: -------------------------------------------------------------------------------- 1 | #ifndef POSEWRITER_H 2 | #define POSEWRITER_H 3 | 4 | 5 | #include "plannerutils.h" 6 | #include "config_modelbasedplanner.h" 7 | 8 | 9 | /** 10 | * @brief utility class for writing debug data 11 | */ 12 | class PoseWriter 13 | { 14 | public: 15 | //PoseWriter(std::string parentFolder); 16 | PoseWriter(); 17 | 18 | void Init(std::string parentFolder); 19 | 20 | void WritePoses(const Trajectory *traj, cv::Point2f mapOrigin); 21 | void WritePoses(const Trajectory *traj, const cv::Mat &dem, cv::Point2f mapOrigin); 22 | void WritePoses(const Trajectory *traj, cv::Point2f mapOrigin, cv::Point3f robotWorldPose); 23 | 24 | void WriteConfig(const ModelBasedPlannerConfig &config, const std::string &modelFile); 25 | 26 | void WriteTimings(float ms, int numPoses); 27 | 28 | void SetMaxNumPoses(int numMaxPoses) 29 | { 30 | numMaxPoses_ = numMaxPoses; 31 | } 32 | 33 | //~PoseWriter(); 34 | private: 35 | int poseCounter_; 36 | std::string outputFolder_; 37 | long startUS_; 38 | 39 | int numMaxPoses_; 40 | 41 | }; 42 | 43 | 44 | #endif // POSEWRITER_H 45 | -------------------------------------------------------------------------------- /path_follower/launch/follower_2steer_inputscaling.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /path_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_msgs 4 | 1.0.0 5 | This packages bundles all messages used by the path navigation packages. 6 | 7 | Sebastian Buck 8 | Felix Widmaier 9 | 10 | Goran Huskic 11 | Sebastian Buck 12 | Julian Jordan 13 | Adrian Zwiener 14 | 15 | BSD 16 | 17 | catkin 18 | 19 | actionlib_msgs 20 | geometry_msgs 21 | nav_msgs 22 | 23 | actionlib_msgs 24 | geometry_msgs 25 | nav_msgs 26 | 27 | message_generation 28 | message_runtime 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /path_follower/launch/follower_dynamic_window.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 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/supervisor/waypointtimeout.h: -------------------------------------------------------------------------------- 1 | #ifndef WAYPOINTTIMEOUT_H 2 | #define WAYPOINTTIMEOUT_H 3 | 4 | #include 5 | #include 6 | 7 | 8 | //! Very simple timeout class. 9 | class Timeout { 10 | public: 11 | Timeout(ros::Duration max_duration): 12 | duration_(max_duration) 13 | { 14 | reset(); 15 | } 16 | 17 | ros::Duration duration_; 18 | 19 | void reset() { 20 | started_ = ros::Time::now(); 21 | } 22 | 23 | bool isExpired() { 24 | return (started_ + duration_) < ros::Time::now(); 25 | } 26 | 27 | private: 28 | ros::Time started_; 29 | }; 30 | 31 | //! This supervisor tells the robot to stop if the time to reach the 32 | //! next waypoint exceeds the specified timeout. 33 | class WaypointTimeout : public Supervisor, private Timeout 34 | { 35 | public: 36 | WaypointTimeout(ros::Duration max_duration); 37 | 38 | virtual std::string getName() const { 39 | return "WaypointTimeout"; 40 | } 41 | 42 | virtual void supervise(Supervisor::State &state, Supervisor::Result *out); 43 | virtual void eventNewGoal(); 44 | virtual void eventNewWaypoint(); 45 | }; 46 | 47 | #endif // WAYPOINTTIMEOUT_H 48 | -------------------------------------------------------------------------------- /path_follower/src/collision_avoidance/collision_avoider.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | #include 6 | 7 | CollisionAvoider::CollisionAvoider() 8 | : tf_listener_(nullptr), 9 | robot_frame_("base_link") 10 | { 11 | 12 | } 13 | 14 | void CollisionAvoider::setTransformListener(const tf::TransformListener *tf_listener) 15 | { 16 | tf_listener_ = tf_listener; 17 | } 18 | 19 | void CollisionAvoider::setRobotFrameId(const std::string& frame_id) 20 | { 21 | robot_frame_ = frame_id; 22 | } 23 | 24 | bool CollisionAvoider::hasObstacles() const 25 | { 26 | return obstacles_ != nullptr && !obstacles_->empty(); 27 | } 28 | 29 | std::shared_ptr CollisionAvoider::getObstacles() const 30 | { 31 | if(!obstacles_) { 32 | ROS_FATAL("tried to access a non-existent obstacle cloud"); 33 | std::abort(); 34 | } 35 | return obstacles_; 36 | } 37 | 38 | void CollisionAvoider::setObstacles(std::shared_ptr obstacles) 39 | { 40 | obstacles_ = obstacles; 41 | } 42 | 43 | void CollisionAvoider::setExternalError(int externalError) 44 | { 45 | externalError_ = externalError; 46 | } 47 | -------------------------------------------------------------------------------- /path_follower/src/supervisor/supervisorchain.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace std; 4 | 5 | void SupervisorChain::addSupervisor(Supervisor::Ptr supervisor) 6 | { 7 | ROS_INFO("Use Supervisor '%s'", supervisor->getName().c_str()); 8 | supervisors_.push_back(supervisor); 9 | } 10 | 11 | Supervisor::Result SupervisorChain::supervise(Supervisor::State &state) 12 | { 13 | list::iterator it; 14 | for (it = supervisors_.begin(); it != supervisors_.end(); ++it) { 15 | Supervisor::Result res; 16 | (*it)->supervise(state, &res); 17 | 18 | if (!res.can_continue) { 19 | return res; 20 | } 21 | } 22 | 23 | return Supervisor::Result(); // Constructor sets to can_continue = true. 24 | } 25 | 26 | void SupervisorChain::notifyNewGoal() 27 | { 28 | list::iterator it; 29 | for (it = supervisors_.begin(); it != supervisors_.end(); ++it) { 30 | (*it)->eventNewGoal(); 31 | } 32 | } 33 | 34 | void SupervisorChain::notifyNewWaypoint() 35 | { 36 | list::iterator it; 37 | for (it = supervisors_.begin(); it != supervisors_.end(); ++it) { 38 | (*it)->eventNewWaypoint(); 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/controller/robotcontroller_potential_field_TT.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTCONTROLLER_POTENTIAL_FIELD_TT_H 2 | #define ROBOTCONTROLLER_POTENTIAL_FIELD_TT_H 3 | 4 | /// THIRD PARTY 5 | #include 6 | #include 7 | 8 | /// SYSTEM 9 | #include 10 | 11 | /// PROJECT 12 | #include 13 | #include 14 | #include 15 | 16 | /** 17 | * @brief The Potential_Field class is the potential field class for target tracking (TT) 18 | * 19 | * This is basically the classical potential field method, where the goal position is always 20 | * set dynamically to the recognized target/person. 21 | */ 22 | /// The Potential_Field class 23 | class RobotController_Potential_Field_TT : public RobotController_Potential_Field 24 | { 25 | public: 26 | /** 27 | * @brief RobotController_Potential_Field_TT 28 | */ 29 | RobotController_Potential_Field_TT(); 30 | 31 | protected: 32 | 33 | /// Sets the goal position. 34 | virtual void setGoalPosition(); 35 | 36 | 37 | }; 38 | 39 | #endif // ROBOTCONTROLLER_POTENTIAL_FIELD_TT_H 40 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_bfs.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_BFS_H 2 | #define LOCAL_PLANNER_BFS_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | 8 | class LocalPlannerBFS : virtual public LocalPlannerClassic 9 | { 10 | public: 11 | LocalPlannerBFS(); 12 | private: 13 | virtual void setInitScores(LNode& wpose, double& dis2last) override; 14 | 15 | virtual void initQueue(LNode& root) override; 16 | 17 | virtual bool isQueueEmpty() override; 18 | 19 | virtual LNode* queueFront() override; 20 | 21 | virtual void pop(LNode*& current) override; 22 | 23 | virtual void push2Closed(LNode*& current) override; 24 | 25 | virtual void expandCurrent(LNode*& current, std::size_t& nsize, std::vector& successors, 26 | std::vector& nodes) override; 27 | 28 | virtual bool processSuccessor(LNode*& succ, LNode*& current, 29 | double& current_p,double& dis2last) override; 30 | 31 | virtual void evaluate(double& current_p, LNode*& succ, double& dis2last) = 0; 32 | private: 33 | std::queue fifo; 34 | }; 35 | 36 | #endif // LOCAL_PLANNER_BFS_H 37 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/analyzer.h: -------------------------------------------------------------------------------- 1 | #ifndef ANALYZER_H 2 | #define ANALYZER_H 3 | 4 | 5 | #include "node.h" 6 | #include 7 | #include 8 | #include 9 | 10 | class Search; 11 | 12 | class Analyzer 13 | { 14 | public: 15 | friend class Search; 16 | 17 | Analyzer(Search& search); 18 | virtual ~Analyzer(); 19 | 20 | Eigen::Vector2d findStartPointOnSegment(const Node* node) const; 21 | Eigen::Vector2d findStartPointOnSegment(const Node* node, const Transition* transition) const; 22 | Eigen::Vector2d findEndPointOnSegment(const Node* node) const; 23 | Eigen::Vector2d findEndPointOnSegment(const Node* node, const Transition* transition) const; 24 | 25 | double calculateEffectiveLengthOfNextSegment(const Node* node) const; 26 | 27 | bool isSegmentForward(const Segment* segment, const Eigen::Vector2d& pos, const Eigen::Vector2d& target) const; 28 | bool isNextSegmentForward(const Node* node) const; 29 | bool isPreviousSegmentForward(const Node* current_node) const; 30 | bool isStartSegmentForward(const Node *node) const; 31 | 32 | std::string signature(const Node* head) const; 33 | 34 | private: 35 | Search& search; 36 | 37 | }; 38 | 39 | #endif // ANALYZER_H 40 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/summit.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | % was -500 cam distance to 0 is 500mm, model minimum is set to 0 level 3 | --- 4 | Robot: 5 | baseLinkPosCoord: [ 0., 0. ] 6 | chassisTestTipAngleThreshold: 0.99 7 | Chassis: 8 | chassisfileName: "summit_wl_16b_512.png" 9 | chassisImageCenter: [ -1., -1. ] 10 | chassisImageValueOffset: -440 11 | chassisImageValueScale: 0.015258789 12 | chassisModelYSize: 0.385 13 | chassisPosRobot: [ -0.025, 0. ] 14 | testChassis: 0 15 | Wheels: 16 | wheelPosRobotFrontX: 0.229 17 | wheelPosRobotRearX: -0.229 18 | wheelPosRobotFrontY: 0.234 19 | wheelPosRobotRearY: 0.234 20 | wheelJointPosFront: [ 0., 0. ] 21 | wheelRadiusFront: 0.12 22 | wheelWidthFront: 0.14 23 | wheelLatRadiusFront: 0.2 24 | wheelJointPosRear: [ 0., 0. ] 25 | wheelRadiusRear: 0.12 26 | wheelWidthRear: 0.14 27 | wheelLatRadiusRear: 0.2 28 | frontWheelsTurnable: 0 29 | wheelRotTestSteps: 0 30 | wheelRotTestStepSize: 0 31 | Proc: 32 | numAngleStep: 360 33 | heightScale: 1000.0 34 | mapBaseHeight: 10000 35 | wheelGroundLevel: 20000 36 | maxHeight: 30000 37 | pixelSize: 0.00625 38 | imagePosBLMinX: 0.75 39 | imagePosBLMinY: -1.0 40 | validThresholdFactor: 0.95 41 | convertImage: 0 42 | 43 | 44 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/summit_c.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | % was -500 cam distance to 0 is 500mm, model minimum is set to 0 level 3 | --- 4 | Robot: 5 | baseLinkPosCoord: [ 0., 0. ] 6 | chassisTestTipAngleThreshold: 0.99 7 | Chassis: 8 | chassisfileName: "summit_wl_16b_512.png" 9 | chassisImageCenter: [ -1., -1. ] 10 | chassisImageValueOffset: -440 11 | chassisImageValueScale: 0.015258789 12 | chassisModelYSize: 0.385 13 | chassisPosRobot: [ 0.025, 0. ] 14 | testChassis: 1 15 | Wheels: 16 | wheelPosRobotFrontX: 0.229 17 | wheelPosRobotRearX: -0.229 18 | wheelPosRobotFrontY: 0.234 19 | wheelPosRobotRearY: 0.234 20 | wheelJointPosFront: [ 0., 0. ] 21 | wheelRadiusFront: 0.12 22 | wheelWidthFront: 0.14 23 | wheelLatRadiusFront: 0.2 24 | wheelJointPosRear: [ 0., 0. ] 25 | wheelRadiusRear: 0.12 26 | wheelWidthRear: 0.14 27 | wheelLatRadiusRear: 0.2 28 | frontWheelsTurnable: 0 29 | wheelRotTestSteps: 0 30 | wheelRotTestStepSize: 0 31 | Proc: 32 | numAngleStep: 360 33 | heightScale: 1000.0 34 | mapBaseHeight: 10000 35 | wheelGroundLevel: 20000 36 | maxHeight: 30000 37 | pixelSize: 0.00625 38 | imagePosBLMinX: 0.75 39 | imagePosBLMinY: -1.0 40 | validThresholdFactor: 0.95 41 | convertImage: 0 42 | 43 | 44 | -------------------------------------------------------------------------------- /path_follower/src/utils/obstacle_cloud.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | #include 5 | #include 6 | 7 | ObstacleCloud::ObstacleCloud() 8 | : cloud(new Cloud) 9 | {} 10 | 11 | ObstacleCloud::ObstacleCloud(const Cloud::Ptr& c) 12 | : cloud(c) 13 | { 14 | } 15 | ObstacleCloud::ObstacleCloud(const Cloud::ConstPtr& c) 16 | : cloud(new Cloud(*c)) 17 | { 18 | } 19 | 20 | bool ObstacleCloud::empty() const 21 | { 22 | return cloud->empty(); 23 | } 24 | 25 | void ObstacleCloud::clear() 26 | { 27 | return cloud->clear(); 28 | } 29 | 30 | void ObstacleCloud::transformCloud(const tf::Transform& transform, const std::string &target_frame) 31 | { 32 | for(auto& pt : cloud->points) { 33 | tf::Point point(pt.x,pt.y,pt.z); 34 | tf::Point transformed = transform * point; 35 | pt.x = transformed.x(); 36 | pt.y = transformed.y(); 37 | pt.z = transformed.z(); 38 | } 39 | 40 | cloud->header.frame_id = target_frame; 41 | } 42 | 43 | ros::Time ObstacleCloud::getStamp() const 44 | { 45 | ros::Time time; 46 | time.fromNSec(cloud->header.stamp * 1e3); 47 | return time; 48 | } 49 | 50 | std::string ObstacleCloud::getFrameId() const 51 | { 52 | return cloud->header.frame_id; 53 | } 54 | -------------------------------------------------------------------------------- /path_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_control 4 | 1.0.0 5 | Controls path planning and execution. 6 | 7 | Sebastian Buck 8 | Felix Widmaier 9 | 10 | Goran Huskic 11 | Sebastian Buck 12 | Julian Jordan 13 | Adrian Zwiener 14 | 15 | BSD 16 | 17 | catkin 18 | geometry_msgs 19 | roscpp 20 | actionlib 21 | path_msgs 22 | tf 23 | tf2_geometry_msgs 24 | 25 | geometry_msgs 26 | roscpp 27 | actionlib 28 | path_msgs 29 | tf 30 | tf2_geometry_msgs 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /path_msgs/msg/Goal.msg: -------------------------------------------------------------------------------- 1 | # A generic path planning goal 2 | 3 | # planning_algorithm: [optional] Which path planner to use, an empty string uses the default planner. 4 | std_msgs/String planning_algorithm 5 | 6 | # planning_channel: [optional] The topic on which to request a new path, can be empty. 7 | std_msgs/String planning_channel 8 | 9 | # type: The type of goal this message specifies: 10 | # GOAL_TYPE_POSE: Find a path to the pose specified in 11 | # GOAL_TYPE_MAP: Find a path that ends in a cell of which has a value of at least 12 | uint8 GOAL_TYPE_POSE = 0 13 | uint8 GOAL_TYPE_MAP = 1 14 | uint8 type 15 | 16 | 17 | 18 | ### POSE SEARCH 19 | ## only if == GOAL_TYPE_POSE 20 | 21 | geometry_msgs/PoseStamped pose 22 | 23 | 24 | 25 | ### MAP SEARCH 26 | ## only if == GOAL_TYPE_MAP 27 | 28 | # map: A map coding for the goal locations using the threshold 29 | nav_msgs/OccupancyGrid map 30 | 31 | # map_search_min_value: Minimum cell value to be considered a goal [defaults to 32] 32 | int32 map_search_min_value 33 | 34 | # map_search_min_value: Minimum number of candidate cells to find before terminating [defaults to 64] 35 | int32 map_search_min_candidates 36 | 37 | # min_dist: The minimum required path length [defaults to 0.0] 38 | float32 min_dist 39 | 40 | -------------------------------------------------------------------------------- /gerona_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gerona_examples 4 | 1.0.0 5 | The gerona_examples package provides launch files for easy demonstration of GeRoNa 6 | 7 | Sebastian Buck 8 | 9 | Goran Huskic 10 | Sebastian Buck 11 | Julian Jordan 12 | Adrian Zwiener 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | rospy 19 | 20 | path_msgs 21 | path_control 22 | path_planner 23 | path_follower 24 | path_rviz 25 | 26 | global_planner 27 | turtlebot_stage 28 | amcl 29 | summit_xl_gazebo 30 | summit_xl_localization 31 | pr2_common 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /path_follower/src/controller/robotcontroller_ackermann_modelbased.cpp: -------------------------------------------------------------------------------- 1 | // HEADER 2 | #include 3 | // PROJECT 4 | #include 5 | 6 | 7 | REGISTER_ROBOT_CONTROLLER(RobotController_Ackermann_ModelBased, ackermann_modelbased, ackermann); 8 | 9 | RobotController_Ackermann_ModelBased::RobotController_Ackermann_ModelBased(): 10 | RobotController_ModelBased() 11 | { 12 | } 13 | 14 | 15 | void RobotController_Ackermann_ModelBased::initialize() 16 | { 17 | RobotController_ModelBased::initialize(); 18 | wheelBaseLength_ = config.wheelsConfig_.wheelPosRobotFrontX-config.wheelsConfig_.wheelPosRobotRearX; 19 | 20 | } 21 | 22 | 23 | double convert_trans_rot_vel_to_steering_angle(double v,double omega, double wheelbase) 24 | { 25 | if (omega == 0 || v == 0) return 0; 26 | 27 | double radius = v / omega; 28 | return atan(wheelbase / radius); 29 | 30 | } 31 | 32 | void RobotController_Ackermann_ModelBased::publishMoveCommand(const MoveCommand &cmd) const 33 | { 34 | geometry_msgs::Twist msg; 35 | msg.linear.x = cmd.getVelocity(); 36 | msg.linear.y = 0; 37 | msg.angular.z = convert_trans_rot_vel_to_steering_angle(cmd.getVelocity(),cmd.getRotationalVelocity(),wheelBaseLength_); 38 | 39 | cmd_pub_.publish(msg); 40 | } 41 | 42 | -------------------------------------------------------------------------------- /path_follower/launch/follower_ackermann_stanley.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/constraints/dis2path_constraint.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | double Dis2Path_Constraint::D_RATE = std::sin(5.0*M_PI/36.0); 5 | double Dis2Path_Constraint::DIS2P_ = 0.5; 6 | 7 | Dis2Path_Constraint::Dis2Path_Constraint(): 8 | Constraint(),limit(DIS2P_),level(-1) 9 | { 10 | 11 | } 12 | 13 | Dis2Path_Constraint::~Dis2Path_Constraint() 14 | { 15 | 16 | } 17 | 18 | void Dis2Path_Constraint::setParams(double new_limit){ 19 | limit = new_limit; 20 | level = 0; 21 | } 22 | 23 | double Dis2Path_Constraint::getLimit(){ 24 | return limit; 25 | } 26 | 27 | void Dis2Path_Constraint::setDRate(double d_rate){ 28 | D_RATE = d_rate; 29 | } 30 | 31 | void Dis2Path_Constraint::setLimit(double dis2p){ 32 | DIS2P_ = dis2p; 33 | } 34 | 35 | bool Dis2Path_Constraint::isSatisfied(const LNode& point){ 36 | sw.resume(); 37 | 38 | limit = DIS2P_; 39 | 40 | //this should be a parameter 41 | double obst_min_dist = 4.0; 42 | double enhancement_fact = 4.0; 43 | if(point.d2o <= obst_min_dist){ 44 | limit *= enhancement_fact; 45 | } 46 | else{ 47 | limit = DIS2P_; 48 | } 49 | 50 | if(point.d2p <= limit){ 51 | sw.stop(); 52 | return true; 53 | } 54 | sw.stop(); 55 | return false; 56 | } 57 | -------------------------------------------------------------------------------- /path_msgs/action/NavigateToGoal.action: -------------------------------------------------------------------------------- 1 | ## Request to navigate the robot to a goal 2 | 3 | # goal: Specification of the goal condition 4 | path_msgs/Goal goal 5 | 6 | # planner_options: Options for the path planner 7 | path_msgs/PlannerOptions planner_options 8 | 9 | # follower_options: Options for the path follower 10 | path_msgs/FollowerOptions follower_options 11 | 12 | # failure_mode: Determines how to react to a failure in path following: 13 | # FAILURE_MODE_ABORT: Stop navigating 14 | # FAILURE_MODE_REPLAN: Plan a new path to the same goal 15 | uint8 FAILURE_MODE_ABORT = 0 16 | uint8 FAILURE_MODE_REPLAN = 1 17 | uint8 failure_mode 18 | 19 | --- 20 | # Define the result 21 | uint8 STATUS_OTHER_ERROR = 0 22 | uint8 STATUS_SUCCESS = 1 23 | uint8 STATUS_ABORTED = 2 24 | uint8 STATUS_OBSTACLE = 3 25 | uint8 STATUS_TIMEOUT = 4 26 | uint8 STATUS_LOST_PATH = 5 27 | uint8 STATUS_NO_PATH_FOUND = 6 28 | 29 | bool reached_goal 30 | uint8 status 31 | 32 | --- 33 | # Define a feedback message 34 | # Maybe something like distance to goal or elapsed time? 35 | 36 | uint8 STATUS_MOVING = 1 37 | uint8 STATUS_OBSTACLE = 2 38 | uint8 STATUS_PATH_READY = 3 39 | uint8 STATUS_REPLAN = 4 40 | uint8 STATUS_REPLAN_FAILED = 5 # used if max num of replan attempts is reached without success 41 | uint8 STATUS_NO_LOCAL_PLAN = 6 42 | 43 | uint8 status 44 | 45 | Obstacle[] obstacles_on_path 46 | -------------------------------------------------------------------------------- /tools/model_based_planner/config/husky_c.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | % 3 | % base plate to ground distance 0.13228m 4 | % base size length: 0.809m width: 0.417m 5 | % edge height: 0.04344m edge length: 0.04982m (-0.35275 to -0.40257) 6 | --- 7 | Robot: 8 | baseLinkPosCoord: [ 0., 0. ] 9 | chassisTestTipAngleThreshold: 0.99 10 | Chassis: 11 | chassisfileName: "husky.png" 12 | chassisImageCenter: [ -1., -1. ] 13 | chassisImageValueOffset: 0 14 | chassisImageValueScale: 1.0 15 | chassisModelYSize: 0.421 16 | chassisPosRobot: [ 0.0, 0. ] 17 | testChassis: 1 18 | Wheels: 19 | wheelPosRobotFrontX: 0.256 20 | wheelPosRobotRearX: -0.256 21 | wheelPosRobotFrontY: 0.2854 22 | wheelPosRobotRearY: 0.2854 23 | wheelJointPosFront: [ 0., 0. ] 24 | wheelRadiusFront: 0.1651 25 | wheelWidthFront: 0.1143 26 | wheelLatRadiusFront: 0.4 27 | wheelJointPosRear: [ 0., 0. ] 28 | wheelRadiusRear: 0.1651 29 | wheelWidthRear: 0.1143 30 | wheelLatRadiusRear: 0.4 31 | frontWheelsTurnable: 0 32 | wheelRotTestSteps: 0 33 | wheelRotTestStepSize: 0 34 | Proc: 35 | numAngleStep: 360 36 | heightScale: 1000.0 37 | mapBaseHeight: 10000 38 | wheelGroundLevel: 20000 39 | maxHeight: 30000 40 | pixelSize: 0.00625 41 | imagePosBLMinX: 0.75 42 | imagePosBLMinY: -1.0 43 | validThresholdFactor: 0.95 44 | convertImage: 0 45 | 46 | 47 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/local_planner_null.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | REGISTER_LOCAL_PLANNER(LocalPlannerNull, Null); 11 | 12 | 13 | LocalPlannerNull::LocalPlannerNull() 14 | { 15 | 16 | } 17 | 18 | void LocalPlannerNull::setGlobalPath(Path::Ptr path) 19 | { 20 | /* 21 | * this local planner does nothing, so the global path is 22 | * equal to the local path! 23 | */ 24 | AbstractLocalPlanner::setGlobalPath(path); 25 | 26 | controller_->setGlobalPath(path); 27 | //path->reset(); // TODO: Check for other fix. Follower crashed when too short paths with only 1 subpath were set 28 | controller_->setPath(path); 29 | } 30 | 31 | Path::Ptr LocalPlannerNull::updateLocalPath() 32 | { 33 | // do nothing 34 | return nullptr; 35 | } 36 | 37 | bool LocalPlannerNull::isNull() const 38 | { 39 | return true; 40 | } 41 | 42 | void LocalPlannerNull::setParams(const LocalPlannerParameters &opt){ 43 | } 44 | 45 | void LocalPlannerNull::setVelocity(geometry_msgs::Twist::_linear_type vector){ 46 | (void) vector; 47 | } 48 | 49 | void LocalPlannerNull::setVelocity(double velocity){ 50 | (void) velocity; 51 | } 52 | -------------------------------------------------------------------------------- /tools/model_based_planner/include/model_based_planner/scaleddrawproc.h: -------------------------------------------------------------------------------- 1 | #ifndef SCALEDDRAWPROC_H 2 | #define SCALEDDRAWPROC_H 3 | 4 | 5 | #include 6 | 7 | /** 8 | * @brief Helper class for drawing debug images 9 | */ 10 | class ScaledDrawProc 11 | { 12 | public: 13 | ScaledDrawProc(); 14 | 15 | void SetMatNoScale(cv::Mat &image); 16 | 17 | void SetMat(cv::Mat &image); 18 | 19 | void SetScaleFactor(float factor) {scaleFactor_ = factor;} 20 | 21 | void DrawImage(cv::Mat &image, cv::Point2f pos); 22 | void DrawImage(cv::Mat &image, cv::Mat &mask, cv::Point2f pos); 23 | 24 | void DrawLineScaled(cv::Point2f p1, cv::Point2f p2, cv::Scalar color); 25 | void DrawLineScaled(cv::Point2f p1, cv::Point2f p2, cv::Scalar color, int thickness); 26 | 27 | void DrawCrossScaled(cv::Point2f p1, float size, cv::Scalar color); 28 | 29 | void DrawCircleScaled(cv::Point2f p1, float size, cv::Scalar color); 30 | void DrawCircleScaled(cv::Point2f p1, float size, cv::Scalar color, int thickness); 31 | 32 | cv::Mat GetImage() {return image_;} 33 | 34 | void PutText(cv::Point2f p1, std::string text, cv::Scalar color); 35 | 36 | void DrawArrowScaled(cv::Point2f p1, float angle, float length, float radius, cv::Scalar color); 37 | 38 | 39 | private: 40 | cv::Mat image_; 41 | float scaleFactor_; 42 | }; 43 | 44 | #endif // SCALEDDRAWPROC_H 45 | -------------------------------------------------------------------------------- /gerona_examples/launch/gazebo_example_summit.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 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /navigation_launch/launch/model_params/model_summit_dwa.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 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /navigation_launch/launch/model_params/model_summit_tree.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 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /navigation_launch/launch/model_params/model_rollator_star.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 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /navigation_launch/launch/model_params/model_rollator_tree.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 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /navigation_launch/launch/model_params/model_summit_star.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 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /navigation_launch/launch/model_params/model_rollator_dwa.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 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /gerona_examples/launch/gazebo_example_summit_mbc.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 | -------------------------------------------------------------------------------- /gerona_examples/launch/subt_tunnel_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /path_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | path_planner 4 | 1.0.0 5 | The path_planner package 6 | 7 | Sebastian Buck 8 | 9 | Goran Huskic 10 | Sebastian Buck 11 | Julian Jordan 12 | Adrian Zwiener 13 | 14 | BSD 15 | 16 | catkin 17 | 18 | nav_msgs 19 | path_msgs 20 | cslibs_path_planning 21 | cslibs_navigation_utilities 22 | roscpp 23 | std_msgs 24 | tf 25 | roslib 26 | pcl_ros 27 | 28 | nav_msgs 29 | path_msgs 30 | cslibs_path_planning 31 | cslibs_navigation_utilities 32 | roscpp 33 | std_msgs 34 | tf 35 | roslib 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /path_follower/src/controller/robotcontroller_ackermann_orthexp.cpp: -------------------------------------------------------------------------------- 1 | // HEADER 2 | #include 3 | 4 | 5 | // PROJECT 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | REGISTER_ROBOT_CONTROLLER(RobotController_Ackermann_OrthogonalExponential, ackermann_orthexp, ackermann); 13 | 14 | using namespace Eigen; 15 | 16 | 17 | RobotController_Ackermann_OrthogonalExponential::RobotController_Ackermann_OrthogonalExponential(): 18 | RobotController_OrthogonalExponential() 19 | { 20 | } 21 | 22 | void RobotController_Ackermann_OrthogonalExponential::computeControl() 23 | { 24 | //control 25 | 26 | 27 | 28 | // get the pose as pose(0) = x, pose(1) = y, pose(2) = theta 29 | Eigen::Vector3d current_pose = pose_tracker_->getRobotPose(); 30 | 31 | double exp_factor = RobotController::exponentialSpeedControl(); 32 | cmd_.speed = getDirSign() * vn_ * exp_factor; 33 | // theta_e = theta_path - theta_vehicle 34 | double theta_e = MathHelper::AngleDelta(current_pose[2], path_interpol.theta_p(proj_ind_)); 35 | if(getDirSign() < 0){ 36 | theta_e = MathHelper::NormalizeAngle(M_PI + theta_e); 37 | } 38 | 39 | cmd_.direction_angle = atan(-opt_.k()*orth_proj_) + theta_e; 40 | 41 | //***// 42 | } 43 | -------------------------------------------------------------------------------- /tools/odom2tf/src/odom2tf_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) { 6 | ros::init(argc, argv, "odom2tf", ros::init_options::NoSigintHandler); 7 | 8 | ros::NodeHandle nh; 9 | ros::NodeHandle p_nh("~"); 10 | 11 | const auto& odom_frame = p_nh.param("odom_frame", "odom"); 12 | const auto& base_link_frame = p_nh.param("base_link_frame", "base_link"); 13 | 14 | tf2_ros::TransformBroadcaster tf_broadcast; 15 | 16 | boost::function callback = 17 | [&](const nav_msgs::Odometry::ConstPtr &odom) { 18 | geometry_msgs::TransformStamped transform; 19 | transform.header = odom->header; 20 | transform.header.frame_id = odom_frame; 21 | transform.child_frame_id = base_link_frame; 22 | transform.transform.rotation = odom->pose.pose.orientation; 23 | transform.transform.translation.x = odom->pose.pose.position.x; 24 | transform.transform.translation.y = odom->pose.pose.position.y; 25 | transform.transform.translation.z = odom->pose.pose.position.z; 26 | 27 | tf_broadcast.sendTransform(transform); 28 | }; 29 | 30 | auto sub = nh.subscribe("odom", 10, callback); 31 | 32 | ros::Rate spin_rate(100); 33 | while (nh.ok()) { 34 | ros::spinOnce(); 35 | spin_rate.sleep(); 36 | } 37 | 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /path_follower/src/local_planner/high_speed/local_planner_bfs.cpp: -------------------------------------------------------------------------------- 1 | /// HEADER 2 | #include 3 | 4 | /// PROJECT 5 | 6 | 7 | // this planner uses the Breadth-first search algorithm 8 | LocalPlannerBFS::LocalPlannerBFS() 9 | : fifo() 10 | { 11 | 12 | } 13 | 14 | void LocalPlannerBFS::setInitScores(LNode& wpose, double& dis2last){ 15 | (void) wpose; 16 | (void) dis2last; 17 | } 18 | 19 | void LocalPlannerBFS::initQueue(LNode& root){ 20 | std::queue empty; 21 | fifo.swap(empty); 22 | fifo.push(&root); 23 | } 24 | 25 | bool LocalPlannerBFS::isQueueEmpty(){ 26 | return fifo.empty(); 27 | } 28 | 29 | LNode* LocalPlannerBFS::queueFront(){ 30 | return fifo.front(); 31 | } 32 | 33 | void LocalPlannerBFS::pop(LNode*& current){ 34 | current = fifo.front(); 35 | fifo.pop(); 36 | } 37 | 38 | void LocalPlannerBFS::push2Closed(LNode*& current){ 39 | (void) current; 40 | } 41 | 42 | void LocalPlannerBFS::expandCurrent(LNode*& current, std::size_t& nsize, std::vector& successors, 43 | std::vector& nodes){ 44 | getSuccessors(current, nsize, successors, nodes); 45 | } 46 | 47 | bool LocalPlannerBFS::processSuccessor(LNode*& succ, LNode*& current, 48 | double& current_p, double& dis2last){ 49 | (void) current; 50 | fifo.push(succ); 51 | evaluate(current_p, succ, dis2last); 52 | return true; 53 | } 54 | -------------------------------------------------------------------------------- /gerona_examples/launch/include/gerona_stage_params.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/controller/robotcontroller_ackermann_modelbased.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTCONTROLLER_ACKERMANN_MODELBASED_H 2 | #define ROBOTCONTROLLER_ACKERMANN_MODELBASED_H 3 | 4 | 5 | /// PROJECT 6 | #include 7 | 8 | /// The RobotController_Ackermann_ModelBased class 9 | class RobotController_Ackermann_ModelBased: public RobotController_ModelBased 10 | { 11 | public: 12 | /** 13 | * @brief RobotController_Ackermann_ModelBased 14 | */ 15 | RobotController_Ackermann_ModelBased(); 16 | 17 | protected: 18 | 19 | /** 20 | * @brief publishMoveCommand publishes the computed move command 21 | * 22 | * The command velocity is set depending on the kinematics of the robot. E.g. for 23 | * differential drives the command input is (v, w), where v is linear, and w angular velocity, 24 | * and for an Ackermann drive, the command input is (v, phi), where v is linear velocity, and 25 | * phi is the steering angle. For an omnidirectional vehicle, it is possible to directly set 26 | * the linear velocity and the direction angle, while the rotation is set independently. 27 | * 28 | * @param cmd 29 | */ 30 | virtual void publishMoveCommand(const MoveCommand &cmd) const; 31 | 32 | /** 33 | * @brief initialize 34 | */ 35 | virtual void initialize(); 36 | 37 | 38 | double wheelBaseLength_; 39 | }; 40 | 41 | 42 | 43 | #endif // ROBOTCONTROLLER_POTENTIAL_FIELD_H 44 | -------------------------------------------------------------------------------- /path_planner/launch/planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /gerona_examples/nodes/gazebo2tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Created on Thu Aug 30 13:52:35 2018 4 | 5 | @author: jordan 6 | 7 | Simple node that converts the model state from gazebo for specified model to a tf transform. 8 | """ 9 | 10 | import rospy 11 | import tf 12 | from gazebo_msgs.msg import ModelStates 13 | import geometry_msgs.msg 14 | 15 | 16 | def ModelStateCallback(data): 17 | 18 | idx = -1 19 | c = 0 20 | for name in data.name: 21 | if (name == modelName): 22 | idx = c 23 | 24 | c += 1 25 | 26 | if (idx < 0): return 27 | 28 | 29 | pose = data.pose[idx] 30 | twist = data.twist[idx] 31 | 32 | 33 | br = tf.TransformBroadcaster() 34 | 35 | t = geometry_msgs.msg.TransformStamped() 36 | t.transform.translation = pose.position 37 | t.transform.rotation = pose.orientation 38 | t.child_frame_id = tfRobotFrame 39 | t.header.frame_id = tfWorldFrame 40 | t.header.stamp = rospy.Time.now() 41 | 42 | br.sendTransformMessage(t) 43 | 44 | 45 | rospy.init_node('gazebo2tf', anonymous=True) 46 | modelName = rospy.get_param("~modelName","summit_xl") 47 | tfRobotFrame = rospy.get_param("~tfRobotFrame","base_footprint") 48 | tfWorldFrame = rospy.get_param("~tfWorldFrame","odom") 49 | 50 | 51 | 52 | 53 | def listener(): 54 | rospy.Subscriber("/gazebo/model_states", ModelStates, ModelStateCallback) 55 | 56 | rospy.spin() 57 | 58 | if __name__ == '__main__': 59 | listener() 60 | -------------------------------------------------------------------------------- /tools/scan2cloud/include/scan2cloud/scan2cloud.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | // pcl 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | class ScanConverter { 14 | public: 15 | ScanConverter(); 16 | void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in, bool is_back); 17 | void spin(); 18 | private: 19 | ros::NodeHandle node_; 20 | ros::NodeHandle private_node_; 21 | laser_geometry::LaserProjection projector_; 22 | tf::TransformListener tfListener_; 23 | 24 | ros::Publisher point_cloud_publisher_; 25 | ros::Subscriber scan_sub_back_; 26 | ros::Subscriber scan_sub_front_; 27 | sensor_msgs::PointCloud2 cloud_front_; 28 | sensor_msgs::PointCloud2 cloud_back_; 29 | sensor_msgs::PointCloud2 cloud_total_; 30 | 31 | 32 | bool cbScanfront_; 33 | bool cbScanback_; 34 | 35 | // ros params 36 | std::string baseFrame_; 37 | double cloudFilterMean_; 38 | double cloudFilterStdD_; 39 | 40 | 41 | void mergeSensorMsgsPointCloud2(); 42 | void filter(const pcl::PointCloud::Ptr &cloudIn, 43 | pcl::PointCloud::Ptr &cloudOut); 44 | }; 45 | -------------------------------------------------------------------------------- /path_follower/scripts/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | roslib.load_manifest('path_follower') 4 | import rospy 5 | import tf 6 | from nav_msgs.msg import Path 7 | 8 | 9 | import matplotlib.pyplot as plt 10 | 11 | xpath = [] 12 | ypath = [] 13 | 14 | def path_callback(data): 15 | global xpath 16 | global ypath 17 | 18 | xpath = [] 19 | ypath = [] 20 | 21 | for i in range(len(data.poses)): 22 | xpath.append(data.poses[i].pose.position.x) 23 | ypath.append(data.poses[i].pose.position.y) 24 | 25 | print xpath, ypath 26 | 27 | if __name__ == '__main__': 28 | rospy.init_node('path_plotter') 29 | 30 | listener = tf.TransformListener() 31 | 32 | rospy.Subscriber("/path", Path, path_callback) 33 | 34 | fig = plt.figure() 35 | 36 | x = [] 37 | y = [] 38 | 39 | plt.ion() 40 | plt.show() 41 | 42 | ax = fig.add_subplot(111) 43 | 44 | rate = rospy.Rate(60.0) 45 | while not rospy.is_shutdown(): 46 | try: 47 | (trans,rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0)) 48 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 49 | continue 50 | 51 | x.append(trans[0]); 52 | y.append(trans[1]); 53 | 54 | ax.clear() 55 | # ax.set_xlim(-10,10) 56 | # ax.set_ylim(-10,10) 57 | ax.plot(x, y) 58 | ax.plot(xpath, ypath) 59 | plt.draw() 60 | 61 | rate.sleep() 62 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/collision_avoidance/collision_detector_polygon.h: -------------------------------------------------------------------------------- 1 | #ifndef COLLISION_DETECTOR_POLYGON_H 2 | #define COLLISION_DETECTOR_POLYGON_H 3 | 4 | #include "collision_detector.h" 5 | #include 6 | #include 7 | 8 | class CollisionDetectorPolygon : public CollisionDetector 9 | { 10 | protected: 11 | struct PolygonWithTfFrame 12 | { 13 | std::string frame; 14 | std::vector polygon; 15 | }; 16 | 17 | 18 | /** 19 | * @brief Check, if there is an obstacle within the polygon, given by getPolygon(). 20 | * @see getPolygon() 21 | * @return True if there is an obstacle, false if not. 22 | */ 23 | virtual bool checkOnCloud(std::shared_ptr obstacles, 24 | float width, 25 | float length, 26 | float course_angle, 27 | float curve_enlarge_factor); 28 | 29 | 30 | /** 31 | * @brief Get the polygon which is checked for obstacles. 32 | * @return Collision polygon. 33 | */ 34 | virtual PolygonWithTfFrame getPolygon(float width, 35 | float length, 36 | float course_angle, 37 | float curve_enlarge_factor) const = 0; 38 | 39 | 40 | private: 41 | void visualize(PolygonWithTfFrame polygon, bool hasObstacle) const; 42 | }; 43 | 44 | #endif // COLLISION_DETECTOR_POLYGON_H 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017, Cognitive Systems, University of Tübingen, Germany 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of the University of Tuebingen nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /path_follower/src/controller/robotcontroller_omnidrive_orthexp.cpp: -------------------------------------------------------------------------------- 1 | // PROJECT 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | REGISTER_ROBOT_CONTROLLER(RobotController_Omnidrive_OrthogonalExponential, omnidrive_orthexp, omnidrive); 9 | 10 | using namespace Eigen; 11 | 12 | namespace { 13 | //! Module name, that is used for ros console output 14 | const std::string MODULE = "controller"; 15 | } 16 | 17 | RobotController_Omnidrive_OrthogonalExponential::RobotController_Omnidrive_OrthogonalExponential(): 18 | RobotController_OrthogonalExponential(), 19 | e_theta_curr_(0) 20 | { 21 | 22 | } 23 | 24 | void RobotController_Omnidrive_OrthogonalExponential::computeControl() 25 | { 26 | // get the pose as pose(0) = x, pose(1) = y, pose(2) = theta 27 | Eigen::Vector3d current_pose = pose_tracker_->getRobotPose(); 28 | 29 | double e_theta_new = MathHelper::NormalizeAngle(theta_des_ - current_pose[2]); 30 | double e_theta_prim = (e_theta_new - e_theta_curr_)/Ts_; 31 | 32 | e_theta_curr_ = e_theta_new; 33 | 34 | double exp_factor = RobotController::exponentialSpeedControl(); 35 | cmd_.speed = vn_* exp_factor; 36 | cmd_.direction_angle = atan(-opt_.k()*orth_proj_) + path_interpol.theta_p(proj_ind_) - current_pose[2]; 37 | double omega = opt_.kp()*e_theta_curr_ + opt_.kd()*e_theta_prim; 38 | cmd_.rotation = boost::algorithm::clamp(omega, -opt_.max_ang_velocity(), opt_.max_ang_velocity()); 39 | } 40 | -------------------------------------------------------------------------------- /tools/localmap/launch/localmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /tools/localmap/launch/localmap_mf_640_6.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /tools/localmap/launch/localmap_mf_768_8.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /tools/localmap/launch/localmap_mf_1024_10.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /tools/localmap/launch/localmap_rs_mf_768_8.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/controller/robotcontroller_ackermann_orthexp.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTCONTROLLER_ACKERMANN_ORTHEXP_H 2 | #define ROBOTCONTROLLER_ACKERMANN_ORTHEXP_H 3 | 4 | /// PROJECT 5 | #include 6 | #include 7 | #include 8 | 9 | class RobotController_Ackermann_OrthogonalExponential: public RobotController_OrthogonalExponential 10 | { 11 | public: 12 | /** 13 | * @brief RobotController_Ackermann_OrthogonalExponential 14 | */ 15 | RobotController_Ackermann_OrthogonalExponential(); 16 | 17 | protected: 18 | /** 19 | * @brief computeControl computes the command velocity specific for every orthogonal-exponential controller 20 | * 21 | * Orthogonal-exponential controller is in principle the same for every wheeled robot, but the command 22 | * output is computed differently for different kinematic types. 23 | * 24 | */ 25 | virtual void computeControl(); 26 | 27 | private: 28 | struct ControllerParameters : public RobotController_OrthogonalExponential::ControllerParameters 29 | { 30 | P k; 31 | 32 | ControllerParameters(): 33 | RobotController_OrthogonalExponential::ControllerParameters("ackermann_orthexp"), 34 | 35 | k(this, "k", 1.5, "Factor for regulating the convergence to the path.") 36 | {} 37 | } opt_; 38 | 39 | const RobotController::ControllerParameters& getParameters() const 40 | { 41 | return opt_; 42 | } 43 | }; 44 | 45 | #endif // ROBOTCONTROLLER_ACKERMANN_ORTHEXP_H 46 | -------------------------------------------------------------------------------- /path_msgs/msg/PlannerOptions.msg: -------------------------------------------------------------------------------- 1 | # Options for path planning algorithms 2 | 3 | # max_search_duration: [optional] Number of seconds before the search terminates [defaults to infinity] 4 | float32 max_search_duration 5 | 6 | # has_search_dir: Determines whether a map-based search should use as a heuristic goal [defaults to false] 7 | bool has_search_dir 8 | # search_dir: [optional] The heuristic goal for a map-based search 9 | geometry_msgs/Point search_dir 10 | 11 | # grow_obstacle: [optional] Determines whether to grow obstacles [defaults to false] 12 | bool grow_obstacles 13 | # obstacle_growth_radius: [optional] The radius to grow obstacles by 14 | float32 obstacle_growth_radius 15 | 16 | 17 | ### IMPLEMENTATION SPECIFIC OPTIONS 18 | ## Note that not all path planners support these options! 19 | 20 | # goal_dist_threshold: Locations closer than this value are considered to be the goal 21 | float32 goal_dist_threshold 22 | 23 | # goal_angle_threshold_degree: Orientations closer than this value are considered to be the goal 24 | float32 goal_angle_threshold_degree 25 | 26 | # reversed: If true, the planner is requested to plan from the goal pose to the start 27 | bool reversed 28 | 29 | 30 | # allow_forward: Determines whether to allow forward segments 31 | bool allow_forward 32 | 33 | # allow_forward: Determines whether to allow backward segments 34 | bool allow_backward 35 | 36 | #note: if both directions are false (or not specified), both are considered true 37 | 38 | 39 | ## Ackermann specifica options 40 | float32 ackermann_la 41 | int32 ackermann_steer_steps 42 | float32 ackermann_max_steer_angle_degree 43 | float32 ackermann_steer_delta_degree 44 | 45 | -------------------------------------------------------------------------------- /navigation_launch/launch/navigation_static_course.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 | [[-2.0, 1.0], [-2.0, 0.0]], 27 | 28 | [[-2.0, 0.0], [13.5, 0.0]], 29 | [[13.5, -30.0], [13.5, 52.4]], 30 | [[13.5, 52.4],[8.0, 52.4]], 31 | [[8.0, 52.4], [8.0, 50.4]], 32 | [[8.0, 50.4], [13.5, 50.4]], 33 | [[12.5, 50.4], [12.5, -30.0]], 34 | [[13.5, 1.0], [-2.0, 1.0]], 35 | 36 | [[9.5, 52.4],[8.7, 59.0]], 37 | [[7.2, 59.0], [8.0, 52.4]], 38 | [[8.7, 59.0], [7.2, 59.0]], 39 | 40 | 41 | [[13.5, -24.5], [2.5, -24.5]], 42 | [[13.5, -25.5], [2.5, -25.5]], 43 | ] 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/utils/path_follower_config.h: -------------------------------------------------------------------------------- 1 | #ifndef PATH_FOLLOWER_CONFIG_H 2 | #define PATH_FOLLOWER_CONFIG_H 3 | 4 | #include 5 | #include 6 | 7 | class RobotController; 8 | class AbstractLocalPlanner; 9 | class CollisionAvoider; 10 | 11 | /** 12 | * @brief The PathFollowerConfigName struct represents a configuration for 13 | * following a path. This configuration is used by the factories to create 14 | * instances of the nescessary classes in the form of PathFollowerConfig. 15 | * @see PathFollowerConfig 16 | */ 17 | struct PathFollowerConfigName 18 | { 19 | std::string controller; 20 | std::string local_planner; 21 | std::string collision_avoider; 22 | 23 | bool operator < (const PathFollowerConfigName& rhs) const 24 | { 25 | if(controller < rhs.controller) { 26 | return true; 27 | } 28 | if(local_planner < rhs.local_planner) { 29 | return true; 30 | } 31 | if(collision_avoider < rhs.collision_avoider) { 32 | return true; 33 | } 34 | 35 | return false; 36 | } 37 | }; 38 | 39 | /** 40 | * @brief The PathFollowerConfig struct holds instances of the necessary classes 41 | * for path following and represents a single configuration. 42 | */ 43 | struct PathFollowerConfig 44 | { 45 | //! The robot controller is responsible for everything that is dependend on robot model and controller type. 46 | std::shared_ptr controller_; 47 | std::shared_ptr local_planner_; 48 | std::shared_ptr collision_avoider_; 49 | }; 50 | 51 | #endif // PATH_FOLLOWER_CONFIG_H 52 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/local_planner/high_speed/local_planner_star.h: -------------------------------------------------------------------------------- 1 | #ifndef LOCAL_PLANNER_STAR_H 2 | #define LOCAL_PLANNER_STAR_H 3 | 4 | /// PROJECT 5 | #include 6 | 7 | class LocalPlannerStar : virtual public LocalPlannerClassic 8 | { 9 | public: 10 | LocalPlannerStar(); 11 | private: 12 | virtual void setInitScores(LNode& wpose, double& dis2last) override; 13 | 14 | virtual void initQueue(LNode& root) override; 15 | 16 | virtual bool isQueueEmpty() override; 17 | 18 | virtual LNode* queueFront() override; 19 | 20 | virtual void pop(LNode*& current) override; 21 | 22 | virtual void push2Closed(LNode*& current) override; 23 | 24 | virtual void expandCurrent(LNode*& current, std::size_t& nsize, std::vector& successors, 25 | std::vector& nodes) override; 26 | 27 | virtual bool processSuccessor(LNode*& succ, LNode*& current, 28 | double& current_p,double& dis2last) override; 29 | 30 | virtual double f(double& g, double& score, double& heuristic) = 0; 31 | 32 | virtual double G(LNode*& current, LNode*& succ, 33 | double& score) = 0; 34 | 35 | virtual void updateSucc(LNode*& current, LNode*& f_current, LNode& succ) = 0; 36 | 37 | virtual void evaluate(double& current_p, double& heuristic, double& score) = 0; 38 | 39 | private: 40 | typedef std::multiset prio_queue; 41 | double score, heuristic; 42 | std::vector closedSet; 43 | std::vector twins; 44 | prio_queue openSet; 45 | }; 46 | 47 | #endif // LOCAL_PLANNER_STAR_H 48 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/utils/elevation_map.h: -------------------------------------------------------------------------------- 1 | #ifndef ELEVATION_MAP_H 2 | #define ELEVATION_MAP_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | namespace tf 12 | { 13 | class Transform; 14 | } 15 | 16 | namespace cv 17 | { 18 | class Mat; 19 | 20 | } 21 | 22 | /** 23 | * @brief The ObstacleCloud class represents all currently known obstacles. 24 | */ 25 | class ElevationMap 26 | { 27 | public: 28 | using Ptr = std::shared_ptr; 29 | using ConstPtr = std::shared_ptr; 30 | 31 | // PointXY would be sufficient, but buildin transform of point clouds works only with XYZ-points... 32 | using EMap = sensor_msgs::ImageConstPtr; 33 | using EMapType = sensor_msgs::Image; 34 | 35 | /** 36 | * @brief cloud the current obstacle cloud 37 | */ 38 | EMap elevationMap; 39 | 40 | ElevationMap(); 41 | ElevationMap(const EMap & c); 42 | 43 | /** 44 | * @brief empty 45 | * @return true, iff no obstacle exists 46 | */ 47 | bool empty() const; 48 | 49 | /** 50 | * @brief clear removes all current obstacles. 51 | */ 52 | void clear(); 53 | 54 | /** 55 | * @brief toCVMat 56 | * @return the opencv mat 57 | */ 58 | cv::Mat toCVMat() const; 59 | 60 | 61 | /** 62 | * @brief getStamp 63 | * @return the time stamp of this cloud 64 | */ 65 | ros::Time getStamp() const; 66 | 67 | /** 68 | * @brief getFrameId 69 | * @return the frame id of this cloud 70 | */ 71 | std::string getFrameId() const; 72 | }; 73 | 74 | #endif // ELEVATION_MAP_H 75 | -------------------------------------------------------------------------------- /path_planner/cmake/FindSBPL.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find the SBPL library 2 | # Once done this will define: 3 | # 4 | # SBPL_FOUND - SBPL was found 5 | # SBPL_INCLUDE_DIRS - The SBPL include directory 6 | # SBPL_LIBRARIES - The SBPL library 7 | 8 | include(FindPackageHandleStandardArgs) 9 | 10 | # user can set SBPL_PREFIX to specify the prefix path of the SBPL library 11 | # and include directory, either as an environment variable or as an 12 | # argument to cmake ("cmake -DSBPL_PREFIX=...") 13 | if (NOT SBPL_PREFIX) 14 | set(SBPL_PREFIX $ENV{SBPL_PREFIX}) 15 | endif() 16 | 17 | # user can set SBPL_LIB_PATH to specify the path for the SBPL library 18 | # (analogous to SBPL_PREFIX) 19 | if (NOT SBPL_LIB_PATH) 20 | set(SBPL_LIB_PATH $ENV{SBPL_LIB_PATH}) 21 | if (NOT SBPL_LIB_PATH) 22 | set(SBPL_LIB_PATH ${SBPL_PREFIX}) 23 | endif() 24 | endif() 25 | 26 | # user can set SBPL_INCLUDE_PATH to specify the path for the SBPL include 27 | # directory (analogous to SBPL_PREFIX) 28 | if (NOT SBPL_INCLUDE_PATH) 29 | set(SBPL_INCLUDE_PATH $ENV{SBPL_INCLUDE_PATH}) 30 | if (NOT SBPL_INCLUDE_PATH) 31 | set(SBPL_INCLUDE_PATH ${SBPL_PREFIX}) 32 | endif() 33 | endif() 34 | 35 | 36 | # find the SBPL library 37 | find_library(SBPL_LIBRARIES sbpl 38 | PATHS ${SBPL_LIB_PATH} 39 | /usr/local/lib 40 | PATH_SUFFIXES lib build/lib) 41 | 42 | # find include path 43 | find_path(SBPL_INCLUDE_DIRS viplanner.h 44 | PATHS ${SBPL_INCLUDE_PATH} 45 | /usr/local/include 46 | PATH_SUFFIXES sbpl/planners) 47 | 48 | if (SBPL_INCLUDE_DIRS) 49 | string(REGEX REPLACE "/sbpl/planners$" "" SBPL_INCLUDE_DIRS ${SBPL_INCLUDE_DIRS}) 50 | else() 51 | set(SBPL_INCLUDE_DIRS "") 52 | endif() 53 | 54 | find_package_handle_standard_args(SBPL DEFAULT_MSG SBPL_LIBRARIES SBPL_INCLUDE_DIRS) 55 | 56 | -------------------------------------------------------------------------------- /path_follower/include/path_follower/utils/maptransformer.h: -------------------------------------------------------------------------------- 1 | #ifndef MAPTRANSFORMER_H 2 | #define MAPTRANSFORMER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | //! Simple helper class to transform points from and to map coordinates 10 | class MapTransformer 11 | { 12 | public: 13 | MapTransformer(const tf::TransformListener *tf_listener): 14 | tf_listener_(tf_listener) 15 | {} 16 | 17 | void setMap(const nav_msgs::OccupancyGridConstPtr &map); 18 | 19 | /** 20 | * @brief Transform a cv::Point2f from an arbitrary TF-frame to map coordinates of the obstacle map. 21 | * @param p The point which is to be transformed. 22 | * @param from The TF-frame in which the point is defined. 23 | * @return The transformed point in (non-integer!) map coordinates. 24 | * @throws tf::TransformException 25 | * @throws std::runtime_error If no map was set. 26 | */ 27 | cv::Point2f transformPointToMap(const cv::Point2f &p, std::string from) const; 28 | 29 | /** 30 | * @brief Transform a cv::Point2f from map coordinates of the obstacle map to an arbitrary TF-frame. 31 | * @param p The point in map coordinates. 32 | * @param to The target TF-frame in which the point is to be transformed. 33 | * @return The transformed point. 34 | * @throws tf::TransformException 35 | * @throws std::runtime_error If no map was set. 36 | */ 37 | cv::Point2f transformPointFromMap(const cv::Point2f &p, std::string to) const; 38 | 39 | private: 40 | nav_msgs::OccupancyGridConstPtr map_; 41 | 42 | const tf::TransformListener *tf_listener_; 43 | 44 | tf::Transform trans_from_map_cell_to_map_frame_; 45 | }; 46 | 47 | #endif // MAPTRANSFORMER_H 48 | -------------------------------------------------------------------------------- /path_planner/src/course_planner/course/cost_calculator.cpp: -------------------------------------------------------------------------------- 1 | #include "cost_calculator.h" 2 | #include "transition.h" 3 | #include "node.h" 4 | #include "segment.h" 5 | #include "search.h" 6 | 7 | CostCalculator::CostCalculator(Search &search) 8 | : Analyzer(search), 9 | 10 | pnh_("~") 11 | { 12 | pnh_.param("course/penalty/backwards", backward_penalty_factor, 2.5); 13 | pnh_.param("course/penalty/turn", turning_penalty, 5.0); 14 | 15 | pnh_.param("course/turning/straight", turning_straight_segment, 0.7); 16 | } 17 | 18 | double CostCalculator::calculateStraightCost(Node* node, const Eigen::Vector2d& start_point_on_segment, const Eigen::Vector2d& end_point_on_segment) const 19 | { 20 | double cost = 0.0; 21 | bool segment_forward = isSegmentForward(node->next_segment, start_point_on_segment, end_point_on_segment); 22 | double distance_to_end = (end_point_on_segment - start_point_on_segment).norm(); 23 | if(segment_forward) { 24 | cost += distance_to_end; 25 | } else { 26 | cost += backward_penalty_factor * distance_to_end; 27 | } 28 | 29 | bool prev_segment_forward = isPreviousSegmentForward(node); 30 | if(prev_segment_forward != segment_forward) { 31 | // single turn 32 | cost += turning_straight_segment; 33 | cost += turning_penalty; 34 | 35 | } else if(segment_forward != node->curve_forward) { 36 | // double turn 37 | cost += 2 * turning_straight_segment; 38 | cost += 2 * turning_penalty; 39 | } 40 | 41 | return cost; 42 | } 43 | 44 | double CostCalculator::calculateCurveCost(Node *node) const 45 | { 46 | if(node->curve_forward) { 47 | return node->transition->arc_length(); 48 | } else { 49 | return backward_penalty_factor * node->transition->arc_length(); 50 | } 51 | } 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /tools/model_based_planner/include/model_based_planner/chassismodel.h: -------------------------------------------------------------------------------- 1 | #ifndef CHASSISMODEL_H 2 | #define CHASSISMODEL_H 3 | 4 | 5 | #include "chassisdescriptor.h" 6 | #include "config_robot.h" 7 | #include "config_proc.h" 8 | #include "wheeldescriptor.h" 9 | 10 | 11 | 12 | /** 13 | * @brief ChassisModel model implementation 14 | */ 15 | class ChassisModel 16 | { 17 | public: 18 | ChassisModel(); 19 | 20 | /** 21 | * @brief Setup chassis model 22 | */ 23 | void SetupChassis(const ProcConfig& procConfig, const ChassisConfig &conf); 24 | 25 | /** 26 | * @brief Get descriptor at index i without checking 27 | */ 28 | const ChassisDescriptor& GetDescriptorIdx(int i) const {return descriptors_[i];} 29 | 30 | 31 | /** 32 | * @brief Test for chassis collision at given pos and angle and determines contact points, requires startVal, dx, dy from the pose estimate 33 | */ 34 | int Evaluate(const cv::Mat &dem,const float &startVal, const float &dx, const float &dy, const cv::Point2f &pos, const int &angleIdx, int &cx, int &cy) const; 35 | /** 36 | * @brief Test for chassis collision at given pos and angle without contact points(faster), requires startVal, dx, dy from the pose estimate 37 | */ 38 | int EvaluateNP(const cv::Mat &dem,const float &startVal, const float &dx, const float &dy, const cv::Point2f &pos, const int &angleIdx, int &cx, int &cy) const; 39 | 40 | /** 41 | * @brief True if chassis testing is set 42 | */ 43 | inline bool TestChassis() const {return config_.testChassis;} 44 | 45 | 46 | 47 | private: 48 | ChassisConfig config_; 49 | /** 50 | * @brief vector conaining all chassis descriptors for each orientation 51 | */ 52 | std::vector descriptors_; 53 | 54 | 55 | }; 56 | 57 | #endif // CHASSISMODEL_H 58 | -------------------------------------------------------------------------------- /tools/localmap/launch/sim_summit_localmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /tools/path_rviz/src/path_display.h: -------------------------------------------------------------------------------- 1 | #ifndef RVIZ_PATH_SEQUENCE_DISPLAY_H 2 | #define RVIZ_PATH_SEQUENCE_DISPLAY_H 3 | 4 | #include 5 | 6 | #include "rviz/message_filter_display.h" 7 | 8 | namespace Ogre 9 | { 10 | class ManualObject; 11 | } 12 | 13 | namespace rviz 14 | { 15 | 16 | class ColorProperty; 17 | class FloatProperty; 18 | class IntProperty; 19 | class EnumProperty; 20 | class BillboardLine; 21 | class VectorProperty; 22 | 23 | 24 | /** 25 | * \class PathDisplay 26 | * \brief Displays a path_msgs::PathSequence message 27 | */ 28 | class PathSequenceDisplay: public MessageFilterDisplay 29 | { 30 | Q_OBJECT 31 | public: 32 | PathSequenceDisplay(); 33 | virtual ~PathSequenceDisplay(); 34 | 35 | /** @brief Overridden from Display. */ 36 | virtual void reset(); 37 | 38 | protected: 39 | /** @brief Overridden from Display. */ 40 | virtual void onInitialize(); 41 | 42 | /** @brief Overridden from MessageFilterDisplay. */ 43 | void processMessage( const path_msgs::PathSequence::ConstPtr& msg ); 44 | 45 | private Q_SLOTS: 46 | void updateBufferLength(); 47 | void updateStyle(); 48 | void updateLineWidth(); 49 | void updateOffset(); 50 | 51 | private: 52 | void destroyObjects(); 53 | 54 | std::vector> manual_objects_; 55 | std::vector> billboard_lines_; 56 | 57 | EnumProperty* style_property_; 58 | ColorProperty* color_forward_property_; 59 | ColorProperty* color_backward_property_; 60 | FloatProperty* alpha_property_; 61 | FloatProperty* line_width_property_; 62 | IntProperty* buffer_length_property_; 63 | VectorProperty* offset_property_; 64 | 65 | enum LineStyle { 66 | LINES, 67 | BILLBOARDS 68 | }; 69 | 70 | }; 71 | 72 | } // namespace rviz 73 | 74 | #endif /* RVIZ_PATH_DISPLAY_H */ 75 | 76 | --------------------------------------------------------------------------------