From 88c5e2b6fb3acb3cf9c38914aaa7de472c01db1b Mon Sep 17 00:00:00 2001 From: robofish <1683502971@qq.com> Date: Fri, 11 Jul 2025 22:40:01 +0800 Subject: [PATCH] =?UTF-8?q?=E8=83=BD=E6=A3=80=E6=B5=8B=E7=AF=AE=E6=9D=BF?= =?UTF-8?q?=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lankuang.pcd | Bin 0 -> 67296 bytes nav2.sh | 2 +- output.pcd | Bin 0 -> 129604 bytes src/rc_lidar/caijian.py | 63 + src/rc_lidar/circlr.py | 90 ++ src/rc_lidar/find.py | 59 + src/rc_lidar/juxing.py | 113 ++ src/rc_lidar/pcd2pgm.py | 75 + src/rc_lidar/save_pcd.py | 55 + src/rc_lidar/simple_icp.py | 126 ++ src/rc_lidar/xiamian.py | 75 + src/rc_lidar/zhaoban.py | 51 + src/rc_lidar/zhaoyuan.py | 0 .../rm_serial_driver/script/R2_Serial.py | 56 +- .../rm_serial_driver/script/slect.py | 2 +- .../icp_registration/config/icp.yaml | 2 +- .../mapper_params_online_async_real.yaml | 2 +- .../costmap_converter/.gitlab-ci.yml | 23 - .../costmap_converter/.travis.yml | 109 -- .../rm_navigation/costmap_converter/README.md | 30 - .../costmap_converter/CHANGELOG.rst | 105 -- .../costmap_converter/CMakeLists.txt | 97 -- .../CostmapToDynamicObstacles.cfg | 136 -- .../CostmapToLinesDBSMCCH.cfg | 41 - .../CostmapToLinesDBSRANSAC.cfg | 54 - .../CostmapToPolygonsDBSConcaveHull.cfg | 33 - .../CostmapToPolygonsDBSMCCH.cfg | 29 - .../costmap_converter_plugins.xml | 38 - .../costmap_converter_interface.h | 389 ----- .../background_subtractor.h | 115 -- .../blob_detector.h | 111 -- .../costmap_to_dynamic_obstacles.h | 212 --- .../multitarget_tracker/Ctracker.h | 96 -- .../multitarget_tracker/HungarianAlg.h | 60 - .../multitarget_tracker/Kalman.h | 20 - .../multitarget_tracker/README.md | 13 - .../multitarget_tracker/defines.h | 9 - .../costmap_to_lines_convex_hull.h | 137 -- .../costmap_to_lines_ransac.h | 187 --- .../costmap_converter/costmap_to_polygons.h | 335 ----- .../costmap_to_polygons_concave.h | 202 --- .../include/costmap_converter/misc.h | 157 -- .../costmap_converter/package.xml | 38 - .../src/costmap_converter_node.cpp | 264 ---- .../background_subtractor.cpp | 115 -- .../blob_detector.cpp | 193 --- .../costmap_to_dynamic_obstacles.cpp | 489 ------ .../multitarget_tracker/Ctracker.cpp | 130 -- .../multitarget_tracker/HungarianAlg.cpp | 732 --------- .../multitarget_tracker/Kalman.cpp | 99 -- .../multitarget_tracker/README.md | 13 - .../src/costmap_to_lines_convex_hull.cpp | 303 ---- .../src/costmap_to_lines_ransac.cpp | 361 ----- .../src/costmap_to_polygons.cpp | 514 ------- .../src/costmap_to_polygons_concave.cpp | 231 --- .../test/costmap_polygons.cpp | 230 --- .../test/costmap_polygons.test | 3 - .../costmap_converter_msgs/CHANGELOG.rst | 41 - .../costmap_converter_msgs/CMakeLists.txt | 26 - .../msg/ObstacleArrayMsg.msg | 10 - .../msg/ObstacleMsg.msg | 24 - .../costmap_converter_msgs/package.xml | 31 - .../fake_vel_transform/CMakeLists.txt | 55 - .../fake_vel_transform/README.md | 29 - .../fake_vel_transform/fake_vel_transform.hpp | 56 - .../launch/fake_vel_transform.launch.py | 20 - .../fake_vel_transform/package.xml | 31 - .../src/fake_vel_transform.cpp | 103 -- .../rm_navigation/CMakeLists.txt | 34 - .../launch/bringup_rm_navigation.py | 156 -- .../launch/localization_amcl_launch.py | 149 -- .../rm_navigation/launch/map_server_launch.py | 158 -- .../rm_navigation/launch/navigation_launch.py | 270 ---- .../rm_navigation/launch/rviz_launch.py | 109 -- .../rm_navigation/rm_navigation/package.xml | 20 - .../params/mapper_params_online_async.yaml | 73 - .../rm_navigation/params/nav2_params.yaml | 371 ----- .../rm_navigation/rviz/nav2.rviz | 665 --------- .../teb_local_planner/.gitignore | 3 - .../teb_local_planner/.gitlab-ci.yml | 23 - .../teb_local_planner/.travis.yml | 108 -- .../rm_navigation/teb_local_planner/README.md | 61 - .../teb_local_planner/CHANGELOG.rst | 407 ----- .../teb_local_planner/CMakeLists.txt | 162 -- .../teb_local_planner/LICENSE | 28 - .../cfg/TebLocalPlannerReconfigure.cfg | 420 ------ .../cfg/rviz_test_optim.rviz | 183 --- .../cmake_modules/FindG2O.cmake | 97 -- .../cmake_modules/FindSUITESPARSE.cmake | 133 -- .../teb_local_planner/distance_calculations.h | 464 ------ .../teb_local_planner/equivalence_relations.h | 103 -- .../g2o_types/base_teb_edges.h | 278 ---- .../g2o_types/edge_acceleration.h | 732 --------- .../g2o_types/edge_dynamic_obstacle.h | 154 -- .../g2o_types/edge_kinematics.h | 231 --- .../g2o_types/edge_obstacle.h | 295 ---- .../g2o_types/edge_prefer_rotdir.h | 115 -- .../g2o_types/edge_shortest_path.h | 88 -- .../g2o_types/edge_time_optimal.h | 117 -- .../g2o_types/edge_velocity.h | 275 ---- .../g2o_types/edge_velocity_obstacle_ratio.h | 127 -- .../g2o_types/edge_via_point.h | 121 -- .../teb_local_planner/g2o_types/penalties.h | 193 --- .../teb_local_planner/g2o_types/vertex_pose.h | 229 --- .../g2o_types/vertex_timediff.h | 145 -- .../include/teb_local_planner/graph_search.h | 215 --- .../include/teb_local_planner/h_signature.h | 432 ------ .../homotopy_class_planner.h | 591 -------- .../homotopy_class_planner.hpp | 95 -- .../include/teb_local_planner/misc.h | 210 --- .../include/teb_local_planner/obstacles.h | 1112 -------------- .../teb_local_planner/optimal_planner.h | 736 --------- .../teb_local_planner/planner_interface.h | 218 --- .../include/teb_local_planner/pose_se2.h | 433 ------ .../teb_local_planner/recovery_behaviors.h | 134 -- .../teb_local_planner/robot_footprint_model.h | 684 --------- .../include/teb_local_planner/teb_config.h | 432 ------ .../teb_local_planner/teb_local_planner_ros.h | 426 ------ .../teb_local_planner/timed_elastic_band.h | 636 -------- .../teb_local_planner/timed_elastic_band.hpp | 191 --- .../include/teb_local_planner/visualization.h | 272 ---- .../teb_local_planner/visualization.hpp | 227 --- .../teb_local_planner/package.xml | 53 - .../teb_local_planner/params/teb_params.yaml | 109 -- .../scripts/cmd_vel_to_ackermann_drive.py | 60 - .../scripts/export_to_mat.py | 112 -- .../scripts/export_to_svg.py | 244 --- .../scripts/publish_dynamic_obstacle.py | 67 - .../scripts/publish_test_obstacles.py | 76 - .../scripts/publish_viapoints.py | 46 - .../scripts/visualize_velocity_profile.py | 76 - .../teb_local_planner/src/graph_search.cpp | 346 ----- .../src/homotopy_class_planner.cpp | 854 ----------- .../teb_local_planner/src/obstacles.cpp | 216 --- .../teb_local_planner/src/optimal_planner.cpp | 1326 ----------------- .../src/recovery_behaviors.cpp | 118 -- .../teb_local_planner/src/teb_config.cpp | 886 ----------- .../src/teb_local_planner_ros.cpp | 1122 -------------- .../teb_local_planner/src/test_optim_node.cpp | 322 ---- .../src/timed_elastic_band.cpp | 630 -------- .../teb_local_planner/src/visualization.cpp | 563 ------- .../teb_local_planner_plugin.xml | 9 - .../test/homotopy_class_planner_test.cpp | 82 - .../teb_local_planner/test/teb_basics.cpp | 73 - .../teb_local_planner/teb_msgs/CMakeLists.txt | 28 - .../teb_msgs/msg/FeedbackMsg.msg | 15 - .../teb_msgs/msg/TrajectoryMsg.msg | 6 - .../teb_msgs/msg/TrajectoryPointMsg.msg | 21 - .../teb_local_planner/teb_msgs/package.xml | 32 - 149 files changed, 760 insertions(+), 28723 deletions(-) create mode 100644 lankuang.pcd create mode 100644 output.pcd create mode 100644 src/rc_lidar/caijian.py create mode 100644 src/rc_lidar/circlr.py create mode 100644 src/rc_lidar/find.py create mode 100644 src/rc_lidar/juxing.py create mode 100644 src/rc_lidar/pcd2pgm.py create mode 100644 src/rc_lidar/save_pcd.py create mode 100644 src/rc_lidar/simple_icp.py create mode 100644 src/rc_lidar/xiamian.py create mode 100644 src/rc_lidar/zhaoban.py create mode 100644 src/rc_lidar/zhaoyuan.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/.gitlab-ci.yml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/.travis.yml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/README.md delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CHANGELOG.rst delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CMakeLists.txt delete mode 100755 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg delete mode 100755 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg delete mode 100755 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg delete mode 100755 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg delete mode 100755 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/costmap_converter_plugins.xml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_converter_interface.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_convex_hull.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_ransac.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons_concave.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/misc.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/package.xml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_converter_node.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/background_subtractor.cpp delete mode 100755 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/blob_detector.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_convex_hull.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_ransac.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons_concave.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.test delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CHANGELOG.rst delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CMakeLists.txt delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleArrayMsg.msg delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleMsg.msg delete mode 100644 src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/package.xml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/CMakeLists.txt delete mode 100644 src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/README.md delete mode 100644 src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/launch/fake_vel_transform.launch.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/package.xml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/CMakeLists.txt delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/bringup_rm_navigation.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/localization_amcl_launch.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/map_server_launch.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/navigation_launch.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/rviz_launch.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/package.xml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/mapper_params_online_async.yaml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/nav2_params.yaml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/rm_navigation/rviz/nav2.rviz delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitignore delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitlab-ci.yml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.travis.yml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/README.md delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CHANGELOG.rst delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CMakeLists.txt delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/LICENSE delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cfg/rviz_test_optim.rviz delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindG2O.cmake delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/distance_calculations.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/equivalence_relations.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/graph_search.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/h_signature.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/misc.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/obstacles.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/optimal_planner.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/planner_interface.h delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/pose_se2.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/recovery_behaviors.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/robot_footprint_model.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/teb_config.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.h delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.hpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/package.xml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/params/teb_params.yaml delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_mat.py delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_svg.py delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_dynamic_obstacle.py delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_test_obstacles.py delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_viapoints.py delete mode 100755 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/visualize_velocity_profile.py delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/graph_search.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/homotopy_class_planner.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/obstacles.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/optimal_planner.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/recovery_behaviors.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_config.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/test_optim_node.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/visualization.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/teb_local_planner_plugin.xml delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/homotopy_class_planner_test.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/teb_basics.cpp delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/CMakeLists.txt delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/FeedbackMsg.msg delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryMsg.msg delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryPointMsg.msg delete mode 100644 src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/package.xml diff --git a/lankuang.pcd b/lankuang.pcd new file mode 100644 index 0000000000000000000000000000000000000000..37d8f86de271e368e3a5e644399c24278165c238 GIT binary patch literal 67296 zcmZ6!2b@)BcI8{9F-_ALV~iz~5g8$bN~nZNRrg+zqcWy3#+b$!W1e!(GL31P#xzYc zQY+^?48vfC;i0N~uX4`PV2m-wn87^docq1?-^A++&K%!&Lj3*HZe!2* z)6=K-ZH&JscKgZXanmRD9ogJ;e78yCCr_NDJ>6zZ9{-aw&zP*Ej?cfxjXHi*iN(o5|Zl}@N%w&uCmzi{=++GT4d)n~8jbt=z$*_vO(^KQMbPi!~M z`DN{@eG}^UUpO$fzdUDmb;72W`iuAceryllu%r6w7cKRh&+iu77nOclz5CF?^^f1k z=Usf`lhu32530ZX(zkKFSHC!?dPwI%^&1xD^N-kZzn*VkebTP4V}HxeGpd`999Vzz zw0!?#<~~qecF^Q{LqpyV+q35PuW$eAcX2$=`{ZXs>X$71D7N*zs^2~`x&CzDJidA3 zFRDxG{fg`B`P*jn7_RsCMI7H__=fgQ!v@#8EdDIE@4v8A<@;@W<=}XpwTI7&-=DYl zoBh{2zBGOJ`1+0kJ!1dD)$g@6c0IN@|AV(Z)&BmqgX@i5^Lh8Y*01{Y_5JEg>iK+U zt~|K(_LYyZ{c8`;?IUIls5H%)Quq7ZaL0%W zzkmFNL*jfIFO`c*Q|ga?l*jY^cMO0-_@R-I#I{x_BFlFuQfDGs(<@P zZr^&#pVhX#GO^yg@#Hvu!Ga03)8|d9Z@n+K2Xo1;EJdfr`8GCU0ygXjxv5dX&^V~G{#WK(9`fTXyF2>HAl1 ze`-=)b2#qr+eaEJ+rOGnU-{dUVtZcCU&Q-tXy_Z;ooD=k@$D&Od3_ma4vKaX+1gEo;Yq_pAyBhoN{NSN>&Oc{M-v6?L{;b6Bz5mni-fBC2-jKrnU{8lF8&tpZgS01%Pwds; z#=IWm?dJ23sjq%7=kx9dZfkw;wqrG4|KGU(UQ6mRKUcq(_TYkEKWN?l)p3PA+562S zrT*I|*L%+Vqqsl$j|%hu&JT`??W2|stE`(iS@tFG|C-*%RW4aLS>N|hV*k+Dt7?4z zyAS;!uD4^r&ug6T`ODK^-FnMawMo0C6#nb=ek*GH-u~^whZfXc?>ANVpZ0p`JBw>e z>Qjo}5A3qKHh0gYVtZ!u^xCUmOfCH1+&vf8c0Vvx<0<{YnJbso)*e1Z_m|s`e)OB_ z!qrXn;TzIFy?y21R=2$}O7r8@xZlG@+)>^Az{tX1;m=;Yr>Va4j68n(SJzk1Tscbi z^rbld?JFOVz3%XTOX@GQuRZ*P`m|Bc$A0X)Y)*aF{1;<;N&T1MA9oB$fBWqtAGMDh zIkG-%S#IO6@UOksrN3qV4V^tv_U`xM{r2C!ymsdY6Se<`vAyElW%2%=zdZdz&zWCU z`Ck6sLubFL`7)ut`+=N4BS(Hx#UIRU?i|10b@hL#K7Tpi|6k(%uB-gd>a`F<$bKbWIdAV{)$L!MR6lcNj!)Uw>X+w? ztiRqbuirH1lmAbhrt0tBI$riD$II6H)>q#=?U>^G z$^L4-jFUah`J#7MmC2O7%=vl5tohZ8O2^c%tL%yMU9xa>b;BKlWpDF&c0X`fd&|y= z8b9gJvHv4Sw$w-L_%x2cVNq*!eoIB)zaica_lLcB`^tT>zwOdLF75qha(!Tz{2rX| z`O8nz{LK5^aK{JjtKVxC9!dK&X6~L=j^A~4zHiO{_NDJM*KfTg@1NtpePn2He$DUF z*Vj)GeoDOa;BDht-@mpZJec>h`N*a1i*IDSd>;2RefI+;zW0iA^ZRt$df#J=XWQ5Q z+pjA17(PYzIq}kz?)_pufA~ zd)b%T*_$WS_kNT9Qg*P0e_D1>Ztwl(N44JTI{ZE38GLMij6VYpXb#5tuwU5Iho8y& zd-S6dDuWv*>HXyPoGnAcetkaV#5lg^%;hyrLOiC-7_2oKCR~-hcA5<2Anj zBgV_U<3F!*{N8VJ{0?sXMRjKLvGvX~{x$Z`UiC!a?bGMweE#yBAB6opeO`{gZy&j+ zx@GdX`e7q-d_Vcw!@|?!3;cQeQ~y@Q-WxCLeaU~1m3>Zs^x$oOUw!zQv9gaj{^f7N zo(@~~f8zSjUtVp${i)*>PwbBE4ecT({^2_Zz&ls=g&Fjr?`O7Nbetuj`_p(0 zd*OIu*VVnlo_hXmd*uq*>q+&+H+~Y=)BJ9I^Ry|#m+5b1e`FuZ`abW({=MG}tq$KX zUiLKoqx^r^lld)qyyp93i*KA-;0xXdWM$8@qQn?E&auuEz1>Oo>c$zyNsu= z+V`OR)d}*S8Sfr3>p$d=rpW*1_wTm#2Q~2cqaWq-9lY&yji;09hjdQ=&-qH-MiqGD z{cCI0rSE_j(?4=PzQ4!o+&yPjdo4Lx`017S-uPqeVe`iHcLTdTF8_?Z$@ioA5%_2K z1Bpjg{`UO93-)iqi!r~K)bo0`Kh;<9>1e&*opJtEPh3{r^a=Al@tN>s`<~bE7kNDX z82k3}OSzx;7yhApr+j|RQ_Y77#e7iwRGKw^s^andy<%I%*OP>&^Lrh&^qo@oPE(8L z*ZEq(&;FjmS7q>B|Ly6YZ&=h;V!!d;#^*MbH71JvvseABobx-cC+uJ5dOOd^>nR>C zPuMh7{xHuc|60cWUR28Cg%`hfd_zM+1-v?EOCCRG%kyRMtiRWpD}NjDnD1xhZ;z{N znLMpHUytFvV!qsZOa5NGPMP>y&mHeu@kX4_e9wp-11jwI_j&uuAH{syGCALOrRiVG z#N$3*Shd3T!_VaT&f9xf%*Vq<Ku0_%E=KsWzusr^9kQS8@18ZJz`Kk$ zaS#O_()ev)wK;Jv& z)3i}}{F*+0AO2==W5(}IbGC$k+BYcoi@#|DudKf~_v?KGe{tsr8UIh%^hpW(>iwGA z!8e#+5WWn4!Tf>l=X=|oXS`B|AMt+K=`!}kd_rT_?iKdSCdK=C{_-RBT_+XKGk4GN zl>?uz?b4C4Uoli1KWy30Ym9G=)7XX|<$k-aJv_GI8{k96t;lV~S+ajaG`@3uT(;() z#0MNB`;g;Hzn6a>SQqYz@wM%h{}snO-hT44|E!+3cUW=!uw^$@ud58Kuj!NaUHpv3 z-#EV|=MUo<{=)uJ{9TJMY7sAh z?}yGFEPs;MUp{(nRdc8CAF?Nb_je39DBg$seei)tEzReLPdIbs;DSFEUMs=Rbno=X zas15Yjcu#nJ68BIQ}+OU$QXy1kLTr(d+St8Bfe_xvQ$!p?*QHa z#pjNym%3&A`{F(06~B$DKRY$Ii8ruktKZ9bYV~{n5cvGf(=xvK`ubl+JUnW7Uhmnd z%YrZHG%V-yp4U3nux~wvcZ&OOXlMxkK6G~C>)W5YySjYz@xpV)pU;Tz8CBRP#ibfg zCEai0?cVE_Rl99%DewyOcjpHl7B4~ z`1*HmZC1QKw7zdp;#0+))p09Y6~E{B03R>Dv8BMb>n8rRefWl^!rtrogcnX0f1KlA z_OHEq*C~o$5)Xrqm%cN!zze;WoLW75^Ps|iiSI6fUmCmS`v%XRz4@f#dcx18>RsU7 ze1D8b*`@>SZN#7cJ{s?BkH64Z%ooMar8RxV$v$+A@6obzLP-raQ>Oi zIo_6!?prx(=~T^^KZxUrx3I@M&&X};Gx4+ea`+$k62&2L{MnnYuj!rX`)0f;`&%n@ zo2c)X_PJ@!tcbtN*9xoEuxCAIrhOf@?4-b-nkR8S&5z)_HXm7vZGE2__WsS&@_fWY z@He*i+rO$-iC^dL$=|~_5wF^w6Hj38Y_G)!2cKwum-j*ZDVr46Bc5k|-#b3}Tj8&; zugby1{@=aTC+4HRS!|0NQO*VaEVl=CIXd_N?`Qt}8za^Ih{T@P_#j;%ofR{TF7uGJDl;BEELM=*iC#f4uxsj^FpMT^07?!P|0s z!GiQR!ufIi>nfjB!Eamd%lJk-o#M}O!FORlZa#mC#&7Qb@|^p`@0N;qWclcStAZ~a zugIPYUyl~P&-n+w*>!bmVGqRri4UJz;3514@t*m*SHD;z{&9SfKbo-VA9egl`Hzg( zI}Q6`mGNx8=gV{cO8nb|f)9KAg?j?8ciWor)!M`VxeA|SyaoQ5(W6wvPnwU4XC}!0 zrhi!d-nGI1nIAi1*8LifC)dTpg+Eoib_(&+(|z-O$R4!cdP{S?)3A)c;pcfC=hyJZ zjCaRtJI`1d_QCH{_Nk5cYkxj>&ui_(=gUWDyr=mX@56YO_?P+a_a*xy{(Fk@g^4G{ z6SjZ)9eiQpYvn(}f7*V&{L-{`;zRpCq~^>-}>7`_~>-898!l z{nanh|A_yJc;DCK_nhyhv3WfF6!GAsU7ccl&u=-Z0{>HK`dxg#g{%J{@^g-7HU7)& zcf5A`yyL^(nEy~b81~utWaV#r#P7{-z^@U189!m4zypp~6yJvbHs8;D#6H>|&2PCO z;&aDKrEbe2-)+2xzu$1j#KM1XnS5^WQ~rLJt+_Jnf&HEGDK+9p$NMkd^Vi`I7PY1S zRBp8fK5=~i=tpM<-yobE@8{_CC)C#TnO5wVe+>T8{P)7u!^1xe?2`Uj^C#@n?N8-+ zoi^%6vF-R9zWs<818JUXdvK7{#B{=K2$wYdI8r5D79cjP;W$Cay7y!}G# zCx5|wd+@fmVw?GS@AwHtJT1J}KBfB!n(t4?e)7W~K6H}s#Otv=YWd=bM?Ajf^}M;d z=d}~$|NlAmiw{(MF-rFDjo5}CPkuDxVeksy&+kL_NcQBkV*HU0z4`nx1s^Z2I^vzB z@8tVpKHjisS~1@Ae%s-5{5>?E#qW(R_yqCk!V4Y8ldop}dAyLX#=adfE5EPqw|pSK zpm?doe0IFA_^1WEVSB**C7$c1 zzpy>*lgE?nYZ-oK*VXwxUc6^u_}`BE{c*(ie!e$P`}g1yieje{I~hY z#W((-!t+@V0e--K8n2393p{o~uY7*-Z^0M(dB{iOFRh0lp2q)gJ~Hh$d;)lA*8GeI z!Sne(`h`7f?3(<#c!1bmcF^+h@AG;lK9T*__(uPc{IlYn@bB-Q)hCVz@AEvT&&%zu zYj2l6reBed7FG+s!TJsIUl*17$)1@19#T#IHGe;Q)tLCb`El`S?Zj_eCg=4BH+B`? z?^ooD+b&(x1|PX@;vsQA_%rxf=dV9}Xwd%rO!5ifUHr?)k^SR%J$Kminau-Yn|KU+ z>ur6H$ge0yiT$$wwMx?j&F{pgooDn2ymUdY+%Mi%`M{3+%ORcrcktcHS;g;1?C4x0 zpV+-q9uNNxKW}_X{(;|H{~(-RWBcpt^Llgl{91aV33}cz7xFb--B=X{k)zh$NS>@a=nX6`TX!v;2+;F z{4e(3`V85>(lxy&6#5p;k9PDT=2O6Xd|&HR$S<JA9A*!KmerS8p0Stl)=;r=OiVSa>e&Bm5@% zHS_E8M{TcuF--P8`Az&edJy|h_&xAJ^{&*@DV}e;t}?v9|D|qQl}{T|T%Y_F`D%T) zc%Sl*?W>*`Uf?zG@~GuQ3%m?p3jbw$Ci^G8ps_d}z7+m^+=`6P@uwPVg}s9xV7#uI znE7S$t>mlCzmlJ2e#x)J{p>trc-T9~_jezf_|^OZ`A^2{_OH6c@#`=CMUC^_J3jfU zNxOa-{GjzS=ojGo9Ur1kIAT_7!9VEzC|}xK=wCEns~XFN{~}(~953*M@J#TpGkPT7 zpuCCf-*H8}2cO$}U8NWg(!T_Mv+=piZz{hKx3mbU2qXFhFS z&zZqbI^Tl+2!A|mS-xM{gSLTPnhU)keDl6RL-l<#-vD38_x<*f{J!L0_00;rA^tSR ztMRb(Os(QH3Vy$N<8vkO!>8Zn_z~V~?K!it&?64ta7T&ncjpIx7~fl5Yv3Or4<7`+ zIe1%se`#A|Jb9b(27Y<>tkiQ6zkt8iO-z0ef6DmlwIuh$7vN8%fKYR0$;uk0C`=@{7doy3mSHZtwPwhYCA4*)``!zmG@Jseb z($@waRm>3AC!WRL*uTx_@!ukUuwX&%m%WJX_piVqGk%Z10N-^Qmiy=K zc`fkj{FdAn{t7*#c=R~_;BCiM$p4!E!oPqA?N5Zm%kWu4XXo+a*S`0A_$KmCe%_;& z{y6j>%8SMM!Mog_G&8Y%*oY%4_;2%XZ=Tkv!uzpal=~sS>F1XoAo6SW&*aBB-q*vQ z@xH}t#rff^zlOgzo+jRezdw6(@=?0q(EFM1La(v;$nizK9DIcyT;nX_$yxKik^h{m_%QMQ zvV#r@JNA@!Cr}b>Y z|G{TjkFD|Et{6e~E$t_K7W!_-Z{$N~^cXMxE%`I;FFpDZ@nz;tSz{j{Lg#3Hh&+#D~8b-vd1|_Ril!c(xUL;C#ODR6F`g z$Dh~(?61GK{8Q*xeSCxdENYvk@11;<@OJP^zJKCt;u-TZipR>t*K;^NCO?5aysnb_@4xUb zLT_z;RNP_cQJkNb9gXA759s~Xcz@PIY`w2%?6;mp^EBp{x99HZ8T_^Le`jyLQ~A;p zgdd)d?++frpZopsd+>nyv|da8PWkCk!Vj;;@#9vkt=_O`oc8DZnb-55;(F%SghxYf z=J&yT`S^|T(zE6GCjTLhQGD&&aX#=l*SCII@lAWvoD&MZi}}L*@3J`Y(ZSn(74epM z^!UB_Amv|M3;e9%C%ipY_9yjk(q~DJOFWhK13fVOx#G_Fz2c*G-lzFx=F5f!HubJ()n6B-t$*@OZwj_#rPP$;fT22#^;j%7d|NOy83(kRpe(qzJ;G3WByrB zCjE)-r@7GY$$ph@8rz|VQG6Kl$9$IfxWH59o6rML4`jS3{aDOb^INLtiTUI4E&Nu- z-kUE{d{83aX8R?)7J4xARm%>#CFY~;rFfw@pZPoK*W&)ne|GQmJ+QqO zehIy)@gn$)`E9+Ea{Cc)nlF;hBJzL!9^}6gpXB%9c;Z#-L(og(Z*-kgsXyBNz>2_U zi`tSuA%6wlkPa#KkD0qPwlCV6^IuW-`Ky%KTsb^F!2gfTxY;q??TR&UhGphU*!hm%9Bn{HyUP{u4gM zdKmb4{H5`u{AW9O-24puJLAjvQ}NDY@H5^|dmne#h8 zO*}?^)_7g^GWZ|m2x7d*KbE*()%3--;?pvC!F(P0E%2q^2l*-T!N%9v5AM&;Cq6ay zTmQ57n|)=j@B3S@pi2dN>-RaYXP*lElAjm<%=LYL<5nCS{L!wfGr#Tl|K9PLKL$UO z&zj$I@c#Lb{?qyn@!jE%w|{j|>=%y`@wxfe-4ARRemzBc*o<$5Tjg&W6+foGCVtjj z)AOcYXVnw`GvasW>(GD9UUfpj7ZE>ej1~BR{0{mF+hgf6t84n4sP~=cSKd_quStAF z^2gNkh$Acb2CjGJ$`J*>M12T+fc1RnJ&5lwTa)@u>3OTtJZQY6Kb8MzMeptU1N>M2 z?L!Ox;PWA~t55eGB7d9ylllSTgUi-r{L6Tx9>ewte?okGNau{NHJ;lv*9!hx&)2@F zt+mj5%O6JmbkA$4-++(O+%D=Jgja$uxckr_#`h8bQK24a%-qxqkv{;>cU{{%_LDy( zA9?z`yg$_{h8|?`jj10ZpMn4FvN-Y6hC5b;{abcW@)1WZT@m`jZLcJMKzt9rD0R#6 zvwUZ7seR z{z&gc`GLfzX^Dokwh5xaCl3uIK_w)OQ-vXbQPn*)cQw4wQ^Qpcr&fjr=AII^|r!1*= z3;xOejrb7%E8SB(&%NV+9(>E|{gUswVbMPX9+Dr7{qje_?--Aw$LYWQ=t93Cd{^E3 z&C$jDCtrR+ul@yJ1O902+EIV2@zRF9T(ZA@6MTXFzo>L*Tn~E+K30w+w&Bm1KgP%C z&#<4{ze@b9`wINsbLJs&JoWwP-(1hqW%1G)`C!)%QNPIdwtZ{axhnAY)BE*9=zoca zht5uWyJd3fJIz0JT|2wR{9dpix^2S_U_d$-i&BvH_;{3Px<;YK&4mkQ@ayo_E%*id?iih4-$0ghjVhr%9tzwmtMGkksV?ZJPWPZa+j`VjwK`LDps z<`d-K%J6lr{}7)M@vrkS)LX%qnIA+CLVV}=R{6+?hpkrw@37zTB=(POj|btG{r)fY zB=|1cj${LJ}X-*woCm0_=3 zpGamggh*9sg&%rG6Jt5ANef?3f?>t=|#et(~`bYB7HI4LU30aq}DK zv*1(AZ>iQU_%hd5$xhW)zt^!}@p#O4zi-ufgncmIfL?%ng6G$oK2Jsd(Dgv@!|)5% z$B>UfkKJ?Twiw^&d+{&Uw~;U8d%J#x_>lSP`9OXeeWLkM_$%tkRd*NXBcILqe*4Pg z=ViaeuQe+__rGI5=fgkx`@G)oHr02HFTVf6)&H$JuuG%zbGzbr<`?tZ^=RUczNe>D zeO<(}=4a8zFdnS`lRap|-g-l5b{#W)k@PRZ6aXj%k?@uvoY)dP#AD<_Fpk5k$F?Uby zC;mduV?O7K>J1UEIiD&1HSoFd^6m%zJn(|`K*N?@9{0QZfz%&~i-_Zm->@I>L0_Je z`+E#OJM5YH4fQUF_hbFh>i1^X&@Y*vLeB~QGoBO|8s|6PrTG@;lk6~_S9!z0FUEVT zp7_%me1@Oz@fSvak3XZnAO6DmPtBJy+s5nUJFp+d!{~Fcm&Vhx=06tvit(-ZuQGVR zc)UUNh}^&97w|Ur%K6!EANixmhd7_C+PT2f=9^yqVq^tA-1;8ACw#DaIK=Z(-wPhx zGPxsv$$Y$NY}OZ{#{)0RPQ~%$6NwLI&ChN0593yxsQ5Mc5b}ZaKd@dAzDBuV;qQ!p zsh4VK7*e-BntIks({Y8K61_WmnPmqh9-)2}exn1Q{8Qw&+#hM*p#LHK)>zm_^?{Im zWPE1)OTJ-Xm!XBfkw4M>3|4+1`4#v>jb-JtGvA>;9l~3u6!|&vaq&E^7tMYiLlh5Z z{y}_8=s#?4guhSey0$}K179$FL#dd5=;P71bXlBw6XIo!^@86~Jl9UX!}a#u?~C`8 ziu*xNi2hu>dwdVswKn{R`2hHXkKgFflgMsF{J!m#GSgaQV+)dCG}3RU-3!cdFLM<{pe?b2aHEXj=UuF z+s3o9_tEdg`4-tbjfazzpUwI){Ad5|jT-OIgnfpeL%-Z*@zb#_e5U+rQ_-IVe97;7 z&dmIm@Mt^vun{}5o}2zOjNg`>$v8Sj33dn?z(zFq2F3J@t(+cJARwld}14XsQnN5UglE=p1MBtDNlaZ zC!SaSJNUk{Hzz-cKR;~5@Z$I24fs&!zhp9IDe1-1)puWBl$q= zANbn(H1kc(8}oU|H<5qw^OJ7^KhNEh`(?LlAMabe&Wip)^3THS{R%!0bi>yI-07S*x%_@qVb!z#csOOyULYi+Fa@u6}X<=tJ2* zeRtYh>P4Ba?zgsl^si$+IsR4tVt@WN`S92KT@(2u>osh@o$p?8?%%}m=DXiKE%hDd zhtNxb$6PP9>+0!oyz$GvK^Mig@jm)gj`#21JZ((Gr>_5&o;~oniW7laBkK13)+48sHPtEU8uL-_%J_|h}`ULAmq-zg7i2d)jSAG(F zn$JJ4=an_;TYP`YC&jk;Da94hug�N6Z>mga5F<*7vVwy>9lK`1HHpHN_M~ysq&O z^^dOKLJuPDRr$+|Z>cXrKlkognJ=N9Q9T5NXI>0{jz44mI{$@U7W_DB`RlP?@tE{0 zCy1|jF1Cexq=&31f3+jF(Q6A!i?7f6Ci;becP8!1`YQReh?lJgL4QyGts54-6u-wF zpobAIif#CO^_G&JC-DaS`j_WSE%HO?GwB!e_4Rpv^k~$(pSkkc_&xj<{>bsM{8aEo zj(1=0_vcaH=l#OR5ubX$>M_drBj0bA#cv0n@BQL!%HUmpAL{p*KmPv9rKe@Sdpt`o zRDnM+o<(0v{@L?`c$9i9pW;e{t$Bgl}W}s(s1VzI|otqZEh7e4gL3D*WLU z)xW{>yXgnTY=#}_A@7sEJ+1uc6%_nYCz4+JHk166I@a+wYj@9@3*BDO?4fW^` zU_3{@mi}Aji^P?xf5F(|{n6iW(ylQDUj_g3>36NV|I{B+PmumNzpwNHI{(-LUs6v< zy|nR|@_g;YkLFAGJ@J?IlRmaWsAq;hGCxQ@lJi*)3;uymThx~E3;ZhY*ZuzPzwn6QXO3Q<=fl6Chv?qv2QglW zhtU(Bw>P&peR4#^!`AP?hcI4Tua2IG@9q9KKb^q%Tj65HZSE8rjNjp1X! zZ?Z|TpZz( zx$MZ_n@@lr123%nZSqU-RjF4^{D12$%fsJ&_)y}Xt@r)9#{6n%NPPhMX8110%jkjO z+gyJl-c|2gM{1x`m{rA9s)PGq2A-!qDYikeB<5m9{{?vLB)emdF zAEWo5`40RQ^+t}r)UP+}zvr*wm)LK7$ovzxCH+&LpLkZd&d3cM?Rqg`XR@E_o1`W=`rN1wv? zv%UkqS+RideDXc$Q;6q0zxBRCZ*6{*dKTg}zhA~5+p5Ee@jgcVtjK4`ug5m^mhff$ zwFzd67>vK2yC;4gSn}p0hWXLw{oZ zGW;X{-}*`PaQIjA*YH=}JB=*rxye5)YCEy$k3;{|Zd=jsBp#uEE%?IyXeVsCM0o0C z#j9CgPX8aq_oBAsJMahO+vNAcf54|x55D)CoLkj&b{V%cajz>OzV`ap1?nfhYf5 zpAVPq6kZ)%oDaTE9DI?FQG6Es#$h9p&(QfJ-fG#I_d`CG{`b}|QBMWlyP}%=$)AF+ z+`mcjSn!RmpIbNa=n8oLvNahG!gp~0uAe4818+KB#h=3WsSiTD?>(>mEc}W2iyp%- z4u05tKJgFywfPJ9cH&j*E99>uKkoV;_%HT*TYXPdk2bl`N7By&e``G`@AE)?rsCHU ze2DpT+27zBT)zu`!2g*)*ZXOOUooGcdXeBmtj83d4?e=%$`6G8!+bq@7UDniCDQ+d ze$w0G17q9z8uXIrEA4-&N5J3vd5C{`exDz{o#&D6Al^6q+sXe(BNE%x&w@9^QN%X< zBmU9fhx%#s%GPJ$|JZ*)ul#=K$*||{FO5ICVNnNPf&U{O|N8pWkB~2dFY)|iJ~3YH z|KPLW53DaFpNU?@{cBjy0le_-BWYjY%gJAMp3x=lU;P!8U+v%*$oIo%y51N4+|1@y z*`MTV!RO!!k5~9?@Wku=^7$HsC#nB=^R(25foI`k9Iwb;x07G+`#}%P^Sgh6=3fi` zVPKcMUyWCdpFxGc6F(IFCd?O-KNZF)&Zp;7z2x9RA0oX-_&fDYjpwD`D|p`Zzv$z* z-s3MMUrPK1o}IfVzd!jL{F(bDP>+VcJ9~4U5B(0?(?;d*vFF53j_>ykS{(M<{t7(+ z`Y-btEjup=|K<3B{_glo|9<#}zX<-mgI~C+hJQ960pG%WwLXUVPJM#oQTP|~3qJps z=Pap#-(6oozxMOFeI|CF$?3>Ag;T+6m%slMf*N z*W4Dro$){Zhj_#NQ7{SN z+z;0J6#0?xuk`n#U%LCfqo<>vz4?9hnJ-gs<$fXJg@O<8e)@lskGGyf<2w3jSf7HP zn*5CU0{SOnzpd9qZ-#!r@r>qQd>`v~>9@i0u8&n57yS_2|Cji1?>CK_?^(}B{ZHxv zGJmIe9{n~B8ELq&GUzT8|b0t?)kTPAN0HBe10DCbBrI=A;f%;9@+On zW%6;<_qhK%cpE%vK8pT1`NS^3=LU%=BGZ#{|TgZSUpf{zwn z2t2L#6z^N{v+_aB8o!Bur1y(_qWh1pe(&&z*PPE|eHZYd>|Fewd<*u${nxavNc4w! z{_?cvmn>|Idfsb#+rDSL_S1cn?}0zXe|H)-BF;xW&Vls};0g3=?jMg{7d@-%T{hft zM)3QNPnBDZ@oqgY`AYnw``givfcVt??clqp@4WMayuSOzt4Dd<5B3kdVf{q)u3tvJ zXUBkyr_oEHFHleCI37I*=a-F%ZSWKCtM|G*KY9uL|E;$Sjs4so?@vB7w(;-858bwA zK9~6mzIgbV+(uu?_;i0x_$>T|_2~4UkT#(hU&5s|@Y|{zv~M=j0oPd<`-w*5c)64rN^*E9KC)^|W}?tTa8@zIYh zeJ97W@*&D+mJ0rf^^xXleFn$t%rEucm;X<`3qE=2JEIHz8vHGMvi0oZQ&kV#v0jAm zUc{T`r-&D>+IO7#|0JI${~hajxSp2!U*@~>lkn5j^SZv4d_C{~kj^=tSr6p0HK!K+ z=-{i-zqp?e`FW0)ZaluP=8g3E;FYvDI~G?D^RbKCGQVOyuHJPVZ~doo0m1hgU#LG` z#G{U{uj$>RhX3>R7PW1ZJzQ4g&sd*FYdh%ur+$F-QkBB)!k{|L5(!Ch`YfZ*Bd>4=cVcOHYvF zk9v*!FKp8M$b5owfAM?A_p|9~I%HuaF?r<_l~o)TYpeIeh-C=a{NJlRUAbz-sNxQzXufaL0nk$OEJHLo}2aIY~P3<6ayE0 zI{6)GL5lfFe4^f6h5b_e@L1Qi#}?Oz4`aO=^KtYG<9wdq@YmRj-4A4bl<&1};?P3x zPyfxsMhq;K4) zvTZ$q@I?4G>mwA`1fSsey?JBT(CfMWRJb?ByY+zdZ)JR#Usyi+Zq5IW{#Wpk_<#Ri z&lkTp|Df*^{Zp*3({|f|_b0x!1m3V-k$gM+fuD!*z_#@X@M+)$>k(wXOYo^aUU7Ll z_v?6nf|^0Ymm0qaA4dOn_h+GhJN?vrf3g=*-{bmJ`Pc)#9D=vA%n5MLJcqy8Ss z*Ol=nuD@ZvlV5c{LiGV<@-5b5$nHnH;`gV%O~D7b-wXOL#-H_s@JHw|6!*kECX?Q28t2 zn~wG0$Zs)!KYS?hH+&ZQ3+v&*C(3mc@yQ{bXN3K6KQQz@=(*fKSA4)@#ADWHh|i1P zThBp#1M~asE0d27k1@`2Qgp!c#q9{v)4;(QqK3-x@) z2k143_qKnP{t-OO`(O1$+6VF3kzcbu3Vg!-TMr69MZ9mmK=oGYm)uzJ*~H`cKj-`5 zSK*_)ejD~0|LyhE(DTvHM13_Qo+SRp{@;2_^4H?u0?&E9Q}pHfr{DvqcjS8355o6| zYb)~k)UV*rAG|H|2k31U-?&WtbB;&#D-3^UyhwjW{Gam`ij`?w*Vv6<;dM5HG`*c|O74t((}W@sj)<^-l10)_)LR&>!=|hcf<s#W4$`olXg6h{+Ie8$NS_L(4Uwe<9Rrr`{m%zh)-`CoAXupxE;R5_CfJ= z^vCYKF0Y4uqF%xJA;nLzew+DC_&(x6=fj6BJ1Ew(^ZS#2wiUc&eWv_v)IU0&6`tAx zUbjD2J|+C6{khH;^+EQx>Ms=it6a~d{6%cL-i-csj6c^S;$N6A=FilxHtG?r&jQc0 z-~HamFR|_T89s`9i0!rVfPw#A4+kH`{B^v8zsCRgejk5fao{!AKMLPOJZ`>q>wOo7 z{?L3s`2qMS=NDw_!oQg>klhG=%lUBZ3w)6Kq0*lQzRY~6`T^F6CmnCopN9Lj{zrZ9 zLT~8bs~$4)71ndGego^hI-X~}j?afoEA(^dv*9oNJ>aiY8?W(|_yWF+{I>ai@G0-t z?;pNLW3tG{)4u_|)xJR)uM>Y_uZ(}h$0^=CQTmOC-NH)F-W=lmj{g}&VJHvK(r zSahQBL*^sEC+LYBf5Y#hAMm!uL+B-4Zzw$1CLdLt555X}=JTWf9(J7v~eGykA`c#L=Nr~ba%){cI2s!uE@ zeoQ=|`%w=k;*sPNwcc~YFFqgr;`koUf6$Md`!TzrjcDuZ2E^_}<&(x4>uSGsy4o zd+Q;HXNV8X55WJR&vSgudc@!v*_60{^c~!<^?|%k^o+)D&6mhOmB1s`@8N&& ze?DINnyBCK`Q@+5;798X#D~SUk5_yc{XVRJP~BdvKV&@v>jyGE%_pG0;(5G{z6v~~ zH8J9O7|-0V>nF(nFy9py#QyG`GJcXyGq&OLSpUuXF7&t39u<0;iq?aNzboxu&qnxO z@p(r)&+jkjHB9sGyTDtTTM@6j{@L@z+lqIU{~aPck^BJsG5vPD{tDv*K4N~$599vO ztD#@A9*_Az{h;v^{anx!nO`R!ozY`p;h!1rjQ^Ft&G`WSsWc6eKgs@~`o1N;x8lyY zp72g9e4_Obtnx&M{m6)^##N$%;zqtaprm zHhw_=0)OKCjr@oBm=lY58vUoZO~o_${pkllec!0%8P9_cSfAW{5%qWER~=8I-@5!bEpl6ac0-frcat=FS1_~ z_?G!$Jt}%Z^mX=6@QthAJF(y=;3v_0n|~&r<$JncJo*`NnbJFC|JijDceP7nEIgk6 zSL3l2`)j?9@Pqj5QN{C<&sug+M?W&gFZ{px4}EXxX_|F^-^6@GuZI2JHz?y>jo-F0 zbJ1tye(}kn@AUX0er7y6eujVI{^eIn;G zhT@;t@A|#o>;5M6Y>KJk{OD8QTWtTNjjs`Z*#4?cG~!kJ8}-c(|6+Zd-cR%sHebv7 z8^ojjJ^d%a-_BptZ;topc$|7%@VxCQ{Uo`*`Bw6O;AgMzW<9&+SlmDHE%9!tTjryw zk0bteJPjX<|5J@^98do~@Sr%F*p_}b^s;+iOa6iPPdqB0AN$?U(*F9I-oJ|X>HIT# zc=8X5Q{s59|LpPfbl+cx9>CkwHNF;uSP!Ee6sROQQzeF2>lXzYWpkj z1M!CYPhg+OH+sD``dje6#8brk7anX!zu@NsU%+n|KThfXGuh9Mc#!@V_zT-t&DU68 z$$Ayx&Gu3^^yHZjME^v859eozkC~tL$N0y?MvO1)75)=EZ@n4$E55(uC&lk2_Pd_! z!P~|~ylnif@f-1j;}i8Ci2crYQlEYxKcnv-@qqVJAIyCEd`RkJ(BFaQ-LFdb9r1wq zO86)EH1|KI9s+yj_s@Dx#0NexDiaB0#D9zVQJ4?9huE zPsqQAe(#cn`8}IAF07#+^7(gNeQEH~{+{%&AYODlGiv!ygAa5))ZK@k@xAz2wGH2k zkEM@^{pJhkuZ6vFetyi{-;a3T@jv)#^N~%$FX`V|Kb8KKuE%Ho@V-4hI37Kn@=)O) z@W-6r&!aw(v3{EMbnv~x&cffR@1wpDd+Pq>)X)F!ty7EmR`tX2d*!|2{Pe@Y9y)$S z@26Ijg}-yY(EPq)w7}=CFNIGAUwgch4@~|)`-#cFM*kasfASCLJv@G;!zyEMoNuT8 zJ;!IpzZ(1P;6K+NNY56>J6;ukR$_em{2C9T5C3+5zih=PW$H71yzp0<^Lc)VKZxtO z-=pwL89v(auk;HspFCbve_qC(TCbtJd(;Q|{&!vdR@5i@eTX0Y-u#6B0Jp_Sg5TesM8heZRVoIG^Kf#dBrspT8gW z0KDk)t1c+E&1b2fY#F_~>p`@3UxoX(eUkno^f!Lr8ZTi#%&(!><$am2!d{R+wEhJ= z0{`UxeDKrQZ}o%-`%L~6JRnRT+tm9V82>Qd(KEQ8*T&~IML!7Vix|)FUCx(+&*^7v z`%8RJeYo||;C1TV9Uq`?VZ5w3H}i+W(y`xsg5rtDPg=jA-)a4s{)Io&{>K=f)@Kus z@II`UR=%%1xbaNIQyJg$J=yo4TzV!!)6=>4?9Pq=@m>|L4b`*~O| z6~4&(RS#R{eONyszPZeJcmHboO~aS?`$(@*W`6kh!Y^g+*ZqyK2Yheq8)PTKp1I$J z@*6S#+|Qc&3V!ePXISq6e`0+F^_<}8QOgtWQt!lm*FS-enU8Otmhmw89^S9FwZ4qv z&1Ug4nIEG6^AWR-E#f=sby;uTdT9KSe5k%p#+URXXFW6X>F5*S8{7|oele=eEc(Yh z{=%;CpFSSF66f#eZ@66flRwZe&*2Gt4zDM?|o&1sY4Z_pG7kd41*3&uA-msnz_Uovnd4JLqhW*|2N%9@! zSJAtyJv{qaQ@@M7-SkQ7WvP#aAN%_HtRJDD8spRB!+I3guS-uS`*=d(&!z7VJ&XA% z^7-P{3Vi_jR`lD>_s}nEO`q0+-=RO@``3;r^dyY8&NGfHz7P4o=8dNo{Hx+^U9W@x zp+1v(b?fI@kB)j%>wW2WK>pqSPv1v+sgC~Re1B=Jh0ik{Cm*30u)w46Rl_$7EbxTJ zdkH-2^||%^RDaf7@Xge}5WjnUK7GG7`9!_{tWT9aj(+GJ^&r`Q#PL1#=3N#KSG<}0 z1pTd9FW31{Z3o}w_2O7>ocxmY)bImro9`n3MZUe$us;lc0e;8dyWW`i6#J!~?Xe%d zE91fT=IqULqCVR7)7SJqJ?w?~1l9URzTWY_@-vY?w|!S1qrm&N*ZLkc{GaVH{sjAN ze}aA#`(t}a{R{VJ{{er-{4)Q`_r?CX-?8jl^q01MZ_pp?x%Fi9Z^Zt4n|?lwclWc? z`Q@)CiT^n%-iOXt2EW^0k;UWPrNG|8pqS` zobyYg7Tb(Z;yYou*zPg>H*r1d%gN93z0H@xzi|K7w@_a}JnC)uRPImnG|s2~PQh2X z-vagj+|Sy>JH>wCyWm?K@4}zp@7?c+^@h*`Z2v0t`geY?S9;i~g+HWTaOWB0i~Q12 zOaE)EPh>ql>%nq=<{!y7!H1Y%g%4ppBFC$jEPP%0DCTeG`^961|F(UjJ_fvH{ge8a z2Y%WyAoY6aMX7i2dI|V%@QLxe{A0UZOkv-IkE-xBUO%4td&Zml|Dex;?{GgBT|fF4 zoA1RRQtx2BHU69U)Ak=dG*T0t@FY>Y0 zPe_jw@u%Z6;=u#?9qH{uUuu6Zy-dWL{=Mq`BL8gtxcI)<@9WDx#(GQsJ@b+I{p4q< zcVRyBynY_VopC+VA@sPTTxpUj&7=QZL}^Ix*P!Jk@BA$~3R zBlBP40&DP3#v7{d2>!_9h4rF&AJ(tPpGJMA`4#FbxIg#PpwLD>MUCHket}mX zerBZjlFa9{UHYct&yIcutbfV-a=eWmoBjmWcM_k% zK7jnki}y4Y`V8uo_I@*3?>qTa>;wE#x2<_T^iKz_uSY(G{*TT-iNBBgbN_Snkk~Vi z7x+`n0p;h?p6NTblfPNhC;4CeE%iR;8_;jD-kTf)avgs+k>BELdD)%P!~R@OZh`^gXEk3Amo zzkFZoI~4y1zvKQ=^jGKl9?$5*u?HUSviIS?J$~r7&Up3ls>_Psn}3p?JLbQ~EB)8G zKl4xM%fauyzU*Z5gEk%%zflIS`T5BQaetm4@Hf;ic>ZdB1V3cG4)wF}sUE+~f98wh z8~iKzBI{Y<8^8y~o7A(QhjD*Z^a;Fg&o}rS;&;{D$9yK=0iV0{ouRQ!ej7c%;*i+x zwl&wEIbv4ktI$WI-*El_e@c95yb9g}FG;r-zsG;zuN;rik5;uaML#C=80bChpNa3` zubSp$JWG7V`P?4@eI@q6d$%?p zdJy!5=6|S{B)+%4m3m6>n2(>?+&lCm=3i9L8hntShwlS_VSYsVq}X;p3H7ZBJ&O4n z`qguOfX&M>?i5*Z1?j{8IM{ z`A)xY`n@u~Rns5ioBR)Wclz$km!kjW`NdPme&v^H%%9)A)hD*mCvg0zOBPE=*PMIrMjf&#_*W zevI@lcK=4@^VHv_N&YYK4E-3t#9LLO&MpgX>Rp zKaZ*Qy3p^Vf25wk`8kb`&=XiMfZm1r2kVQ`6QK7sUxU48d&hu-;(hPX`Wg6B_uoJt zh~8nujLsYxcD)e! z<}+7PFO+%{^nvJ=oR1T}h<;%{p86%dBjK;a8^XI9zk`bUCB@@y@OAc&jHe#Mk16ni z_`CQ%?k6VRsSP~m=M(;sJsDE;2a$dy^1Vl|Pks))_jQ%wg+5sEOgr^k=2yihw^A?U z`=x#f|2}M4)-MTehHZ@(=ib z`v>qj{D<|AOxr)_DKke*6#ntw+>&Dr0~Bd;Aac z&HL34FVEFX$Iq*LY~VffSF(5Ud-GG$kA#2n_0%sW{E_{Q^5MY` ziPPQx{lAXm?f-~xu^)b4uYR#6@*BQi`cL3Ld_M8y(Lcj{9{J1z{h^*W&Tst?>n(yW zJlpVzBEe}TPNe{uRt^0mZo z?!N|}#eX=Ts#<^=_viW(^w{b-qIh<@-s9h?_gb&;7uIKhcMsItQ2)aEzm8W_w-b5@ z`}gzq9v1my>oLgBz|X#TPx9m7)9qiC<&V9Db*KApE#~KAO4AY z2lF+% z=0{|I%j8eYw@FVP`A+*c>V@FX-Tz7PW5mbSC&-;fylwp{_Kfj5VgGt|T8}R3A!rX?%%@6I>H`OM`Dy&#`U>@~hPK8phJF`4p7AT^fBc0; z#pl~%{7FwMd&m7`zo-WFr$diDqeu4Rgztvmb-fGvPxxz(Z~EDi&okZ!FMmD+ePPaT z>Zge(JziM9fZvS5x(BS z{IPzG`dr?R^~qVEJ6?M0G|Fr&43Y4gv{mvTJU zGe2tn>ELZyPeeU1^GiAQ@R#^E_zLx~k8R;8=~4R^d=d4Y^oz7TpdR$-^&NU3^li$S z>wRWEILB{?o(OvZUN#>@ejR_f@wq{9J>qNd=CEbCO+PO3J<_ble$}qW^LTrh)(fQ` z&g+4(z5{xgqn2iU5BhBQQ1c1&8v%c~9%oITaq+yyH{@$MpL{}GPxV^C54rwj=NYF( zJm>l#`pJVYjaS7R#eQE;<2$Zr{z$$t?$7!m)!)Q=sm62SYXVRB{)WyzBl?4zzd}FB z`!T<>e01N)KO3*B);{um*3%H*f(MN+=^sfvVf?B1Dz^Q6vX^D>rSYWtQ3w9@^;Dl+ zCZ6^6F6i}S8T{kxQE!KRb^eulRqVO*yZ9sKv-6>vU$T4f>4}H6K1Aeqjek3-KX~G? ztamcL^Lk{|uM$6unVaL6@xc7``r*{8a((v?BHs!>Gkn9J#P_9M6aQ~L0_(BTAKU#& z;p^$oY5fEJOU2a|{0969{Xy;TsRzYItj?SJ4)=)Yn6 zu6`AY$D0a$MuYqTe2Mc3@Fm>ejsY2;liyITxbSbn$1UJV=NA_&SfKfNOmV#Q8-Yij zotk_B`AF8sx?xeSA0r+u)-QMeW%>y*f4i3+=lGuVw*()!9|H9N;8Vxr z=y&kny_O_Dr}rK6_rr(M9_o3bKHksIcwoH`^KbOKqd%Vd)W!9lo%+8tUXBso{2=`I zt+#BGzJIvzPQE|%Q|M*v4;c@9Z=a9-)N`3H%lPIPeeeH_^MUV$xeI*{^)#$+V!jGI zy!E~zMZQ<_U-d@=3Oy=%eDLCfMHD^?tdZ{LF*5O)KUj z^|9#p*7WHezo$P0`IZ?ya=yZs(C=YQpR5-=r1Q^fRe6#A!TCvCK*}4Dy|8?xQzDMJ+guOB!L46M zS7j?YW}ekSnke1FZk*iZk6o6ql8@Za!{)E{@?okIez zIKPFyg8pdc%fW;6r)l1pdPDT};6dx5x88Sr#6KE`aXsu2`97~lC0-%)#O@zOKOerY z?JfFj?2WhSe~dkHee}qYYa(7T-iHqXuNhwn|3<#c`f0{H{?7AXb;l83xSz$DD}PdB zzPaBb>n~$pjnCyzYw+35SMGUjOxR1~OWBti{@D5%))VLVj_+8%j{7q{)%*@T;`o>N zi+I87TPUBdep#%y@@d#J`k$npHuLwaZvcLBe5Q5X;(X5MqF-lyaqE97P5)ZOzdL@A zer$g{_mg*HLm$=HHP?H<-}C&A@8~bi_wfC)z5~BEzF>X&1J{F)KZ^dp)+fk+w;lL? z^gXqn{FEYomEDPYoJv#jA>x~(e!}-pK85k`_aS@N_UU)i3jP9q{-V+e;$NPN@2B}2 ze1`c5y}yXZ{5_O^Eb)G=7tr_l-hS}xqP7mbjq-7^-``jMJMgLVUDAgIzvB3m{j4YM z_pAGjc+TTd{T0evCU^Anl6{YO#`SU1zf`c-j=%N)qaMfS!=CUxJRkWz^V$BN`V{m+ zj=$0SF<-nN{p7e69sM}m|JQmg)~kbW^7X;HY}O9Y8GNGc5BX~FiRYtq2C?7z zBk?%F|M~aIH`lPwj+a>vfZu!mN#_;(jO`iv9Q1h4?$0Npm&U)DPZDmA^IJbhzMc7D zehmAA9?|t{^pB$G7Ht=|&9Yv+0%FXaE>J8gfd*JRuJ5&92f&pkhs zpN;iE*7Qkx&HUu~&DYV-u*>3({>O^nBA#>qeeyH(^RvGdzK{E{zokBn>p9*L-fX3R zkMXGPNBXH!!RL_A$G)21QGC*hKXHF#;mDOMJ=o%-3@~e64iF;s2SB$&crD&zZ}EKe=p8;)ChCmj+&RKX390d=Klv=_igo zRS$_cAN&T}${WWv^-So|zC34GY{PdjUX`lF` z@dw6NviHGnSYOlF_1xe$yubhUKMTBU{7Qdj;_sbjl;V2AHi4hb4{Sd2uOolsdZ(Mu zKRfJ;`IOV=U06f^_IkhMQ&@lfz<6H4K%CEd9@QcSe(?IP=sU1K#<$cf;!m7klO87U zs<+Xf!DkuoiWe?}hmBXizP?-V3&yADIkDH~C(wt3f84*Fc$D#>p5^iUj2HNX$6v_r zK|I2?>cC<@_4CA^)w{ADWc7QA_qun=c!&P%?7yg#`!$c1FKH_3gXs@L{|@v0=%DQ*%s;Hk*yrX!$RPw&0uk&RS*k$gS*MCQw|WnJQZif@j1*6aTfk5V6c`%@V&U@x#o*5^^5h`)CK z0{Tnj{LcF8u4{9>@Erp(-bU}s^$v`0$^W_^A^MvKZ#%KjONdt%o;|*Zw}lfzAA07> zzQlaA^rNBIu>Gb#3iiwEU%^N6`x_SJ_hTsH`tT4?urH_h*r>jrt+iE9rW{Pc6PN z=O=s^-?Q_K)ce4nXTBi!WB=Lj^?6uN2Y++bzC0d#fxU7+9Qu>PkGh^@`&V;9?_qu6 zj2@>2|7*U2d=>KFItk{Wb9i-lo4DdI#Hk`qvTPTmOc>kMU%F1AdTr-^UA| zNB-9OKJjbc+eW`Z{N(-02en~e%@=Dtl;AsDAA`NdKD*vV{Y*;WN$VG+hYP;Je2V({ z2cK;Ifc0xR-u$ofufbQieiVHI@wNG9;rrm%)l)gXKmF0jN4g#ceHDDaG)%D{evsb_ z7sR&4U+@R+@1gsP_1laG(ckjE#RJ9hijNh~et$h~*4yshDf1mVuk2+<{)2jI@PhRP z=;NrbHa^3y^OCRB{;1FJdTZjl+Tok64^w_E_+0CE z8}x^MaPwQTe+ls&^Uvd({4V%Nb2_dsJRA93_uodZ%kg_&OFc0C`MIC#D#^#OUJ&@z z@r>82w*9AG0efk^1K)>u$MuHdmxAAMJ%R9d@C(*ggP$0$wg>X%!5=ujU3}wT1%Kjr zm3j*JBHIi4vGV;~|1Ldp_+RJaPM>#X9B+Sn@V4{gd_F(@qZt3zU+CKgK5@OQxbAp< z=l_V8@bBh7SkIN~yFbI$`+COnS%0qg8}X9!f#55Scf7%R7(9>hvT$#<%@+FSncs}f#`2ylO@TlWG)(?emwZ9aPAilgK zevlq1j(5C9z8gKe>wEA=j34VMR1Y9LGC|`v{V(}N@S=Fa-~*)hmcJQa;Fnx~13Z@g zi~Ke98mgm>bo>D)t6Xh?m zAByl?89d?mO8S8B9WQ?x{DsGZbc0bZ!!+M~M0j ze-Gh}_`S!s_^ybz{Qaq)Vt#wRXq*K9+3 zU(yTd`siPBeu`gKW0~+8?72d%Ewbg??E4W#B!J7ukze z`mMX4miQa>k8dsPCHfiq@44S8dN0-=bbX%k>(SrJ{iV^%uwJ0~8So_hx%aakG50%Z zSEul|tcM2QU_L^zNW`1gyOZC>o|}JgzbV(-%HD_Gv1v}84}Z;g^ZC(x;J-c}lKAMl z%F^&pZI>p$fIboaO1wne5BLZ?X?%J1=Ig`%I9~%^0N*+vBmOVur`E`g-@{KZe_fwU zJ`evbI~@DDAL?rypHu$?Ut#{C=gg~ue>8sppUHT2|5fFm1J8Lsc$#?K_!52tJ*oMK zz2E$8)GJ$`Dn2Xt0@rgWj~nmH`UB-PEBIgQBdBNM{TVOQj~{%l7$=^Ge)4?ZXQz&c zZTjtjmlSiyHu)O-#}(C#&#+ItKgW-(uZaEedZnzN3_tSzwRyb!hw7#2Cz|+8dU?f9 zjfGwh`-1&+ew}`^^jEaLk@auHeH8IF>nRZL+g_kIl~zW0BlTwVqh$S5^I`bgS?X8m z^&}Jz#QF`^i@`r&pKM=LU(=4h!1hD)L3+uK_2O86h5mN-&%|T+W7|9CFa0DuAJMDP zFV6Y1%%_hi?jJl%eMIxdyr`fTBe*l&G;>`CZRtp}ujf$`&Q z^bPbUa(@cOANXtOJ3or?#Qeuzy!u7f&rrX}_;0&3x9P{q{PXt|{tEwOd#n4c;ZN<4 z=pP5(GakICw5W!@!uap9H8X3}f0_@VKQaAPOZ3XNLcHyH8;M6P_2Jak9Q|>Kpm~^jmfRbLYv;{5q%x~v+VDvujl)@o=AL#T1pSs^JhFtJ`p}}@c#8XiSKz|UXKU80OMuPYsp9I z_m9zk$nmN8^jJUK{Ey zpZXc%Pxt$RUrT+@A@Ti|OW%e*)%7jXsf7JEpR7E0-*sUyiGq8{EhVv!UL_uU)D#+e#H9VK3?{)L_W{_2k)Q!jq7*xK0^OyK1T6M z3IFHcOOGG?hW!hARPM+655@mw?#KQY{Ws6!`c>hJ;7`nlpznr{@iuxp^aj2j`fdE9 z??-*tD)2deUhoF<%ltojDe$oRpv3n;Uk83Q--4b2f9P%SU(=j^vM1?Jz+dVyP}oc2 zLHftpzi7Thy^;9^^4-z|72^-R8v0V}smRAQcI~KFrhk$+7vbr=o^-5L>O+jrG=77B zaXmEkw~SAZf8n%t=BL*yVZBmerGg)aPo{sC`9QXT($1Z>;?7uLAEmU%K+Qr-we%{D<`1!MFJLBX(rH zfb*NG9|?TrczW9_KZ*Fu+lO?%JbrIIKYSAN+3}nB`p7?9->F=24gH1n+YjD0KlC8Z zkKcU$&%$3??+5=v{|4vBWzWCIm+TvKR;+Jkf1tcTtY7YYEBpz3f&DG(;nM$HeKNzp z;(w&^QT&kgDa;4*CrxwyIrhWn5^vmpVfs&fU-^#~&Hp^!{WPR=h~vl1ec*fk8GfVf z(y>}^G1nuJzX^YRMK$}YQr{rkr}>!v8vKc#wAdLJW;ez2^!jy~ntsn5pwh~EzECxiYCy}a}F z^ov1H?|i!Wq^PGc|HS$p;8kCbdKc!)0sKqw5!UmfM`k{o57OKWf93d_dKcD5alA+V z2R)BDa4b+{t5f*f#}b~`|*D8|EpiL z6#Y%$$I)LHZ+6@IgBp0k_1&_6(eKFfP5Gd}cOyscj_*moI^tF3F~UAEzQMEBpYeO1 z*ZD&Dx^1tFD6Wq^hhMPX1pk4)!F(6>s`xX<6Zn7CHWcxR=bQBn#0&K2Hs1{2PyMv* z`yrig(*3s<`AP7RY7dI>bG!OuFFR;-k#D5lPCfk#d#(AU`e*QV`YYlK);sk2UFbjH zA3R>35yu{kq<{>)PCAyn+Ya z?;Cv){>|%+NtYV@gTJTyC(Ufm-?JVB-+STe-;eV%9vQFd!57=qbCQ3T20pe&j$9J; z7_NWN^TqM5Z|Ju5uVUNyhkp8;-}!p#Tj1A}BaHKre`9_QTXtM*yMLqm*AtHue>*-W zJ^=q|jmJ2icpSXu{1*9G_z&xO;TL%xZ>z6Eyq`+bN%4E?OW|jn?_wK&=YILrn{vOe zevy0&>)R2}ta>8zE!YR%xBIuyKahOri}(CNoS*rRzZ$+Fx4A$3z19tg{S6Hb!SB1j zDC?C9yA|;RdI9qDjvvrZ4xL>o^fB-ST^3U>miz+!f%Pql@dtm%`&HdZTu<0f`Par` zei7f0zp}oDeB-S7=*u&I{Ng>oA9}If4`h5wze)H>=ZA!^q=zgQ_ecGNzMZa@e5UGP zzUsZAEouQ@OP;_0xf$e&Ap5cAjmLV76i8>0)pT=n`!)f5!MUo}1C* zycoaEFNlAN{l-)BEj9d$`3LcW-|Jr{d>VX&`3L14Yv2u!FZ!#p{+II|N6flk`Pma> zf0A!d>?ppAdXuN(`=bX2Ux}xQZRJO#-)$5g$b1g{<*DafH!<TjGMB3~sg zQT{6X$)~?)()}mCk^iZnKQ;fw`$n&7eW2!B_;0O47w>e<;@fUhT z;+?(Uq@D^riF{O{e?pHye%yFK{F&zW|JUBxyG9X3Q9M=36e%p!Cdz^$ek5Din8jEL zA}NFv7Gi5a6cnO}q(~tt?JZO+1yiI7F)L^xSO|)hg;?4OqIRC&xtzD@{0CmTVP_|^ zcjn!XbI(2RNPE9k54ax}J`MW8>~|A>3;5jrc}C_h@NdB{^tt)}dXBa?`!Ue3%Kgr5 zEx6ycd$;O0^cdiWh{x{pi9f`H=((^y=$8iHUfWyue4sE+pAY{IK0@LPcpLdo_;2zL ze*WQapfAPx%RHXer{AZw|(exUvO zgwIa&M^q>6!Y>J4A^*DwUJ?9V_|_tN-@z-iF3tbU?_>WU;%V>*UGE&{ho8s(fZX4N z4>GhKMm|^lIKtnAA40#9_ci?lJ--q6pZP-ZhkbtVeesPQkAv5UpKLy7>;pfOcpH8S z_#*YN`ZRuCY3cN9RCHW`6H~bFpP4q`+H;Ru2zDfN2wp{TVyl^-?+WKkq@7z}LO_4Sf3H)FDm-;_m z@7Zraal!r~c%*)t_{b;P^;EA|4>9wk@^3p||DJ0820jh(H2a;=A1l13^hMy)?B~y* zH;w=M@T~At_@?-a;J1V6$F2v%2Lb=)dci*qoqjl`zD@lJ{szA&pI_U#F{ZztdiBqh zjWK!ez{k8fovmxbhcfe_X|G6h3gg1 z=?^eZ`}S8|pZbh^E_$8PFR`9%^l|0?N8k&YpV54Z^}AD_Gk;D$HuwPjOzMsBALz$X z4`e-{pG-ZvGE?h|_znLr{7>|4z(-#mE<7E7%=@7^7W)I{0i%?&6KtMqSOzU ze)=Wh%~kKyuVO!#)Tf^>Kev8F))Vm_{$u!-vZwmrBmJKGfB197kT!2izXtu`e4l=f zuxi=2@B;X}gX#H}zX@-#o^U=VexqNJ?=$~@|Hr81hdeL%!Qm4Vf58WWug-3~dA+%G zM%S+qFR4>RvW|114)@@@8;4qED83IrWWrT4(x!Lb!Fn#ATC{ki&ZKGxbc{*?ZK?BjlU@(0z^=GP^jqko4#)|{WePyJ2&eRN>n$KYZ3Gv(Yq vMlYZHd-AUCZ>s-w3v>%~3v>%~3v>%~3v>%~3v>%~3v>%~3v>(o&jSAdWV!?H literal 0 HcmV?d00001 diff --git a/nav2.sh b/nav2.sh index a63364d..fcfc471 100644 --- a/nav2.sh +++ b/nav2.sh @@ -6,7 +6,7 @@ commands=( world:=map2 \ mode:=nav \ lio:=fastlio \ - localization:=icp \ + localization:=amcl \ lio_rviz:=false \ nav_rviz:=true" "ros2 launch rm_simpal_move simple_move.launch.py" diff --git a/output.pcd b/output.pcd new file mode 100644 index 0000000000000000000000000000000000000000..36c50b807be7e045df19bdc20db7ff4381ffbfe5 GIT binary patch literal 129604 zcmYJc2Ygg@y8j;nh7bW`M0yFKmk?rr5W-{*(h@){@MF1LmdkRvT$hU-#qP3*h{&=m zYZp5Z5QijU5@RxRAQO5f^bipdF-D{}0V(@`f1b1Y-+lRVcJ|E4_k6!k`8=PeoKA^- z?io5P@rk~D(i4*t?|JCY56(^;I_sgwrzZ~k)9gPb&iM1J|4E$j(4!ChX?EO%;rETp zxqECPm&T3A89rv%xWp$DpGtf>Zd}f!;fek9-?;JranJC?5&Cc3(7XTN*zt*}`fuDH za)yo1PE75aK5#%>_VApM+58`I6LNkZr-C|_kp_O{(|M(-FB10?gy}q#<=4t<;Sw-%dRlD8oAKv8bN-A=5Hyz|zVx5T@xz3pML+*&e^Zx3ObDd^^J#LGd zQ}+4V>Lc!Zr!V<44}a>+KVIqj`=9bx?#Oj?&ugpC`lF)OJEIy$c!mr9x8GXr1mBPF zoG1Kar{y{`W_;~-i8<@PvA5J&{6>Voch$f2WS+BRW2GAysqb^08+&)U*++i%KTx;I znOXXcyY-1v{wXCD4*Tssc7Hu*-;-ZDbyq9h&7&{*XUqsVoeoyIsj(;gE8od=($e<0 zHIX|1Ja?nhF)zZ}Zuk#gDRkDhjoAH5a@IQIk5=-$wf^;6wmI6XYpd%ybAlnK!Gs8V z@r%Dp%ok4oDG~PVXaBO6TbygFE8R)cud;?R)*s>APx;rJ-DK-8oqe7?U&mQy`=2_t ziGKo4bg%#QHw$d`r=_iPMikCt54QR<*5|qVo?Okj-O%%LE^o1}8~!or`N+&C$kaK1 zar=C}UufU!8rF(j9JOmoXD{=oFFfl%)-~VlnD?k%qtA__-|;_`;9axk>|a^?S^p;w z=eujp-iN&W%YWz8d}QVW|Ap&&?ep5UulV15@Oyv!#C$j9&~(4#c0yBKe+P2+FaKlr z=Odd-{8g9s*!4}5AN6zIcF%T`hWXzOpF!sH-Caq~@OSsv+A0$7;qM;dx_oz8%g%OB zt>MJI6M3%PJbym2)5oqEe>C51)nI^Mawxr#UQ~bF-Ff@13e$zhmko4IJ^K|hQsKtT z?c$^yDs?Yh53|;O&h*S8bZeVCV@6k#RnCTU@5lLnhI}XaFOO^EtltuFDt9#Y*Im68 z+5F52j2z;hn9ml zz#HhnK4$0F=D5!^ zJH>w86-r;2)>lk9oVQ0eT^*tZk@#?hY!-+b^6*+T z{mg#OKfW{=Gq=B6c)rpfe7|+D!Gr`Pu*_K`c>xaxBDt7~w{ z#{S&DlKbo^d&GAKHgk*V@1f2kSbrwAS9*Ovc6~5`7+3w^e z3CPjQ_FUB;w?uwk2|aaeg!{?EU7X9~js`~*W}011E;tmdx|E6Uh;ttCy%PMObvMrD zBzCww_|fpLoWV8raa^!fgRbriGf((a4t>J2bioGxijBQHSlqq~`+E+3ToGK*JjacS zYRNgS2(rhnY>1w#{9<=(<5}%<@Y?DemqvX3u@16A}z3ylkEBj>aKFnzP|BChr5S5H|2RxmZrw`w)^07%1%Ao4ZXYOuicpx z{Pp=Hcb@wzGTOqo@5vnGrWv-hLs_#xH_q#te^=7MGHil7Y|%A;Du?>^TRHBjXInVP zW7(9F9DG@8=cD0;KCR*3-w!(0H3vO!E%~wU*S2l#w42n~-+#&|d-j6mDcHgscf+<; z4(I59#5W52)!O!-v%@E5Apd^}Z5}<6YciZ?nmrhbips{$XQ0#n5vqxdbTifuock*wt!@7C zj!sQvL1;{Rj@y4q68rgUu>4{+SfPzmuzX$cm+qaB?d$lDe*{ZscLLAIcb5h)TWi)C@u-rt%rV@B}KsXm@J-jVLduI=nji@)s8K2j27f9-EY z_gYXUebL`S-h8-@J^r$E z?6mHtU$PIq7IZbtqjQ%Ic1vw(XZq%YJ>8`zulbLzcxUj+cX}hk|MDlyb4_lSo@|Dk z*B^ZFN*~jmx~r>woet*U<8O7aY2b+*?%BcVFu#{SFfs={YK^S+!9M1&&$l~0CNBwT z|37Gb+JC6?OCkMMSm^Dyo(g3j8D%zeSJI-8t`{~tvuZ|YSJG(Ka{~W5H#E;3W!HC! zSsI#Jn{A)5*`caSqs_)nt$jFjbj4`S?j-uVAhfpaDE{tAu+2Y$_b=@)9Em^sbFe0o zj$BpYCzc0~u1Nd0o$MOi{$YRj@ZFWj*Ym-97N?{8JGg!|HpH{B^W1G^Yun!8j!EBY zb_TryM{UQRzhbucQ0GlxhO=d9X=#RUWdmiag`MtS`WaX*4gAvvoE5-EOu&b>Lv91; z!*Kp)SMZ?x_>=y^^X=>!Y!+vjYM-AvHUkXS&3v)&N&LiA^ViE-zUV)3Z+9?h0GWH( zzhPUd`S)H6CgAroIhO!7|7C2=9rk(2#>f4y56xsP>tv6xVS~*-HckG>|4aAA=1-@^ z=YpMDIIS8Km|wm;u9GADlQ8e^_>r5zLdeSh%EdpvhXyH8DIm4C^`WJj3F z-~V}k$GkYZe#yr9{^$I+V|(+_-M^vFiT2xR@o(ZAdz-Hp-k5*9kt6)@h;JsoshI;7 zbr&>$ihH)RwRKB<*h$|eDu|C&{irR_3cK{0&7`A`=tyJ(=9%mz*-x0m|inECz&;7tK zUpkGWtB~8yhMj~b4|U#;4+>z5|KL}AB#gK6oz8w?dtp3$r~jkj`#INjtl??HtRZ}U&*EzKD1e{% zgWX56RJ-%9tl>w)d%n0ioW%vh^NVJ!0<&xff9G*7tNr*2!^~$^y>4-Vuy^^zH_?wn zhO1}HcnQq1#qhcQ7PgSR79N`g9@~ta5LSPbHGb_EPHP-Jn&++HAH@;l(Z3qIPvhub ztRZaQ&n)fdpB8_Bv)PM33A4BLkdy5eKTc?Ro%`quJF8K+sotj;Oi6@-^^YXxo^L<5BzftT~a)|)$k87m#wGz<0$lN8-6m3 zeQ0R0fMU|emqp=ci`=xd&Dg8!*v<`heOz^YY-u6S`we?kpV+hr46@TrUsw;U`z5hp z1@TFg)1s!3-_@{&`gR{|81|~36IWe?obCrJ)^{orbDgS7hw=4M?87Q_wbqU9RfoUL zMLzfA)3314t1UK^%;@`G3wHCoU$dTkXX_IYe9&d~^E3QnggCGU+yAA>{NcN=BY$7m zZzThJ?%L=ayb`fBtZf@ak0M~0BjEBcO)uUJyV#@6rVEm{w6tT z&x%9AA38Vee?InpF*bRReV$qRPt!YL{^0vh`{Szjm`qO0coDtZWS@iYPePCOY9Bba z|3zmvSuD9LDbwF>(q6j{I1GDLfvxqc&^1+^6dF~3X-^W=g25gKy-w&<{ z<4@Z<3x^!w+{%e3`(OjMv&Z5?8WEdR+UK|5YK7gdGMWGN`JTor=svldGTD!fF8YCu zi{X63$WhpxF{3%(Z!+$sMNL;Q)fSVtY4HPPe@wO~W;8&SzG7d)*t^@XrDgX0(G{JL znULw!#EgFU{UUt94(wwLaYPZ%wbhlqy*#c69ohsYOmddBEJpXk{L>e?Dk6^Ak8a0d zCkx%RZFhquTHtfU$y8xKyPA%GMOa@yo>x3i4YJ+KWc!(B`;nOpeAY^0#UH>YeVy2~ z;!?ihx@2c+Z7w{~9`>h)aXZ)Ga54|4@c*xM4t0*$_g!LE+n!ck>V;gdBtEU>%oKaB zw`&T|C*zw|bDt`3S9j;#aIUlTR zJ_qdOjB30Fe;J`OBWGppX2Q`JnMfyKokpjTh27JLZ*`evL`* zfiDWP&MItEZ?HxHIfz64>l=O`w!+RgFkAutL$-ghIPtyHBazLU(C+}xGQclBO8!M$ z!LpV=m`@Q7@%N8m?E&`f5B~Ctm+|od2aJt>uLU=*V-E-8qxQfJ1z5v8>|aC9QGRU> zcK%ml51AxU11L(KYGhgs;eWI)J24GgcZ_ax2k)y@vdwcs{YiKZ` ztMLOj_P)vI=J?EY=;@Q#p|*xk<^=s*-<8-Z-#ym#L-wsHJ~kiBy8wUM+|rP5D1eWc$NBc<48{9B2CvZB;;bH%M-qG1;~5u* zkQwt|i)Jk_+`jdRrmXo&P_ckwhaQuwh?Pd5BUd@&^FhVbvN=bd{%3IO6N)3gL2kYX zj!92N$A5yKIulgbQwB=f0D1P9jt4lkHm3h^N(kM8?M+j z*H-ti`(Pt|zdk<-%yEl1%7{_2y5li>lp zSbJAe8W{M+kYu1~@*wH7$$sdzASU-1aH?rH`y**c{7 z9l6JjH3~Hgc-)0#LmZko2gUf}G>Em3)t(mn=Wa>^7u*5In$2^iIoN&9CI{W=jy#t+_e z*C_ZX@$*a3i!Ao-IBV_^OfDFX?w~rDyli0y7##a=#zX^=- zO_uJX91#B1zQ3_I%Gt23o_*H$a5KahIoO2Dvirm+9*@ELjmJ|yMRDAS!g1ik0myBD z`15i6&H!{L&wYL9r})sW`2Boz{Vmqg#XdiF|A+jIRP$v=SA5D@Wts0#%qzay^MijU zKJANL&u7oxfou2K_lNI(z~5oM7pI?k_62-siusZ3Bd>4{gTXUiY$aSFzf*YrkHms~ z(60hl@f}nqg~JA8AJ#mjuh`uo`5dd~m<_b>E=HF&m zMZUG_lJ|GVr2qN%?~gqFBL40+a9tig9i=DAM#KAXVqw>+SQTTE1`qJ6skhvrLlaJo~ z7rJ%9ep|U?7XG?{UB75nH|#{j@>#O2W2b$GYzr6pjT6vs$yKmF_1U)q@KxgP-^)+v zjHcEez#f#F%}kiL6JJz8ywC{@znK`S0-Gt`V>h<8f|#@i{v}9UUO{fSE%LV2EpA_d zKkb5T+(+CNcF(NpV{rsIM(lQL?5&HhtKgrG$g>xxPhQf~;)|8UJ92*)j8{P{))jdyLf7`#`fuzl#_w(7Z%9YJAZ|X$@4H#d zqMYxkc9yJzX_Rn7x-M>+3i*nwR3Xg~YXj_1l{tzTnL;w_d* zjr|TES0DelnrDl+*H*W}KCH&a?81*UhAYkmv+ZZk{{^PZ2j55F8QS9Ch2= z)yR1{{%!|2B?9jfX5Y4e$98efVdUqS;&$XN48FR__9-_A&EvD`gvw>J8$lnR08{e_#8|?lSiBWb>{jKtg@g_?# zbGN$lk8gl;77tUwI=?|5zqH>91J5iiwfI`GgJR)=<$L(sTZ|)`G`$Mm=P16oi1luV z1HXap74f{Au}4kVe=kN>9=~9D5q^FjzQp5#W|ricUzD6EmWZO+n#vi?jxGfhw-^m*4Bma`IZ_&FL+3T)38%?aV%DJ)kJNU7ic&>8ndjx%JB0h>(H^MVCbWT0{DeI`@jOu|WbFtU^ z@qhKO*&&n3>?3j9=W|<+VuWj}y;=)#N^#Yjk^f50v0w9gsq<)git zieH~sUb4hwQnqVOu$)|Wm3ydjjKx8P=Y4RW`^mEe(2GA}lMk5B>X>(*@iyX7AE8=3DWj{*l4 zZ9ZD|T;J=OmG69l4Zdi;qjB^iuunaUTNe(Q3SZTTv&aMQe2P!0Ge4qwiRb*@_3{y_ zlTj@EZrH(}G~zk(+1IzsXNgnYbJt(Z7b}N;hWMBDw-u$Dm(VDa0vyK2d zpUnT?75+!L=4SX@FZZF?w#VddX0OEMR$Xd?j0GI|oiXXR8ZV*z-@z*}j$+u?hyEWv ztGj&f2~5?|zlmp=xZdo>L3<`xT!quz7ZKftXS#sgIDco!Z% z1-;25&YKH18))Y!zpokw@dK*IP>xk+RCVbwuwNg$zxV`kal&KQR^JPjOSbRtD{gP~ z4EkQV;phCx*qK)?&Z@d}9{e;fqe|V9zZx9kzR~~=K#@j=} z)hpk*fcz~azdr!H66QHq5>wwn{Jur9LhL+>c>g$iT0f*5?wYgb@F`=71qR?xH`#A} zaq0MoN@{vS!THCPyZqUHD?G25VruO@!Gw7Ov4Ihb{S_N#tp5q!*Z>cp811y;6wB)! zU9lZ3`=D_Svj6Cu#ZB|vpOt5#9$*OjaG3Q!9xObs80v&_nq{>+74!aVvA^Oz)pV(@ zK(!f-qmQGThlou^;QNp3Tz!+KkA}nl86Vf1`h;vS^hsp;xR0ELt)JfxRvuP+OC3U% z@{gSk9s|3V`t-dxhxJLjwUugrno*! z@uqA(xy#?LiHeHiy3yd3W9;YFEXBgw!+RFLVL5dDE$IZhX1{HkoM17_+ix91A5LYd zZh^Hc*KEI4y-LcV&ghNuw)$I*ES4$BIf1->o242CT|-_Jo0kKJdmJ974cFzkZBkAW zx74%i#cv2Rs-8h{7W!s!dXLGc!O~r94T^KA{V~~4oynx>i;PcD4HB_VNICasnsr6@ zzb0Pp$(~#X*Zwnj`0g$)`Ez{bx*&NmWc8Zm=yNx9!Vg|Wex{Sp^>XyYG4Mj24F9Zr zXIL3IQe^U~ohh{!;H#^Whrv1{@zU@lWs3XWeDMFT-+&th*WET)&)6}qHF?zT@F_WP z@rmeosy(~%dcxO=2ZbAh@4p>X?S$;cx!sjP)k~x=%)l?tvhP*Dp`5bfXw^%w=Ws_O z@#Put=Pz1}+%0vW)1O3<EcQ}Ja>Dszn-`faie1|l-@U->%&|QpXWh^EvA5r<3C@_&2Rw64zS(Mi z#6_sSg?$8NFIyQ|FTjmt>rq@Fe~R+jRlEJT-`{nY}`Hj6*xr zxjDGCzSRhIn15AVMAeJG(EpY&I1uF{FyH{+Lw!%wDzg*IEB7fuU*al83J>uVwGQ0X75mzge+Q7cN%)Np_FH%~_?{LPYl}CIpV-5ErEt;X%M#6p z;;)GDm3Ih$!~O>y=wY!o_RYQ*2O|C-z7-s(I>uR?r&ovY(eRhpk2Ld%>@OJqCgfK2 zduzeAx7hcGI=>EvX<*#N19k6!2gEHaU;Gi+Uo|ax*t|91>n_N-Fx89L{`%%a#e4Ys zH#OWPe?7IfrSasdFBOk1|4x00#kRsfrL!Mn-!(gg8ysx-9Hpi2j_2xRG2@+6_hDDXDXjtHM!@Q6;E>hun^nkF zXE1m!Sa%QcT?_Daf#LQUGX{XS^N2Si=v`O#@H311wm#9E*!C-ABI2s1t4oZRlU4lw zh_4X56~X?rB5xQ*CL-vL4-DnSgT?KW!POy)4HbK)9NJ=dJGxf~Y2B%y2ba zF!|d9R!fkv{(kmwo5h$N^X>pU)fq3OJVp7%1NemiXEq-l*lzhI;UeXOlyBPl#Pj&N z8oMX8KH!u(bblRqdLew&PgaxgK;5U{$vyZS)p)#%{_JKSRTHwvI2p;0YHUtDTVa0?ynK8mx^InMGdx>O0j7$&ZfAK&lR?%ZfTMfC+;sP9#c zy5eKSmCBu^FRX#5Ea$pthn$6R6s5C|;(s?8f1-OncK`Ng&>KoR587!1boOZ-8p7Noo zu?GxmNrts|7q0(+-Ok0g?ZJ0!NB=#!Rc@$s_NV-|l4sirb}cq}7B0;^{JG^%!C%Ob zi>-;+d98e>6kioV_jZDTw^|&k8qt`!UYuCmejn#pVDTp3fdjv?Yj!1V=j_Xobv^Gc z^t>2dtt6&a9oGiCUUD+eEyRvgqB|SGHe0c8JJ^$5V7ZWSDe8kb^7Ix*HTl^`wqe&c z+xN(g$*OGE>q8HMyYj#(ZzDV2=gCVxGQLi7OMR{IApC3|a+QmpcCnTD{LOzz9<6>} z@u}{kn64)B27cN#&hXmmx!C+liyvo}j>m=^1hXi9nFfbZ$(r-wwEBbN4&s0EIg=rF z{XDnCY>xU4R0FnQTSx3tgf;9n-lJn)0yc0T=dRw3By7ok;>U8cMLK`+FsGiqg&f}w zlkLr;JD9CfKM33pap4C0{)L&1!A|Aa{4l=0EjGUbnNclGZ*awCSM?0rKlI@%i@|zf z*3k$*Rbo2$Zn&jWe$neUS z{!qGB;gXYsmkdfWBV|HjdocAxcIzJ|L<#ow(YZv6qg zI)HWNqUQ&})oI8=E;6|rJCNe!J(|liA3+!TW6wXcy-+=ac)1>vtBKoFi9J_3k1zWH z+tMFjP>4?zkEdRM)sAehYC^;lE*$a=XMYE_HfYx<4kfPSH?q~K@CLcWy}R8zrzUf4 zE`R$wyZ_`Rn@omd=K8>;Uz+`guXm1h{S2N_^)tPlgn7C6i~Z<(Z=P!-zTfi;=?lNG zHG^5e2y5)`g0ES_HvDcSYxeP7(DaUc0D4)#8Y0Bcz3?HMgoCjs1Hl4cabIzdDGuk% zIaPt%)6la3__#4PqapWHyx$8u7i;mpYTMNFB8(wEU;PNWUioh2zf?1jvA(RA}feQM07sD3gn?PkL{H}*aW|4`4a7w4dO zN4Vv_;wj*nukE+%x4Z;KI&FWe`s2Tt-l-lSDk=~A)QkO8%>F*#_W}cplV7LU&Jia= z{VSNKm+?PapLh?w?qqo8zT)S>MLm&+JZ$V5Vlofgs6Iit82K{QC+Hflr{*o5yS0G1igpAjxnJ?A671iPNxE!eD;BYju>fH-hrkA*`L$)WTzj8y&c-?0I= z+Il`{{Sf=p%J7VOMw%ukNygBX=drKx_Pz22!Uhl2eeO=0-Ud6}*`8gsmnZIxH|(UI zi!L#(?4Hz#W0&H=Ers~yw&ZAzE zTYt_$oRa!Mx`St44k->@da?mB|7u9_(Vn|>?yrOt3#Tu<6`fsRb;8lTTA)MGA;qLk zlcUksDC0DGOm0Zc&j-PUL;8}JRBm(=b=T<@V^S+XoZ`dx*9Rvr2*y=sy7H%r-45UF zvlyh)!7s_T_A>rNJrQs<*oUjutLW_>K}Tbr)Sxhdck3f z2AgkHPIm5VndCOE5J&&1EV-aRcIr6$6-5pCKzPev6swkzf5Wa_v7GGu<2{TIVgG{n z6?b(N`_nh!6ZeuD+W?taT6SOY0J|r-T3_&e+l@0l(|uHDBH7`Lz=Tb}z^N7&AHKT@`qv$NH;Oekr8Z!J zaaQE0IlJdWJSY2{iLTEI`Qk>hpMw;0SZ-Z?1aL(m#i3wdbZ~%iS>n@(p+e$DRIkFm zg%n@!xoZ$-_gzqR2#T+<|3TFfOsy5qwwxO8k?>IiiAm>J?}OrHWXS60)oUP|q1pgq zOKfKndY=O}9^j}(TQM)aG+^nL7MHP)*uS3GB-NY`U=1C?p*gO;SL~)Wb7oxsvOSk_ zreN5R>R4pw6(j3;PqF$MTCFm!dr2sb`m%jwlyHO|Hf7aC@xt zVo>^`dQ@r|sSU`&r=KGa8XbD`!BNOV6n^kydJTN+?HS_b?}Fk?#5WR0!?_K?H~nIq z40&eL1!~~wrx&3lPU0Ozs{cteKCHUgcxh9R+{9AOSVNkLmnS_U?wr2#g zeVIM@(D*v>(rep(9aLS4_#bgK6Po&r+eB93Ax97kUjny3YqdkF$!Ry~6V7@dHu13d z(m{%~m9Ie;%CKox@2cJntxdg1;w28WZDB=dSk^s7wQi#w&RsO-XZ)fXL8Ty5Nfur&Q@W#VAP70_4Z6L+L%L$RAYP`%xt8b!JgDmX1S1TA-?R~#_^c?JWmid~(^UK+L<&pEjK`V&!e(M2z z6&svrez4u7f3n}b!Dt1@$ou?V)iSMvTX>ZCroHhArL*TDr@g2{^LiG z(ZM(b>RrsI3s)aq@r<}A`(E*Wi<J_+RwnH1^oz zh2|fxXP8Hv-HCg**!AR5EMC)Zv0d!hDSH;(Q}upN9V;Pr?1+BUT5LsKD(g9Eyo6+1 zwfB4O3X;D|L?&aA?cEmdsE*?B-CHf*DamPzZQl#`sW_+$nAxjID8JYeo4nnwSKrOS zEBleja55Z>X1{HLglEqUghjx;K>_B|%^$bkNY|MmiS7KH#oQZcKmNH&$)+qrxCGdF4(2o@RI7c70al{P4(E~ z!4zM#Nm-3fDaJWRJ}4LdVGnDr#3A4`g;OV{i0nKNKLm&6!>%zj> z=v{>?en}kH!XaDiv+`E(5$uQhVj8mll@`l`cbt-(Qv6TYl^*R%3b9XNbmkcLaijGe zq#SAh{;9Cf$pyEAZHkbqE&SbW#u42)RekQY*ys|IPvyiFr;8KIK9az57r9Rz+l_AQ zLbr>Mmx%c(#o)fU?dFdpZ>6(0gI#a6So^~D1NMxnc@Y0499z4y5G+{*-ngAL=khnc zW8LD_#eZsULnD0mYV4g?d!QIxwK_US;vjS{;!a)?XEFHOZ|x#JjkTJGw6w$Sh{BtZ zRWC*shdia^Gpk!r@4RL*sP5vu(+!E;)jM;X4IQ$1n_uop4HtJum2`+o~M^*M2O1X~b`?yp8Ss*wNs=-_7it#Z~;QKcrss$paH zg2|zI<3%ExXj25a18G2=rC|BZ|{=2`O4o6lI=Df1EIkG=+9c+IYlpZJv3 zzv({mJL29I3yb4bu2a5XX6cK_)Kz$Aal8}ldi9YhkJ2Wk3Hy@|)_4UT{)E}~_fG#W zHvFpj2=NK{NQ3+}jcyef#geSiL8zpM0j-SYqu&`>k>TvTuri zRg?N#{bbYRzp}<;?j69UJxD&Oqg|sOr8BFV!^Z?1^>j#{8%M9h)+}IuIvKy=VU-rx z3h^p`#{V`pY@pc=I$u4z_{J7BS@xOSnBgw%^PaoZE85R|wEFv=^ZV>9-VOg0y7>95)=>DBEYSqg}5!>ywAeEH2a85hrG;hD5!$ z;4{w2tEo`^vYuD##D80@g`<1n~o^uT~v}>H*;6@F5F~?-E{0jrDpQ#MzQ7Gag`Cd|!N7 z7&+05j%&t!Y0cOpZ2Je+r!#h1ODA^iyUZs^;#sc4&yKYE3dQg0&4EWmchsBEJt)0X zEyv^~ciQ#hyQsq{Q*ON^=Nd6aY>*mK?E7_i(q*N3mcYnXV6Z>>RFkin3Bt&VXX7V! zG)_rzZdzI=e0z6`J;gB!gDYm#XZ9+j`6O_2icdrS{v(j{LD<5k#(}8+of>`mjJ)2H zRt@%BKLR~A`27YU^(BeVSMP&*BE%!9cSF}J9}2e>(&t~FSKa&HnZYuGHD@^F1IfRS z#Fk~S&zJ0;n$fX-OY@Mp2;GN(*RIjuQtn>;FRJBW-j;D4>T}T=WTR9+Pj6>X9FeYv zhc;bPkBsJ2sK-jNr`9I<6jw%VaM|KFyuYR1C2>l+UOa}b7vHXZ);%Bb4Pu`Um5JZb z8dQrV`=b~W{q;T5Yy@~I18$^4ndW$?4n&-c_E~jn>QU17Uj0efC$3dJKGgbU$qsAY z2y=R}#G8qKlTK+ps%fG2DNFlYesLi8KWv|svw%m+lKd+chHF9(bHIfI9OaE=PyGD{ zIFfnEmFD({f0N9pA5Hx}s&kWksvk$!h~Jj{tL|b-Nh?QlY<`og88d#g`8ld_RhKeyNLbDXKLhFTnj{iHW`rMn&}{-nq`-&tN7`XZGZb@mQ7{g(id(Bk`urmox?UPWuLo&bTA^-ij#k#3^D^qQsGHJVo=ew7>yHepyM=V>}C-dHja7}*H_+rv0d)k>)+OLC|@ zplUp&v*Lm2Ei}GJ_+0V${Y#g_1Na^FmHg8ERd8h%*rUMW^z~b^xc+nWcsx8t7Iq^5 z&S=LO-)fj-I=PGu+ioQ``OHzh_~a$64GY1Kqgn~z_W(BHUhI7vyM8`3oT|yu_i&Zq z$QwsG#^8AEi~V(TWX?VSLBap zxdB!yAV&XBWKD69H*Z1x+C3((!#_M@&#v#4YZE3H52pGDaS^JGfwx5-)eoKzC;DH; zi6zV{hW~DFSX#doH?6bK`7RvdaG!SUYXP=1$?mUMOffb25A3#j9QLD|sqomj#6Mqy z6VxlQifazD7cCsc=9-To+(@3q;&kELnWeeTgr>XTjXPLezo7X(c)A2Vuf;o$FY9YE zp!$Td(-OhCpIUrW{c*hb4bCruocF^v79m3sV#`kO@ay@F;`-ZI%SP;7)xYhbYc(z6 zF@Q@J*Q*zpngfgXRpWGY#b#plD(rkK{*HLLY3x-waf@bg{0F{jw{diDKKNJqXu`(7 z3D2u{KzN&ZH^}pD{t3u#f(yR0ctP{f$Z3EZbzW=P=SuWJpWj8Uwp;vFlJhFD##W0v zG#4df{TBPIIu_M{DUUDwthog0mr)IpI5%ts&svTxtpn4lCL)aAJW4Edfb%T})1QOa zQVmE2xb$0aX%+sY0v$d{{8nn8>#o+Y{!R8-H94IQ?!p#|o4SZ@7a{{&u%8FuF}>QA zQH{T+wqXmgeud$j@{4m; z?&b_U?>1tBFnDYWa^m&5Xr0uWu+L#}b70O2<9syBMKerPcTxSZi#(JW7k7<0hpKt_ zp!IezaJlg<)B&Jthl%sW%T>Afb}*;c8=}8Oj0Sd5??xrIKyxyyYzHR{+zOa2Dr-5DG;;2qT zzik{HGTql)rm@r3o9=4{2)!@1=Uz=$neCg_*3i0bhpk(q9w57h`B3cHKH}JXF!kg1TXDadX{l?ZgVbE| zO#4hX)eET^m9qcLAQPWw*Thx-H+4OGO&>KAQvDIK=Zei|%-Cc$T-?QT{(b1km&jB* z<2q!|hb!Ice7$_FYLljv>@&_ym|3%kG?!I1nR9~G#tkYT8dtsDWI$X2 zwe@fy(w#2wKQ>PVzcPR`+z8JU=DrEwgs|xsGw&Sg@{RW+R>U`bi5{ya?H1?tp(at>{b#y03glFvU zXf6kJ#>nbG?DRRudy1_WTg5`cE zVcvS<1C?J+E?8~69P^&=^<|u;cu2(;dB*j1iAlw0Z8E<`E|$+F=8xK>^mnq4Y{XY; zCXwp5HFKj8HnOqhXjFqP{H8q7#EfR>Q+>`_IkFMRQ4{;Dz8=j0Q9TU&I(mE_{~aI> znGR-X%33t@VH|qd0No0}hYkaKUNGOp9wXyN%&)1psK;dW4C-&qMpkdK&%#T}1uO5P zS*hZ##UHkt^dgx4veoKnuFh||nU?l3_%H<H8PnX44cnGtzq@j#;^<~z&r*;b^))Z2rq7${@!0)y(M{F6dOV))!yFN0a3#9d z-2N^+5ue-J{Z$u7ErsE$=w983cT>O}`N-F6={o+cEpe3KRdD>Zw83=N4=lo3{`j6CZ(ONO*TRGFq#de-s(BQZs?lsuNqXZ zFNqmBmRr;rsRv5+43Yt6R^Wen5kqM%>l5Hc#i#3#(W&_M-sn^S?sYDHxVNKu1&Yhm zSH(=YkZKszhe)lo)$MCeAhAtQxn0!~XwF8vNrTZTi#3>OkO5}9ruaQbtVS$(*7E+u zpT_5aEr^@4(cdPV%Q0f=KKP!0@yxTR&G7U6b@uuttAWw%7wQ@<<`U-QS%b>yi$^6t z9#rg8+`bmf{Sy5?-Hh8&OhgR`@%XRAR`(DuceA*LoSuCa|E8Fin2mG#g+V z_`83>gH5!)0OdY37g79)dK+J$nUFohCR{@oO3IFP?SWl7!EXmz?4vw_<{=Q<7|%mL zg2iRx_ptSYIY)f=HQ_LyIGhXD8?*oQ2Q!}!8+8qTx7?@wRGw5aB~D4#sD4J=QHz?U z*q%Eqp3-NlLPL#5oHI4i|Oss9xN?uHcaiw|K= zuwC=!gM$=5g;a||&JuZPZ=V%|V>?V9)H9;l6ygLF%d6)?@u0p}uZ?1D`VCm;KbR@u z)!qc;0k=YzrN?(eX6|?|3H1K@o#XK*uOh$ zR)cKpgr=WaKZ|PDiCgWnW-F<7OtG+LB8fK;_Z8hM9s9Y{c)97B>Ef(xrl@l9nyV-N zRy9B37Q~NRqk({Z% zk0y8M%~sXlQZJ9z&+H2@ht5m9O&7aH_Y_~DJt20&R=*Mwha=roJ&!mR%`0Zkr#%<_ zFxZe}`>kdZiTlz$Rqv<1KdnJ=sODQM#sM-am{pSKp}W zl(Y}(nN>ZN=4y%)Pz{^`XP^Y_q&U~23z!?);8Q2dN?mztd-&PTYHJ|g4alw-|4 z^0f8(sa6Dzm(PB~=gd;E*|y?Xs1ag@?LfGcqvAs8dF#kSmL-1VJPz1p5f ze0SPDJwuN$nr5@!w{Ri^o+ z;`XHrvJcvG?4D2e(KS*0XJ+Xb_WU?{@TAq=Xg%V_#g{6V0e_n%{#@oBmzcH4AE--~CH?yFybJf+E@?x|T*T7&AWIvs3f{I+;4$*SskWLJeN zH2X`~Pq`=N#qeDp^G)jc70;;|)KhE!V!mE^NyWeL4q()P!we~G;vn;()SMxA?d-E^ zVyT-%@6;!|6#TCKGR@9<3w`bm22(EbdE}+F<&re-RI?X^+tvG{SwP}j#Q$ifx;Tv< zlOHntAYZIJ8gsPqLE>{V?LP9|s&Q2xqI~hPmebj@JBVEh@Qu&mi~ZP7j}H+h(f7j^ z_2${TV6SxjFz`|;`mQ;sQ@BqSSge5WJF|~{*_+S7aSr>F<){y$$K*ZeLME8!Gwf-^ zxMk&$#Y-&NsQPN<5W9jaYOOv;T$yrM!g%NoF;1=JaN#`A#rlQ=?<>v%|9G?ARL`u~ zU;LQt$ZvC0qN1h}<6L6j1Dt_ob^c_0g=$wWkGt9KgRf-|>pS8um_rS(;9)sR7LZ@> z8qFG2Et#&-J;j5mzDV^p!xl9FV+6oYFW8(Goy7;O-$$;)7DH*4+HW%w)fcZg*Xz?! zze5x<;>~GXb9SS}Q<|l={lk68gf}Z*J+R74c1wNJxG()yeOT94Z!kUtt`i))*REGh z*R=SbEVhCpW^G=6Sv;g-XPwszGbi)Bd+@j7*=B&>yg4x5EZJY}d--GaStwo;-=g^rvZ3>5581$ zR9Ydo-s}|3ms9_RYBPLs9f);Y?pa}RoN9r@=j=*q!*9L$7@FZmj|?10g<+2tH5-X- zE8rJepo1Q_Bd$z6N1Dk{yE6g#EHN%cGtbqdq#h#GkSJCY&qJOYxh--ZsH+A;Hn-o> zr{!wa2eAzQuHYG3f+e<*uLyGeu@cTdvtMEr@&cUJr4GZLKC`;gB8tzNMpgf2bElGXzxgqdtrGPPeen zuMe$;>${D;%0+I!!H+j27RkkTes6y2Isa<#afJ9Y9zVDmoTr|n`dpg}W;%fWpF|(L zxNKBo&BpiQP1TR7PD6TnX4Q{exR;rwd(p9-U~|nvIfNYV2E(cr=mxgQ>)%;8B%ZiolW_~>aCh*W#vLiY@ZRa| z_I+{tCbkxF4yth?eL-w3dC>h`ut3=Iu+hEB?YB*nxL z=?)^J-w~IJyQ?#SjWD-WPDw#243x`Y`9`@y*K3D za6IRt&#&2M;eqspAA6(M-m2$r6u!W_kLpx~brs_iccC}+Sg-nhrh<(c88#3$npzug_(VNE znvtUV8O>u~(A>NJ)U$rOzkHM4nWFktFV`~`jHz1C0_3&>{-TksL3}=S^j6EN9Gm7s z3#(`@tK!_$*iy5x4JKrne$n3uzn4XRL;Xrw#N{d2h5$0%l)u~3{PHIc{}F%Mm-A7- z_Hdr7Gk-S#_DB(C=XU}4u{Pj{rgn|iDLzVmT>ag8_lj_c@POW7q0h>-J$0<9eHK@< z_>HcH{oZ`=QO>iC;RoTsMYC=(c@r0*z18!o)>nCF@juFcDK3)_jhQJcfEz6ibTBVxK~)dsi<2xy_K`P0h7n@9i4Zitsl>!rRIV@I3atW~UH4 zSsbL#$V5o-zUBkmSNt9`5bl7l*ea}Jxpm=fT`w*|ST14SUziJ#j%}>MkE{*8Fmo8+ z{|H_=VE0sQ68T@7A*p9kjB{@HaBN&1vBq*ZlfJHSk#gC}+lrR}FP6bgV*9pPjHCEV z^_eYd{$_Ook}>LYn4d5hKG)(4^3+4IGe4li-C}DtQB;{;vLlsQNLu|i~OKwW#~OS;)s~vV=fjgI|RHBQXZw7;84O;_XSn~jr^ zJTUjhVll#g){}gw$I!&mBaZwbtI}qFNep9lDQx zi_HzGj!1kLTun&*Wr|g$U*cF)tD|?a$Zm++p{K%lPH|bPr4e5w-pm*GhVcO6ofPk@ z9>$x&@@I52+xQ~l{W5W7s-;oQp3YJH5}e^zs-@BQ;vvO{$R1J?;?wu4w-Hw#0rT9H@Xz&$QlS92AKE2MTHOPL#yJm@_P+gOHi^L=8S=1{eJ1HJa`l7!p zUW>e)eXsd2VAd?%Uw?PTj2L2HkKY5+V-rW%n#CLF{`$L$gLNOxb^om<bk=O9dNZk`Gh3~g0vlw!v1-QXSBH-pgkRIV@o%wdsjgz?Ey zh>6b02OHn3USQ5C7?PmOW7xq!M=q4~%0c%`= zukfK$1;kRzc#gs5iuxXrId(KxgU1!4Qw6vUsFr;RlE1#uz$7HN;Y1l?M zN;Ch3J(TZw`>l@1s@J2fUMhW74UcBph!apgXl>hH4M#nc@G)_AL-bwT(Q0gMf_+w> zk!pz*%WojBqnhTszA1V_PydmumX0~n}aGIOg%W=Qum@e8RRB%k+TED za=nQia5aU(uPaZoBYGlVRvKPb^T@^mTKS8I#Mu5Ph)qI1akCUjM^3vT;e zRyBi8IaAe5#ZRn3hV*`fIn;o7b8K=q{U5Sa1LqiE-(Lb(*6_dc-DgOQsXjz`L%rY7 zdq0RiFQ#TZY}aV!lDIF`Wi3598P0oyT`#Ux9F%HZ^u9vzX^Lf4=c8Jhnn)Gr7{*s> zHb@PArkwca80*}`87h|FkG$^0cZG@Pe?W%H?ECbEyTBfsEx!1m^{`BvJPS+G)K3}KCAXDeqsgZ`!(EUIp-6Gx6(UL3z54T*7F^9r-<0F z5_vsjoLd(-G}XF@2S`h+CVo&pT71`5?Ekmqn7z5aO_RUp84JNv-|+id_Iw|{F^p~8 zht3>8Kg!vQ%V5hL_FS6nti9E{SJbz=d33pb9!8y&Y9I8ya3h$()~OmT#-+n0=zT87 zYz-T>ZH3?1$~udXLG|nXXzP(o3Nvfouj#-M=;Iv&}y*2h*%|7MKjZ>O`{0O$P%=CyJJf6#&F{N52YDmZts;=oE*L$^3 za46WI3UELbeq|4_XdYa`I_$hRuc<}Nd&b8qcMp#SHa|!#qxY-+jkWw_vA^>E;=U9+ zYrc(SL;8|j@SNR8vA^E;p?6>Ed-^~W1L4m+&R_mopVf2p!pt4^S#uVMWeE3eOV!td8;#)bA@e6RXC^=_<*8D34D>I%f?sTXe^ z@4`!s-D!N6W^{~6-^DqLH_PO6g~`9>4Q3zN1l9|KZMr#nUyt5zC7awW)r})k1>Bpb^#w-WLzunz%V{mbEL)7s{ihVnLEK4UG2A$ z2ld*hKIhE`zb3xxWjvDRSHtz&=i~y-9L~fRt}z>~dU(~+l+Heg&GKO{3rq$yXGS($ z^Q~0-Esiu{-goFl7JQZJZ^eIVhJ$D4g}3LvwhEl_gXKFUx7s`L8+y;0`~@{t*yLRP z_Sg8`fyjP=-ABLGyMgE_biq~F?-Kl61pW0H9+2;NVW#a5m|J}BKzxvEGO2f?=$**o zwbVZ@3_~n|9rEg7#L1`+T0POkjmY6mc2D_badfKx6#t`H?#glClabRaU>fDZCX<)A z)i9Fg)?ZR?BT=2lRsH*#TT~3)5`jGpkk-mwU5c)bqtG z2K!z;cbYRJ-=x{U-usD!B|gG$B;ik04>KRz*3+feMvRfDZsWv{nx4r}O(J}wCYmtdeanVDVMEz5l5a@oy@#Jiq}*R{98Yl>dKf&P|AC&kegps z3uN)pl#*LjA7Z?OdS-Qo;v)3!QN<<7cWd5}=9DTQt=?gADXPg4=2Sn3I05B(sYeSb zc2-{x^`6EVid&#&%3^=Tzw~z+zbD&8Ek{W8HmcLp`&q;Tc%P42ZG-xNm?uoEr`g3B zj_Mr5gXy<3OK00$5!Hc!&F!<|XzCyA-Mjj%x&rY4ipR;>57uw>u3q~7?0vhsCvsvn zGr9&&FH?MlV*l+Q&bPTAEougU5qe~)u0z+v%ze#x1J&nf2CVo8y?=z5!>-Y69Qvew z@6#d9%z`_B+esHj$oj4B1s%1|;zs5lzc)*#FTO=Rvf@${BQQ(IxB~Gf)VpVCKea!q`4NYs8n#i5qmc=(?+FZH_woMjIlIPt zH(VSxGMl)?xSEi-4!!$~970I@pt`shX1*HIJ>ev#(M7v=8Q@eFUyaMlTnS0-dmwKS{xm{K-h<_CU4^C)Xyq?SKTas z&$vGFCD_cxCS!W94Lo;9vtQH?qIYR&&!wm2w~gyl?pw2V#Y=0Z2etnud#cr;cQzzB zpE2VX`KOTLdhyKYMM&J8YD?H}tHY5lV_%rjJqql55g*Yaq_Y!O2}cvsxr+nWJAL&2 z9nGZ#t65Lq2>L6T#}N|mq`DexYe?LJ>T1*jD7jUirryt$Fi-C}+KX@c-rnV;-bQA{ zgj7!=?gd`V`cgFuSMSV~oNE@e>I`KwRU6N`ZO)H)V7;HK|CBqeMnyG&;uTffFP=?x zQ<~jPy*NFmqd8x%|5W{|niGxa!D1D-KzF9b5<+pd)v%v z#+2*Sd$?5BC;JI6WV)vB;R&rTRrja9&T8RQ2SqKP^}XtP`m>C$Qx6(;GE4PHn)d}Z zH7;8DAid8>^+Kv~Qm?BxTXD|xvKcR}8ARd$^u2gs>90QPEbvGFwx2)q?47aWImXrV zu3WRR`df#nSUDW%8*y$c%j=QbAg1hM*UXr)67D<|`%Vf&gLzhOPNr~$dZ{L6^dr7V!WVe& z>(M(ab&c>|o0Mc*v+4pp8>sy9cj$7Kqq&iFS2bTE9ZdcyI3t2g_H&}5)?2NDY9_~| zm%%Mm!Q*AJpB@*14THn*-iZLtHZDRrS;@I-ax`P1OUx1CQx9{#8;&yWLj6k0kqMV7 zf2W=uac*!|#9to%6n53jADyGPJM{{wZd^FFA~BYKd-+`T!RdSDF7;cb^9|Gor1+7ZXU_DJae8{+G4TNM?)73^qc29E)juVBDEYZOZYZ`&aj~u$i=F$< z?xX!tj6Y*W0X{#8byXsl9*)(l4CR8sZ(!zgU~!KlRjpvg`n?wSsTX5Ta67)m>$&K) zV83w)b6?ZDFg6l1db3imt=7A3z50ih?;Ifh-N^3~k+(0AVa-Kt494*KHC~w63_Q6L zem4w87nfCpPH)GTH3idtg)Oh(H@CtKZZ>X5vr@qu@RNE!PDj4i{GWZuZi1t}t@4Xo zh`rta z#j%(ksg_{wYoGJ?BCIcp&zgVby_>klWUqganK#zYr5e24O}81ZsQc@kJ1@*EHl9$i zTSek}`@8Ts_}tCJt6GD4KP%W%@4YkIKiptCEBj3SjD3!uxRpMl&FE$o`@930+lfEa z?B$<`2X|vb#Gz@H<39VWIxF!5-rU{q!53TEd!1=LXIlIY_9<+*R~(E!Yu3m7<7FlT zdiT@HcU-$(IDC4h$62Y4MRT82&!c)R@q4^emi2stZ!IzzQ}4#XE8g5)Z-%*dPt{_H zPgC5j+7JC!?@!V@q*SXzt{WWH#JC0ewOq|p(%hcfonM2UXQ#D;{UI5 z)w`iy8#p=qYzvctyhji4{LQc%t2rn2UEWHJn~O|;gPb+z+4CI9n|dh3n`w5mFu!_* z#-tzNp3S*WvGEeJ|MXU}FPhuz)&ESIUd@^k(8rZv^rLWqw>$J}+Zn0XL276h9%?c6St<{YUZtYX>k;qtE1Uy;y@~R zhtcE9w%|+h!ICS;O*P{j@_44#&@-J;K708QcD0G4{EK{R<&I`n8w0-Qeld29=2yqe zZDOBQ3#8x57i-SH@+so<)W4%zCUI`!)%CYz+m*-BJ0QddYMzgBAF{*h-;um2k0Z>j zbJQB>w+9zr<+^+@;A4F6&H2&1YxQDi&E!GBI$mujaSm&4U_68BHdX(o->QB_c{lNa zn(wD;v^Kr_Tl}!zvF^PaQSV_7AO{)LM5URZtsxhte5kOK>T~GZ=idSTcLF{)0lm;W zm!H5kcyn&G|GfXz&Q#~6*=xF99HZs{=spLpJkRHz_Pu5#s!l*Ydi|COoTHbU5Lcr) zZ1TVAJC=@=&i)*o%?E?|ZLid)&+G%kHtN$C2cmtGY$z@k{?W`s#i8PGl%G@UIq&>D^Mi134rfK)9WEEQb^eC}vTPSaGzt z5oQ&Kl+)BT^r+eQnj1qNHuONy(ac`ucQkKS@vP<%kprft zLiqnGdi7_EWfW6rzJ%s~s#ae$HoQ9pd-{#IX1l-aHgP4jJsB>zz<#UvTKl#u=^fr} zG~94FGsA<_VY$LBiboXhC{}`-{Cx%~?|!m+1N8!Eev8gRy;{VxrHTy|Uo=fFu(};R zqv}93|7Qg8o#u_GE~p!|D9m)VxjMRDvrm7EmEe5fb_Sw*M=g$1oT%DnaR|Cb>mes; zvs^SE11_sfvthm2t&^;-M(0ReZ!<_0cZfSuY%5)+zQ|%3)!d@%_UPA4`r zen9WtP;OSwOPztE_pW&SRS$HbkF9fRZFg|p%OTB>QN0TF^2Q~qca@r}koYTcP4L0S z6=>!RIdEG8^F#3q*>;U$3h9gLTP}~w#6R3;x~bZaIYGZYmw0>l>5%5$sFp@Eti;2J z!vXKxeN=~oJlk*eepup(km^9hy)4=I|3a$w&^ocBq2FqFsH5V?EjNe9&n#*A|G$LmK^536@B z>0pL5*#-1yfbFg7yEHRa^$6ks)eET}E6J+zwwj3~UP$%f>W30PHfj28_}PQ#ct681 z>Tl9JxHaoZbt&|g!Ba(nJ^oJpPj`IPapY(N?+x$6-#U-X|94QcTr_h)?;um{hsQr0 zu-+fd1IMnytE8G<^B!^ZT3m$cPHW4$#Pl$mhV9~Sbt6WtLpB%EC)FJc^P~JR+{hp> zlirDWldVlSiul{sbL8nC@VhIlr&2m4el>QjH}6GUadLsrifct)usPR> zS+%kg7Ok5EH)t}oLeayoau4Qd!;jhFqyn}q7u^1L^ z4*#R~lX^3aRZp%yXw^BZKSD9`z9$!wV>)f|>Ebv3hU|EAYiE}Jll6G>5|K-MS99Yf zls8dblYIKFq-Wp?yty=bC$f5fym>BC{;c9L} zW;6q&Jy_^=Y}#&QxdQ(syXU!n?i7n73@nIvBGw{M*NTFggoL?pF>};*+%wC z{Wa=GQyi+;p&z`l*Z-or;$2DJdyO|!-=Y{taj5uqU9Wx{&4Fi}Jn>BhxO0%jFI{5x znJuM$9$DQ??Ls=Xx*U5@fejSa`T~se|1tF*U{;pr+BQu@KqQDr@4W~}6Jdr0_FyzI z{>Hf7d~CO$pTD0;Oe~2eCSenT1dSoa*t^1j2;+zY!py7%GsDoP&_S9bA|f_yh=Ts- zc|Re?zmLOXuMF>c*ISu$R)Bm-lk{nBg4ra>l+DXWKbX#6AeQQs&@o z7j0%=pMpJ4dbZY8sjsmgtg-f{R#F(B&HX<=v5;%4A%1H@{8`4{sNs6Xkgt{S4%9{I z$G%Q8r>u*uT=zQrq$YRwp{DrUDxP(1ykB~o_`3mnHs_vY_GIMp zCue-GT=!q>>s8TTpca9;BJNFd3g_?G``}t-mNk2COJE&x6`Z>(&Z3@er=eSe%W?Ub zonWx5u+tUzpw+DD4e{Cjh0Q+Z)Y$Uaaoc>##511?JFe*G{g#abQQYZ{5f5 zzE5oFyq<0B&3ExP^H@u7N6kVGj2s(rMRBLwZ{CTGdk?!=1fE;NdwmbPS%BTCjo8@z zSbxuMY}|X`#mmXx_hVC;9}cI+^|um)SPpFFM-|0EagJ^adI_UeBb@8Cb2+ww>J(JHQ? zkoWsCxYyC>!8CvGM;>7h&Ys?L$ukb~eDqVZiC>Ne-m7jf+AhR#g}F&nA0`fdKk(Qe zKJaVq?<;=eRs7(SJe%E-;}6~QC-CO?!zY?YdyLyz^=!I~)Jm%u!AG2oP#BU|v z<+e2O3td@ zPSVw(zl;6co%kWGXXdGf-;pPzrj7Pr{*b7fp`&-T!a{h=|lU$BZ=6xA_6>8Pwh~JC&N)0-(AkX7mNZ;w)Cz3Xcdcxul zcOOy{FE3&hdN?EYGtRM3*}uynX*B4Oz*U^5aQ4ohvoG^A_G;Njy#J*^5mU);aZZG| zjdK&6DdzcE$E<-~am-ggeu({=_#|GBbBK&#)O7H8XERxcBFBrkNIfX)U&f!qn_bRt zjfmfM9jagInwj?Se-q25bDDFSkI+AeExDbVZMTS*zTa{Zeq%sKI!yB^J)s9-T})^MN@mxyfkua(Unny><-` z|D_g+I>6d$>WOF_jKXHFjryj?i%U}jEwx{XH5&Wf^I!_n~>>9s&0VzWr6pTKTzzdR-hZ_F^A_wv^nsOg@0z zB>DN`483-Jk6d9-3A~fQ7O*d+Xhf02oK8G*cGQ2Z2eF4j|I>TtorX5Dec50{(Z?ib z$vH~)5Q#a9V~8&mJ#}{Exz=sP#!qr!2B=<#ybJ&C^AQv9 zyS(?%tdQf)^C_{u@0uasi`?IwPQ1uo8~Gb~&3_X3fP5IRo;+*)_6E|s8GI6XBi6XZ zKsxz8&Ir0FjH{72u7UA{4fzP+kNK6B5u)r4lE_ruwPP|U^`q_)= z`sH$XzsC7$iOX%!KFGW9cN4GUzQ9kKE$j_`|5y4=db0Mui`<`jFYEtXp8aEBeLabj zPk_078acjw?qY88O#Hi;oBGM>4mm#^J!9gH6ZpP{S@{*tx0RRTIh{S^TxqV0xX3<{ z_25fmSsQZKo1)Xwix}}Zd*Oeg&&Ya#8YZ8g=*=@ZCKgdjUsvS#^t;7guqMbJFANm?PvraTk?6(q?`j`8E7>}l+$1?I_GnK%P!~9%Gc~{v#)GfKeW~N^Ua(#! zH&*QT()ymf_aWFtXB>B-hBy?T>KyI%nOPJ67HiMmKzo9G=4!%_k4CSMwds@g#D2Zm zxBj0vMvfV^_rQeZv8Zn&rev>f{XJL5G2%ANz=_XdgKA!ROwy&9loeBQHZ+?#@I7%w zIGAyaF&nz7*@Q|%GL3B?0sO`+FtsJ!zRm1apsqJmwid_} zHt|bVJl6Bv(E2x7^}YRF>T2;>o(wo+R?I}bbTrtq@;$WeJpZyZbL~TGAz5%n;s*On z>tf!Pxw(CyaH+@>xA{E()u;9ze*HOcq+#Sx>%%s>p6y*yUrSs|&Oewcy{P9B)2`u~ zkD=o@!?XjXk%3MqN#O|N2z1^-_gek_CqZ76#bzi z$a^b^-wurX>c{DL@_6k3Xl&6s^0IrdCu6_{t6Zyj+B)%PY+rC%j8}=XVlI_`2Y(BE z*uSIQov*F)8puzA3+LadH{|OpeQwU5a=rQd@vniwx;E&~CFZ*vT;P3t`MLOs%W|$O zF=4gqjUkO^)YP@l$X;o1(11M6-uwsA2r&#d5<7>~izV=au1W9;!k zFDEc1XAH^5GyZA5eR^PA*1U{!>ROyv5e3nNt9Q=?{6PCQL*D0}OiF)Du2qfqVQ7UW^hx2hqAIgRzQl=yT9 zv0_WsX8}HVZ^Xbij++-ap1#a=Za-DY9gV?^3s~!KhVNLiW@*8 zP;K11v#oa>lP9Cr;tHKKsAvUlYFY~k~kc&Fl8a*FJEab8Kc8RhXV zW*lCL{aOm{W#8W_uDO_fybElwitiRi9>;SGJb^x5)?;O?TQO?-jfgLj{SlYbo)B*k zPp!uO7jnG=;FYOAoG>&w8@?a;Gxqn1qsVcBuZe9*dAjv|v0vAX9CiAn0)M-ALKog$ z!UolZzqoh>&(dQ)2V*=rd-^o&K`y?FMjFpxRp3|lDY!oGov@aeEoo(czvXJMQ2GDM zV*QAJ-Z*YWd{+C^_4f3~wd9s9$?uo4pEg8HDBk7TLlYBhc_|o+m{r%nlihn=t{cqn zs;9k+_c9Dzw1BZMlT+RlQqoUUCN59+8{r4t@ToCV3{dO^4a3gY=9_FEeDTzhc z*J?pe}&c7CFLI%8&SWlweCI@NgJ4}R5ybC>hmHJpC{ zzgwJ96WczK>mOVYYfpW)XFJXV``(UE@5DY${g~>C*lQ-I>!zE!#&xO_rB4~RuPWB6 z^Y5H7Egyz{Xt3(Vac!-JbmW>VxQ1G;UrmF=-SU31r}1~!L(^$V-=<}wUR;lT#9diy z$B3&JWw2YUt9QfB+jnXHsNbP(N<4KC?7+Fw*Hgx3ljSk+c1TxUV8GYHF7317ny)JfHuehP5MpS01LhxflpqjbJie z0uQlw+h;C@=BzHYwq{KfbC?dU+&Qq0IUg=GCL-6fb~Xne-45GQnA6{>ttVDCyxEh1 z5$SjAakgIIIi1nzbMtpEE`EXgN_-x9I75m~j~IhK3S+m3RrKjzkC=>{p9fc+5^+N9 z(c3xZ^oVnu9pLdNe=BLQsLOX%@N?AGYcst&_jp#|A-kA`E%)Xm7iH+4mhsv5qIU7O zTb8gMy9B;uUC)`J&LGgAI%gA{E_^*TH1e~KtVek@cM$JSi1&WojsL;>IhS}rpMC+@ ze_z&=@9P|%eaC;}S7JWCFST*x28g+Bf8mG3r0TFOX5UUHe&~-~wV&vA@XXHGlLsQF z5R;@n8=uwaqjncL2>b}_z=M&SJJVB*8haenfix$zZtI*)^4zSsjQOB-e2-Bhi`Xiw z_Cm^oM#eGbR^Wn>2bo)ug9q2e&v5r69~1+j?}Hp6~?Fa?W#^$9+NEFC3_{OY0})Nj`(! zO`Hw+JYo*of4MhFPxW;6`>e>p?bBp-eb#lTj-Q&hazeB%>dN|TonJ$ZioTw4fq^*h z30)cTzOS)IUj#m*2Ay~g*co=`Q(~l5fl1n@3BDe=ygDSzgJaDPN5X(|0- ztn<&viN8lb+63OqF>;f;z=|(s4V=I}JW$Fyi2PT~%9-8nFEvbjp7Jf_l7Xv%l^qgi z2rS5TuKto(hdGWq+j5+I4!&PM8_$qiV(-*#K9zW^FR|LC!~+f4BLiY>_^hlk$-~qp z+g~Yf^XZQ#5HA(7_7`Fg&fz@rLZ84-os)U==SAeFFY%1W`lN6Y8`A95G_^~{T}phhq#{Mz?h-he$%bDY((88Ko|_?JFSCt@emi!NeK z^#E^{*Oclh+HLD8@)P9A=og&npiR&(JlpXM&VOcn<{e>^l5VQAAr>5I6h6hhEkD!# z=r+@*5?{B#Z!C7dfk!m?*D+Ue{cX91RLfQmaOFDVuCc`FodRcgw&O4G<89;Gti@b7 zyE8GNwVRn>RA+_1vll}>6Mb%v!skP~&v?L^oqZE>dM~YCKy1)7V_vC-o;nHQ4`7Q? z12XpC_WGkMAD*Ch5Ui*Kl5B)#NkSr{WdY zPcZNH-Kpm<_s_o@CujrBRn-mF-}!gBwQ5tCpUSbZCq-PM`#B>`XCckRgIm8Cru+7eAv6r(va#;JI zjVsL`oYA1BrTi1)0kk}b)hoHrVZ;NQBbS6L9P#{s>(}%B0j#%D@{~&cZ7}cA-hFG| zqgl7xSd**B=O<+3(oa3GB{ym65V#oTs%yBPL9B~h9M9}AZAj!Y@=2`GPg%Z^?+zu# zdjlN3GVHb-e0wp}PB%wbaO6$w#t34frCdXG z)0yO71sSzv&1L+qzh#erHT3N->?RjJ1D`mL7_pvMwjq9VVdT8}f@NKk{;HZn<}439 zvI&3JJbsHg$q@sH`N-8*7l?Va>`!Msi;2Aw`HniJy>`uKPs+ppoHbfP{@_fUA-tC~ zOV|FF-?lC0eV7-w)Sj?^YFK~A@KKe-YR=UjMqZLMWbC2HYo3kwq2?j{M%ILyGbdQz zNkhgyW%x(D%Ua$|L;l^p{3CdL<&iI_rR>}&W7n}u?u)+@pWxq|#k>|fS`+Ub{2n{9 zJ=WyZ19kX?TKsJV`RxYslhv%d3h)T$&TJ${sNj8X0|*_|0^f1=cOBQcl4Ftv%XV~<;s3DCce?I_Qz0%Rug3M+ve9xxYRE11%t~+8!~$4Xx8`8tdaX&q?c9T-yCIXy%(y2f6d1lE`gcKiFhq z!Rq*~eS&j7tmEE3V68YGZBP8}&2uVat!m$Hzxj>efw)%9N!1*-?jVOmdqlrJ{%KX< zHuf6BJ;V1nAMG<@y@hej=C)0)S%W*Cu|YS1Tdu`lF2)W_XYI?EDCE5?0OM~E`R2iI)Oc^e z{b`?H#3!1|yh^+<3!X_j#=a8y_U5Dywf_e>^r!LJnBn9x#tCRN23~4jW}c#7Q}^57 z(!Z!BYo4xeYT0Nu&-I<~S!#fLyuR<2l9A+e4I)QYYC4?|%co z)|s`LW@J0RPfSdo>+C`~UHTy7A&>WZdhPmYV7C6sqJ5_@c2pId*k7Jf)S-zn>*%In4_zQTWb5f5lvjJM>kx;CgMkq^-sXPa8#i%e9}zdQ4ciah-9F_a&dfSV-=TJ$>RE#G~x{(^y~P^B3c5qw{sU#^I{xUPqtHS<`H!vaGf?ho#P>tV^7e}cUX#Saz| zv;T_scpleM$hv!$y`JXsh;I{DMT{q&pca9#4S6QUCN#Hn#c0 zEx#kS>4UAdNA*(H#yN4kF_pDo;}EqntxbrZ%Qfn9UGI#!xw%s6Uznbi3t>+^vyvk3 z9*1@)F=p1>#~LDfx>@r(xw!NOW#wACujS-h3sZMgO-pN1_E2zbXn>5s=2Wsi{*LD7 zNZxf7c<4XLi`5m}09MsEYHIom)+Tkp^N1npxX;ya5=LnEBagP9-8t{}Yd9xbO)B4! zd?EXQBY#p1N?jf2kJvAB`IrvmYcsRvYd5s+&iZ;F>Y+Pp`Vl+rj@)0~ zJF!*vmw8w59D3;Ym<2qL`#R1$`y0La{jjAU;ZN6wuA|%!G=rJh+84iRKgewGgg%k8 zz%eM>cV(Z*N9>UqQZ$NK$2GQ^yrw@f#wV9Zy(HceihF_ zj=gpXOeMG+>SK37Bc<$_o^fsFO+H&=dvzhKC(6N~h8MGpy)W{e@y=S+e)2x4nf8nC z%8jQU#Ctq0mnL`}@}fCU)X?Q@t3}YEN#C5jMCSxqqqN^#Jppl^)EEE1gRAdsCUXvJ z!D60rJiH!zX|1zc12wi0TaZhy_8|S-k(2hL+{Uye>@(*5w><$f3s!ym|#`(Ymk`EPJAoJTAkB0gXr z8TU0_-iX```u>Ca;W^2ff}7!I`b|RL!`=|JN5r^1Kl&=6P3mX3L-ARi6ZJ^NyTl)y zOCqMBCW(9luix|7>*2LMSuKW3l=l6uU|frxPi~jdCklS&6x{}5BPWE zQZ;qWo$bx!-odF7XG45IZo}8V??`+*IXEHa%dS1wfcSu18u=ATW9Q7!7~#1R%l#$r zx8w8Getd~FHXNJKl$>QP{bh-Fp{9gfCg-}U=X34Y@z|O(xR*ym=h-!2&xP2Cy$KD# zrp7TV$G!%!FS#XRjM$k{`%}Cxb!)wTeI@I2oIDVZ4u9u7jdZXw6G^9dkocek;^e|-!H;uiU*=?n-xRwczjpz;u7!T?yMXt zxfyauT*K;?xaQS}w$3G=M6QMBAqL5c*Qwn{T#%L9pzS4R&C1#E7`bFye=|HMKipYn z&Owtyj*cT-opZ^roT2%H&=N7;*1m}u$`So#UUT?qBe4mIBZ0r-+G^xHfccCdhpGfq z=*A4A(LA5^_^RKa-89;Kj~FQFiWtw+%Z85AX!d3m&vJggd_Q?{#Qpis4|2vmwd&wb z_;>V;iSv?X2i-xzy|jw*0pzdHaOGi?0xmh{tm9Jw(zSQ z^EkG@SHzR7QR4TZ;os};xq}$beBXF%8raU+5!ca!%Jb+MG3T{o{}KLOUoSt?8m^dw z{RMIz{aw$au0o0r{}i~Bm;-x~HPwRLH_b=3h9cGkKZ$qSJwB_AX6}FYp(1Qp_o&7A zU9e_yi0%=S!1)2685HO7oryWv=dPxMy@%uWy^Q@b&mB$-p8C_hezhE}&o1ja3P1QN z@4(N-Jf-|z4g2xD4814BX|?1|{o@_kvye@nO|5l zdT`ZC;4m0lnm<~@ayIJ~H@p>fVPi|L z+2gmra2soQZ^YjCGWO-V_-stBcBt#YxX!&SUIgyS-r7wJx&{#*Q6eNL{vdJEzmV%^rD)dzkOZ5Df)oVBVwWWPinDt>-O3a^8XScN(%zU&qHd6I{*PSL6K7ZU*m&chGEM6aKv{a2tCc zS`Aqq`^y*`U0m#14fiiDQI1Wl!RM|A57|ZzC?>cO+_IXzoNyI;zr;Q0Q6qmZ3H(9a zMt%Ysf06UL&S&4MuIc*VIS@~i6Q`a6`etL@nls8>*N=#i+Dpm$!mpKJ>w2j*3Xz5`Ot7Bc+L#KX- z92jE{bSGkM44P^0z^XX@lX+b;t`~6?b1|_O`boeHk4F!Y|8{Y43!d@v*gM8o;B4{v z+OdOU9oqk+9=Nz=kHU4V*DkT%>}8?`6@RZix}EiNCjMk8du)V^*p!&0+?(Rh zc4{|SYX!s>^;}vrK|)Qn16B4})3D@v(Lxo!urH?D=Z;R9}v9hHrb;RN|!3S4~~80sA7&p))VDu0bC-wz)yX1LO?UZPdw6+86eq z`PtgKcGA33%ttK+V~J5yuESRECeK{VbNLgw&8PUw#ToGg?X$HH^Lpouo7aO|kt6m9 zzpNgVTA==2O)%qU_lO$o)VlR|#I{B3={7t=d2SDb&2+$b74n>ZPHfOEVgTp+*pDp! z1AmuucEH!^BYqg?*Iy80QGZQydBl9wV9~$8{be7w$DXJ$;+&5jah|q|9^o2J3rxTs znXyY+M~opi$+$&qf!L9lu32CR);{c=5l1TjqCfsLX_G@)BcE+!^iDRs>sWGd+manMa*SDgPf&CjDf47&%ev`|`yb}3jm+Lx5{5JK#ZD0n@ zR89Rm_O|%0odMmUN!R$he%ZQ%@BNc`H$@z1jHf;fu}g-&=ZMwCD%D-I?riObSTAC4 z=ND8>PRGb)rS4v4{Q2#c2E=mD!i^u!-q{8AyCd>(aXIp(GV|h2Lr-C!-XC*0%~|LV z3%x;knDl|hca1CTsW;{qccOk+X1>s9&{2HEtTN}?7>CG(w!Y~a#`i~0y|^VcwleF~ z)~bx*)$XwO1T2a?Yi!PWG-`2Ny+4ojAusV~rPf2Ox7v^ITvT;>$+PHNnGm1p9VjE7 zB{%sNUv_6`S;$XN+tWI)xCb12YOnb`hp)(Mt_(~=UKufep7Vdr*T62u%N4(TOhatr zZ1T9#tmBXTJHIt1m*xw+l;<((ija@M`5%veKh9d57WG+ch}fSJ@1-9-dT8WCU%&GX zi1nk+E+><^5o^$#w{gZk6mu(M9^+GM*7j|=-hH;#2T#r{Rtrv!y_i6wK|_t{vM1+~ zh(D;=SaQcW-u2Yr1~?<8=&38Sa?#}mpidBM#vb9sx4ndYat`ZuAkWlXei(M?T>0z7 zd~${du%|C09vGG}2Nahu9xi+4FIoFs#6s1dQRBk8tvy(t$Np|H6Yo)HK()U`AOn8Tn4=A(g>R zVr?g$0D2hs>`#eTr^VbQ*O;?usNu%iqn9c2dSgQSo78O(Lr`OcS`+cod*B>};0D8p zw+>)W8)Vheu|`J!2>nob#Ae?Ri(dz~e?m^4gX{32_Jes}dwB+R^p8x$jvRGOmfA0% zelHv>X7A?59s#$By1(2Qakje;orBNXgY9^XSx)EjF5V+IA0BI79Tj^GoTcmBTlFQ} z7wR|>Czsy&PzFr=3*Q?YiHpH5j>1X!&o~A>U9knUg~hTOU_*|U-aFx zVr%B+K12IQJl?)$ig$+lWm1o>Pe1vmmt2c~{ zTu+@q^DMD_H3rojSh?;E_)uNRafb3)e)SMyyb;8D| z4D@)p0qR$4=jH9&M<$+c&w{a{=T{F({=Yl~bvW&P^B8o9IHp5x@n^CRDdJPmvQ z?E_UC+&Y)OekVQ1<}6|Y`oU3CnuAp=;rMRUN_O!Z)(iTxt`6fzQeD9J=$4Vrpl%@c${CgA{v`1>s-gLyw&_$_BrjAp;2+R$}3_9FKxi{rUg-fhyTWN*iB4Y zn==kyvlLryKYzc7v3DI?O%1Ux-(3~w_d1<3X^!CBE9bZFtl5<_PO!#`CIQ&g0M_hs z@h#${0dZ~eCBVxfe^kFwUYR-U#l`EfjcS^eVcROPIYT1;fAO}hT>m*>OG|kkt62vF z*^jT|hj$a_4~t{=zf|k~iR)Z+$xiI>FmjCo@b0~Q?vD*m`WSLtjqTOMkpJu~PV4AN zC$NB+b1!yA?#;`@Kyn6pVh85q|LVx2&m^8L;9OgHF7^mmhuy(@an{Le5m(EFmU}EF z<24lRx2?ftx^WC?rBIR(wMKmYoyPI7tF zQtfMQwlK%DGxv1HP-*-wy4u9OZ{Rb`Z9liR%6qCI2kAoWU&Vda@(xbN50>*@Yjf6x zo#nf+XL%0a#a`XXeV6CNf1KGOwjlSu_GneS-&qr#(dj%@^*lF#zpRU6P9nRh3|3N$*093V+v_?S~JnkH5>$ zVHOX*MZ9Grwl5pU*vDtx-|t!jQt#q}AFW}HZN!GknW9147Ei`W;H-FFk$ zwaQpeU;o@5a>MViq4Q&Hh=Kp?;5_1q`iRG-J^XUu5575J%Nob!FD@WMV@UTIaf97_BijT0Kc-4b?c0H^V2=Nzi)UKl^Jmfv6s(J zEW^H3vsW9kHeQME<}t(FT=mQTTZnT$qrSf?j(7Gn`m5|0xh}`ak&=#tb$t3au&347 zuTy!>+2G+2Yh=V2#b?y+yLRlF*e~{XPnud9Ij_%+^?^U|dcFfgGuZ{oYwi>d2`P1cX=FA|5xW3G+!_`CN`cnSZqJ8JRpK?AGwcxOO}*XfVo4DxOp zhktLr{UQ9zZv2fn^F4uo`k6WqK2#l&*RZ3V@O{o3RiiTbEOXU6AG(%3aVUI>I#eF- zF=ET=NQgb^)8!kBIqQ$mF2OG*Z7Y2Z`mtf#Z)iOXpH)iySiyc9%=*ldOR66`KJ4m0 zFQ{Ul4~q4#9Zq%dh4{r$;8>}4Z*S!D7q!FwE#mtv*dHxe7seackQ=1_M<4+3$abug zMG-?^JGNzfw)Q56^CXwvK0f0F`}?eQnv<#@rEZmRi!}oIB(8xgZgBQ)`#4?=2j?T3 zSE@&`v8OzoGRSXW!>XF66zXU%N%AH<6TGR6jSJjg#H*EW_jR`hqBDNo*> z@ts-|5r_A9YRTkC{o^{#EBw3If|?K1_J}uAUjub(aN}87>;B@R^8d|ojFT(7|1oP$ zXRP8}18b4)cQB0L1BgSS>lu6l^HyTCI7V*&EhVWBTHYhEc^so&J@uTd`HpolVwSA6 zd$|YnxMa=8JtsbhymBTsycMy^qh-vE!KatIXUH8cV^4lyUP*87O#E^yeAXRh)MnYA z(Zd@&1U2yGT^olRJ5j@?e|aXqYY*Ts_{pP*!&kGHejEM5YPrDuqwb%N-P^;u-wC&Q zlyLyRJ2sA&+d%%yT*yiIEcvA$25$kokKg%8X#U&JXg+T39(<7Z_Z+F}RkTD|H zk!o$`&h`;l`*4n`^)-8k-6Q6!l4e^poz^EO=foKe z#C?%VS{H^Zkdyal|26s?;XAA^j~LM_az<>)3TjhvemUVqPxT{a`4ixey10Bp1t#3+2i+R)!tP94ZJWbcl|(J(yLSpLX8XhezMm6)o0uM zgGupOO$qRg$mP>4)K0;Xkhg#~eOCR4R2MrOYe_9ldI&@7RldZmiO;ex(lK)8CnG*s2!fJ+lI zWPMijBIWyR9ATw`qS2{6!Wz-oFE|9^&kcsMj?c z94~i*96DyRs$)-mzr;Q3@2ax_-_5s>uKU#%!ih)B)F1 zYr8QxEzZ|dV?;Y|&#bdfc%Q^EiHATx1h#Q4{F7YZKg^aRZWx0m%oyzF8seWrVXN)K zm0JSuhG$p_u61kJeESsCDw+21OrBv2;)Hf#OU04IytS+Lk{EB$qd@#7ALbP>h|xUD z4#bAm6n;ugpi{&cymxGTpNK2OrPWsve{i0xTzx+u{O1297VZ<@bw-Jpq~}o++nzpS z6@5Che#xK2J$}mlImg*q0Ds~A45MC=bhFj2F|Ko-h`e#*z(F(T;wRfii~%l;Pw&RN zur~J~@8+~PUVSt9lm1-}gKOS6p+l3o+{>UiM(s^|`qWGiOaJP}XGV;{o&g`|8ZpC7 zH~o`!JrK;(xfMSRoX1*#n9QgtZQyGxW-s1FtlP;t5%2KN+)Mw!7Ql+R$5Z3GSMR?F zJNrGJK{}7Qur&^~OpQsL6$^)veUOf^UhuPnog#*@N5&f2i?_W&EZvRw(j$&{M!1}O zISJEq&I%n*d{uxQ*+$Gdl=t~c2t8;sfkgEzdO7FZ983Wz}b>n*0-D`^2kf|KdO1 zjx`4E45qjFU$v^c*#{NGc8jo^9l*8HJQVv%Jl;6gz9IDHHegPa^j^Jx zE7z2?F3m$LyKlgrnX`Y*+S-;qQ<6c-ny;*ar8K6)M^k{K|>*M19d{^--&(kz$1IHgR5g-5K}~sFQzp0 zK#nz9$n%>=&(x;KkvR80zh!M!Mh+`i!9AGr;_Kpg?Vfsi?mP1%{4zX_3V2=WruM)$)?k}92A;FvNORs} zS_AT6zul7N>DZ&EU2#o1ul(0#Re>*w+kCny%|nr+t5$>SN8Z|)S>h$mNg0b@C?iIw zAXjXHolG-`>+i9jF5yV>x?R`erErURe(HX##-BE(*0YQ?SizqB9`7y9LoseI|Dq(; zwmK5_k9_bWwMI`3tQ8Imd&Zdy>Zz^+Bj`!Ywv<@AinTW^j&a_x@sIq60oU7O(gBRl zHMtp_D&el4(;h1^tDEQSueN;IQVrAGl)?t7A!dm=CHJIBN?0mwwh+n{^ z3d2XpwQwJ~FVrpui;U}0bNJ4OmW7WHXI39od<9aX3|Ts7uuSsyajoyNxQ?E$eFD9x7lB*4x%=upVjtp zzw1Zj?>Q^M7{k0#j;L6N^$}~PVjyxU)xGo>F&1ZV8=ny41rDa>s`AAFs-*30}>tH9!%l}4`2npw&IS0qy2YtPLd4p!2!&+VhR&{>FJowhOU_`yyf3vgJjt}*1$nX3u@=be!s2K*{qU~CH*I%>dP(EAn0r}6yf!2cU zZ8!dB4j}Uu&%;L?Bo2EfG#K@3a*ye0#hy4*drO&p+2%py-EfJ`6OR!){)Ik^q3omm z6V!QQeS$-M%JbUH9FWoBqv2!6@l%!`b{HEz3;uaO*4aU^ni4tQ*30}ZHBWrj`^NlmIr4}_j^k5) z8a*K5HvDerBv`kFCtdpBsyuSH6XtQjRS@qb=PkvSgGC<&%c?7-zJ#5BkN0;X_!a6) zq&|i7SeM_3|3sbz51RLz`pxZOx1U1YRQsOX>vF<9#x){eU5$FR5yXz{#a5%k+*Umw zdh@gDB*0n0-~J_gQhyR{t>7W(Yt-(r56GOI+&%CLb8T|wto#J^=h1lxtk2_Fukl?m z1iA6{2k9fMU%NK!)y1Dh-Ys8Ro`(6axTSM*5{59GbN)H_737d{&GIgoX@PAXLG48S z%;ABtB)-Jp_$@IyaY*?dzAtrdJ*WDI)}fqhET6~ueZ&G;^HAqj*jr&gg>`2**YO-Y zUX2Lx3TrNET8b-*-@-o({XnsLIX~3>a`wT2Zw9Vt><89FZ^C$Rzb|5~%D4ag#5>r6 z?uy(7xjn8u zc|?mY8O}R;i`?vBaEGjYh#B&n$kX3t507LoTolK8T z38&-Su>a_&a-Ip{U(C7GFf>m`rbf=+t;cLu=!CQGg@)OQw4GtxiD9Wh|tsaG1dDeIE zAKbUI+VhCFzx+2o+o4gBr2EV^CuJgBTuOOCtC%!B1RNvw8{f6I6{(DNiQ*FKVN8hzM z@`598vwqWjPv4iI&F<-Wz+BYU{=r za{2wc{9AkBt<%}l@nl5_d3t4T{c~N28LA==6<2w(BJnrGqvUT`Beyrld7WxU^eF6^ zu~u%*F0PVv4ObD=RSSPE9M=FVv`(g9zA}5kZM~s0#*~NEz z<7bzWC)V56^Iu($HW;M z<{-{tQJa9;Lk3+1^1eObx&w)Aml7}3V$=F!x0iD7JAJ0aP^q@AMx4BEVrpWNcH}0` zS=d6J(iJQ;aWKTSoD13Hn!T(obpT$D9J%&rC!S#eIbkh0*Xj7a0*>1Pj@FP^w*ddN z6Webm+!uRWp7!@LZKf}Y z^EiJ8y_)#D_z!&m;6Eu3n%OSpO71W8%p-)%r9( z^t1go;x=;rt2fl~-B$4)neQ6!T`WbdPH~%7Lz1?kz2x?xJb%%f+`pW|d0-|7b2pAV zH3MIZzt?atjWc4H&eoON;JW&B(;l8@6LRY1_y#p*6Yowfg@v6PMK0W+f&0_=*4*~3 z@b7IGC9aNfBYPyitKD!d`CVg@SreTDvMpyVPfc3a+mjV)T%5#q8j&9-j7a-7?cply zNHyngk)f9))`sic-Yzj!>l9!Wv6kePmp!vQ)}HqwcgVHsJ({aJgL2x#2_sd{7A;-s zo93-o^G?dcN27&}pW22$Tg;xhgEdtVxaCK`Ta0a&TT)1T_e@~R@-c{qc;|_Chjw;w z6!qQI9fM!zF++;(#b%z$bJqVp4-WY$ep)X5EY?wj@Woz_{KgAs7qX5D@W1=RZkuna z!ywjV-YP#_E+6&PxE}v5Z_vG_ZCAHYtku7p$Mr*7KyIEqy;&2R#c!GC8h@Hs>VJ)e zJg4<~d%FEDTy6eVA6!0v%Xmf~q_*?+7v91@y~Z_kC3pXneUb2UwT#RkJrDJy@Wsp@ zAm-|buUW)8xDr2OZ^$dy>+|C8_F9U)%k$TFt4}R&#PgW*p1regd=~$=-r;*xYeH_P zafP}@)L2+=i?K5^c|NH(v-P!iwk^EcVb;xyY&16iJb;!tY?`Q8@sMtH!W*D(SCohjo>r5 z{^{+@+5>K%uf4!>v*pQ?=S6-kkAnOvYb>qCg87%+Y3BQReOWo-a__0@WUcSZTVPIG z*4R}|zv~}tlrX^pWa->-g_}2~Kx^qJ3P#vv5Q0q5$vz7<9ocjJq{MTBrm?_lk zE(m{QKe)3V^tVSV;-nHSYpZii;EKn1O0@^ zHT*ku$^7QqCu8&9#}@o5Kh-V|`&;BQ)}7yD9oI!|THJtmKfY_vGI2oU=5j4AF8(Y( z^#99*(X}y4!WoXfBl;M)cXepX zO38uZcb}~2UPfOU_WHYcU*@6Yg7~b#rRMTt&h*BGPT-W~9k{ksUzQuDO%fj%yQDil zaWHX)_3rM}dd?3VgBd5px_z^IdMDlm`9^Rp{JZO4O-Q*J^mU;%Raud+u%|6pgPl_t%H^D^GM z^>J|+eAqbGynGMxQk$Li8nof|%edy%$VGD~>wTdK>iOw! z>d)WRkSs%|u^;>EIQ#wS;P!|!gO9;sNIVCR|3#ZYeAhXimj<7~`q9ZX=4WbiXnOSL zI={l#J{mK0^uN9rc^Aeu>N2j@j)D@gu4ULKJ>q~!F0MaZLOt*)@IB4@J;+7V~(Y?pCAij!EE5xaA3+t;r>6z7z~tOgD}K*X$T0?%PS zE$?&<=RW{XX%w-geZ-fs-o}`lMXeS)#rtaqUbl#t^-AIvHCtXGCdmB0B*7WU}HJA5y?JF@hV-7L-`>61t&tEi`IId~cnWnUM7J8a~dI$$T4@cwGT{t&AWOIf!a5bseQ zg7#CMwz-?Q&~Mu=CuVc@ft$^Db+1*5;@`vSw<@-MU4-X${o5c)nBn zO`?r+rz#o=_S2STSBrZXtK?jK|DtT|>ET8*(fmhfccWHfb4vj0~w1dP& zCHN3AkdMhLtKxUXew|feo@?)(`Rom?x3ES(;@K6mZr9|%WqF>7d!T-lT!z$}(2)GP znCG?|`|~-^zZl=Oi5&JbV%ep{p{rRlM|c*i$X_eMp2N52_jiT8O|vZ3sQ-fJ^F~~= zT#H@Dip{5U_6eF7JJ;4@>_;#c7AsS$K`cdHoHf=LZ>z_qeTf}*Hs$t+hwUk#2aD&v zjpsa{wNb2|A zx3bu1HBwf>XR3cZr1`_sU zec+k$$Cxb^dnqbiR5{g&%uzt{)o{8)7_t<{SEh{dSg&Ay8BsGls)LERF0 zYnRrqiqH1TyN6uo>hLf#BmS;FEzdL7r#T?n7qKto?z&&(KiGF>f0h`aIhq)bJt>#g zH_yoJx%-f_UFBrj|Fs4@s4sR--a;kUuNuCU_?eegsRE%d4}N+=d*4%f(5C!V4urd_@@z> zg`Ms3tjRrp(K#7swyLqa>)1K?=(+5X{kc>}D<;ma0aHDfV;2zP))1!+0xwTeK{t}|F-{Y1U4$+mJ2%%gnyDW9MlkI zMoIjZJ#lj6{YRMSU+Mt-CCZ1xAtw%k-ozkk8^K5 z#=8HvTbczHVc&$>eflYLd2_CkI~L=`+HUoRmJYpeu|M20z!@ebaLr%Uv z%RWss0D>2R#y$J;!-&W1_bmTnNBARW$c~wndT&47^c4A6X#7$ueg*r{BjOo*Kw4ir zfcKJmh@3wn$3ULap}uM_TX!g6Uv|j2->GG?@6&uwwTV1N{7Sxr{VDJV@H2N%lj;`O z5jj}=t~g0$_h+#KtpoR<{)#rd&b(6T1obpYiFF#u3o*q(r2jlp!Pwo z=Jpr5$90aHau4?YjKH|mR#F>KO^_c4pxISrK+1F#A#Xm1t z%)08svl(F&xzB7j zfV$w}$d&9LkrP5bf)?Lce)k*jtEckJwGZ{Rx3O^v{pFD>fk%d}zB!H@2J7|K)Z~GC zJ>tXW?AJf|ed7L> zDD!^hKVZ{|&n}I;Sj{cZZ|{J;{9PBt8+Q!Jh+C((o-O|z>HgB zgn_INXZa1oKiF4yS@;y6kNg4a!g7|ybHq2`Gse9e?>RfeJ{0@1)ghF>;k&Es{DGJEaV z%i|dPaqMrh&MVJ{yfAm|*mua0-X%{Ln>vnN8yma=F#-4|&Sc-U~bx-%5Jz2)p;34C^ zM{5e?g{5k1c%5P_a$v-=;Hr*}tcvop&b7xf?B90n`#>GYa z{X@?mb8^Ko#2h*ey(spexRx9P<8b?ieEwpg#Nv7W-PxCOpSUo13*x%0hrklWB&kuy z-}NtQWu4@r@caWO^zTXMzDh5jdgy)V?IwgLOc;nv$GfpDepb@|v&ZR~BHFMO-mco91P`pSi!|AG%4ov|Jj`X6FZQyt3|_#_59ORi@WmE^P3Fb%e%F~?`jl2f{?2)lrg8O#Cs~8z!jGseuYWYZ zMt_9wrWqNXhCaag$3-r0KCU0sXBoSyPiW45=R?0?&%|6^?8ji@;qJuUN!!plGXAc7 zdvuV&!HU3#x{>1#W=|yz1mh*~4Y3Y+CVmzdliwnT!TvbyurY?*5$&Fzv2R?D>t}Js zYp~8@Ov21PY^_|}6rbFFv-~bGmjBB;Xhe@3=GV>(bH1wl3i^WZ zUHc**7i0067^|pvtT>4c@*LEJGe^Fr zm0ARw$vZ2t;r)2;W!U>l^0E%B&$Yzt&V}rWZ`z8lD393NoY(KGjZlA2F3!1Rjr`SW zd}Af{sz2vknp20u{ugKJsgvku>)W3H;^JOlW*dp$>`53Lxyhd1ZxX9?kIzk~RkN46 zl7p^b%~xVeyRk=|AyC2Z_75C@`E2Z~#NTFC9C3J8{A@1n#rfv)9I7U7j_Vg|BEO9D z^xE}0xce^baBuvh^>KLuJ;>csE@QpGJiYelKJtiefjiNYfp6`|dbf{YSJc)&-84UP zTX9J97w7Rg^Fl6!oOrom=J-WV?P71&`(AjyHDECvsS&?I-dK~n{=w$>Z?Xfkq9@i`Ub@EyhW**ne;U5CA^iqHBNdG~S^tfh)C$$Jpj z?KWctb&LvP!qvokW#j^`l>_XJmGNEY^4JGpPVGEvb697zsJq*;(NbdA)o={nX5YOL z-?bhs&%t%B%@I2)SiPA&Q%!!Acn)%OJrDbmXK0^Gn&+Y(ntATk`**V5J_&5co*e7B z_8_>IoF617LHtCm58s{GnfxfUQMsS}JPSEviStnW*(tGpT<41}Q4cNUs_M+ha}XEv z{Av##{ALevsrEBzUCP1MuF9FWE})L0*n}8z?a_HW$9kSiGh)JWY-kPpq_ukx|5X#$ zCZ=j$>^wf_xu^*;_lYKaH{mdQdba{+Nb@h$ser>CYtMP6&XT|`#yN-XDT(#td|3QQ ze3k<|<8WoHz4gzXPMo$p)}FmT`(H}4O4TpY7swAUd*)4ST$<4=cIthpUxdas`9hjs zAnuy@4|lM3k`5=mzQj;1cvjlUh4}m?_^3ks+JpE?IT(fEm%T?Z_FtF%A^z@XXH}RZ zXk+0(2iC3cZqQ&V=WmfQXY4lPW%kG6@C)ilID1qdDqgCFhM#A_Ka@jat}50l#w)j> zN1?MIlOBb7!g6c1&*DgGl<5o1o#knJ{IrMvo18iwZ>%E^#Qag8BA3s2KwSbg2lP$U za){9m@{HtBbO*mof7f3d2l{NS5!%P0P9rrxo@-a$iI~7-eB%Jti29S~a(`VS{=vSo zuG1_DVol=E~~5@xSfjyY}(@qD>2IU=g{RnpAn=i+%p;Irz*UcqGlpmgkUq z_r^sGHs{0pxrX6!{6p<;1gkhdp0hoDXtBjQS-EZ^c57^K9*k$ytV?lJ5A5an5rfIo z*jY1__~O2-@vxc?H_v$>Yc0~}j7CLPO*89gU9S66@ENSTv5vCF)$S4IY-H62G|ohS zI_ieT-r(z5>-*NV$ISY#$Y0dee7572*^^uaX4d_ilR$hFe;)@g7yaw3IhVQ+lct`M zHHVTrVGR*2NpL^s?aqg*FpOMyZR8_KPfso1`$Maoe(}iVJ&)Ihu4NfmMA%Gu{?uvl zp_`~zJr%j3^$7TH)G;&oGUu?jiR0lj$p@gn*gW(Q`{W7o#|$>%J>&n#q0Gn8qK=xc zy@d7)n73;4iFcz$Y%eEydP$1;54#^iU(S31URS<3mzodE)XO`047o+AIEMY3#5-|} zSQZ@bJox~|`Mitq=5)qU#BTY-TKLfSSwkc9{jPZjT#GoTm_K+$zWoK>Be+-OCcY!| zJM%q{*NJ`Mn$q>aZH&40-V0pxf9ExFwyn1scU4V38+)MMqkH^VYc&ywoXB= zop1eA&Ww4n`J>lR^wb$Vi^P%OH{!eU8Hi8v&F_4k=1S(so`?K2KFg~?*EQc66V5jg zXYzW?%fzKzw>}g1t7}89l)Y2lqx>0jDA%F-Bx;_>jWIs4*T8(!o-cI+#0tbtodJ5X z_U)Rn2Vd?ky3=UsCyfi&hI|{f0<8JMoz9s*%8_6$J?Hr{>c7O5fd#2U;yaps>)9D< z{9qA1v3J3>i2i-+O4Oc9)xu(uvV9v0`ZUMWXt?OlDtR?DRU z|IW#U!{@}`elUdr?L@3*jql3s-I3H^i-GuYQ} zer>EJR^vQMV~m|OkHGzCMD8W;@hP6Gb5YIHU!YcEepg7`_%bnudhAIrTEA~hVx7Vm z&vSnD~9MkJractg&reNx7C&tG&G`AUoUE#5IPLh8Z%vj6=&_a0y*iOZ_~ zs(22%QP}>7x5*D~iXIT^2*gWw!qx1`noJzmU!xVEW`%f;y>j9_@=?27*EW9l@yC9O zkLnh&mV6!a7jYzON$SQsr(5ow@taz96Gm;q{!fTp_~toVS)U^!w?*@ez1){vV;;|R z54l!*o>kJ60KVYaFNnO@m}<_4YU`+MITy{+?9twhnT9{_ErtHRSVWVZ+lLd2P;=+wr+u&?9y*TwsibE}ri zq^WP{msqQHyzeyI)YwLTg*mVBf5{!&!F|hFLtk?K%E)cK9<}q;b}+6`N5R}%9B=t$ zCFH&pk?;R9Z*@*ChxSJvjpwvK&OCR{hiiy?#g|I4k(IpPciFdV$a5BU8C-XMJsd$I=B>$m`TzEgZSvHP$j*opOY64gra(?Bn%XaZuZt7BUqK)__W5s3I zvr2r>L5^7!IDp*U19io$!#DZwqsY_c=gUof^ykaDr*!>tEv_BABjOqRT%1#2?q;99 zTA<>YGY-EFF0mV5@M-WIh7{HFK0d&o*k8YcXJ{{B0X}Rmd%l|IUJx~0XI0eSlk#io z@Yt-y*smI%!+x$m=|+Naz%yHh&8h)oI*1K_E$|t7B(NPx3)FWf2g%y8x;^qAo~3)tk893jFC>u6b)!g|R!H?b}M$O^>EAWFWv2o%q z_LA)9UP|KmtJUxIFIlq}+q8i=++9NOf>;%7YI>-Y*`9ZK}Yg*I<9QbIdaAk+@$?>`gfgtYe2t z&4|~1yJZdW{JOv;)K|Hob&l(C&Vn;aUXOPH9!7k-Ca&M>S+XW!V)9k!nTm5dr^6n) zQBxLh-}{1dVt)j^D_l?N!RP&8yX9QX!sk23#JxNpJ6!oMUdrEDo0|gfbw0AS9`OWw zAME|nzv!FP2M`Ak`?ViIf1!?teK%qe*FSgzx!R_{la0&mWe{6le%Y<$>E-cVw6VxP zHeyFs@SM8x3_gl$Adlc0R$=$8BlN(2YznMH-D5dpb3RP-M{c^Q9X@4i>`meooe`)&klXJzbD=bd$%Q6B3}d|=G1%E0KT6XHjf zg)O>w!s+oJo1o1Z5yv!Wu!CCIC}N&@tY_zy*h`*f)>uDdc7U z`8?&Kc)!+9smq7WxpzVy-%IP$XLFrf)c6C%{pb?Ms~aXi*1ahYR&8z1Pj0~8?-J+fH1u9+}jr; zro{$=XUt;1c8Yk?-fE9GW;329e~ROskM>aew&YNYS--dO%$c@}jje&I zW8?dB?ul!_=cz84n4zCjkJuR2l<)5UaGrsz1$itpct?X+|FZ%g5Z~@LV|G@4{@1^6 z!?T(dSe7~3N&P@;1jL8r_~!=B0~av#2J_(0tE~Z-AMC|mzx%0ok6{geLSD8Q{``2b z(Qmb<;*kJ#qD$in)zDL))^}lk zeburlS!bz`$MW4PB43tgf(BgHoL!qn|54W5U(A77eSv?9C-AJYVkzPV;uvBJVj610 znrj<-!-WrgL+k>4C9p*MIDF2jFU-#~(Ns*BLj(TL-_j1#cb+E};b-a}fg8{d;CaAv zchAqh z&ab~G^&7fYUFY_6ySBY%uMLblzAHX2cgtgZkIuYuKYG9R^of~UBT)O){UtW8Hba-| zF2`29AF%=Y*Vx*j*ONx3R2&SvwMuY zz|+Zh?U7u}vz|qNhP|1oCtBT-bo|wuLE~#8Zn_dD#m`kBXtgMXuT%*Hi!B?$2IJV{Ky)vSgUc4y)e$8F-B6C)jVA; z4ZJ{VDm&x5&Wd$LusB}%7mxBx8piYYy_*Ay^*ImDJ_2#M=G&*^hd+pX%^oiCH{ZK` zI`*LXS-i+u6=HO1z1pkmdDI-Um(#VR#tZ#1#E~blwQa%G!H)60Lvf56GHP1x>Ajp7 zwL7+LAJvphSJ4TM?yqGrA+k=Ic#_`VSe*U7Q1u9=o zyy)^VTk#{ExlVN_s<0s);2W$AE|9Z~?akSDp z%bo>u)XvmFmht=B@Ym{Bt53KMUc^~&N(;zica!%uC!U|rnwD?V247#meQsh8H;sJl z*Ja!J`={6~agsgw+#T$z!oVfOrLCK*=it7uPe=Sg9CSclDWL%@z)vXfFVDj(mC|C#rFQ z9vwB%0TbnN(|buBv)@GbqdG3c**W*4>qQKTzu|M&@VuQL`Wfd;^GV_S5vNth@rmnI zLar}ItB%|t8*5enQufTM`1{izpUT>(q~0PosC`Br4s%L)r?uF9*G46MABSUKh)>Gn zZGCOR4L;p;R^;{KJl18zE#Vg7bJILrXCbMrG+|V6&ODcTY^GrKJTU0Du*+v;#Qk_q z#9b|U#&cOW?+|a>cR4@!PVy7%F?8QSAzcpdz^2X(iE$g~7 z=gdUsyd0lj>~RSHcsh$@7~hvwKk_F*v6h!u~x;#H}*`tFZ&LLH%seT zZj!n;;sa{Ph^vdY+lvIxD%O?SQ_h-nu8KI7c3VH?OlR{%{fm4{bMT#LV2VBJQ`D?J z)OTZG8*u$I_{~$pkB=BJo%6Md zd>q^m`{`M%_c zK25xNB#swo!$2cPY|Epl6Sr+ zQwe8P4zusnT9y1s>uYL~$n%tEXl>55;opNItdZdh@mWRe`ya8E zCd7B;P;oyIciFeFr+0q*mOaQ~cVaAR%UHLPt0PxjYziDDa0zYloe$j?pXD1|wd}8v zbD=o_9`=i@v9x>}c*p+o`i<<)4dVFrO?d@4{Gu+%0mt zxo7~A?`MtetwA{F#hBk-RA;yt+Z$hDYvOqQku_*ASJuQD;>gYLD=v>?sQrWng&3MM zFR1NvZPmfaAy>xcZ02`2Qm0nq^hliFT*H1G`)k%eH;UYFJu${K`WGhhOvKF|i@J96 z?VG8AZAZV<+{D?bb*1KU_U6-X62EKAseY;Xpm~A)2;?o;vAx)Zk4vv<4+HaQ@6qE0kg^X9r<@^urcezcP%F1b2bKprxrXPYhn0|`0Uzn zzJ~9}JOw)u7=rH$+Yr489zzds9525J>@;#N*8n|A@w@VioW~(Y#q$%-<;y>D@57VH zH{bO7?G^IATvzgWoU39EWzAe1Lz|=Sj%&&FV9w5&Z8HuJFb2;$ql7v?*yDcYs*E9?Yl)42CA3iNXEEQG zMk6J%WjEgOv^zB?~#pNoA`^m;`9k^CIu;J~=- zhhoiV)!h~E;u^AYa-8jgu18i~P%*JC*S!O;DRFY(n8rD093G1u?i4yiM}ms0dh{nqx{V5%y@U=Y&^sNALceI&M#gkr$szOofGGn*bn9JIzw5mhaFq1r=oA<~w9OrWoAG>N<*L-mjuOECh zPn=DzPLpd|2>QWtrgchrwKK>xm@OpLc5x@YRT$fMH^@H|VwT(C>Ww7tRYv3^ZY4C}2Q z&p&je$Ac|mKO2K597C&CotY1~h99tJ`*H6F13MB|H$K7F(C;>o_jQOpI4>{ZoUMF! zE712Mzd9j4P)dA(&HPULoTqKEhY^2UqGq++0DBwN<`TEp=J!| zn;L&VP&X;+7oL-T%ZPdGceW0DQrATdfj`_bIB*88iFeo_W4wn>am0AWjbmm#M@(@V zHd7waJbXo4_U3Esq5rR{H-WCI%+|F7A|g_Vi1fYEMnFVF3`sVMeOFc1wfs3=j@Qfa za=cnvW#1RavMA(GN>Pe^F9b!5h=4K4-jIgSB%yCbL`1~C7aOSiJo7V-@t-jmXV-FN z?X}nXzS+FOJK7gt)s=iJ$9k{f9T)Nrm#`;lh;dIPM_HURmr*OADRFB{1*Phvgh0ymK_>bzHvTQ}c%(F)dU1HYv|*Iy3CVO_OjtZmn!y)&+J=T0)a z39Q>*`wNM~llF`2&~wbET*Dq0D`RfD_%Sh(^+$CbSAf~7@%R{c)bhZrh;fO56Rzd# zA$qG~{cNQt-T5Z&LF_Jdvt`8AsqRnh8Jtv|%W9s<4%XaeY@4%&KjQi|;G)i^+`@OL zRZv4-v=@84EzXw%kH6sWYVh3~#MF2XHJrDL{je5)Rn5Bj3`}+r=hqQ?tjEq&;LmGv z&gd*Vr|#$L%OT%@jj{LVTR9%Y1?#YfTX|OMu`I`qZN)}p!Ar%g-X)G*g}tc`+Y8?V zTxvUb&f9Um{32t~n=4aKkTr-Fmn4lBXSGkCWzYUL@V?#Hg~eP??Vw#?>CQb#_pesW z%+6a_t9yduM7+#8-bLKLnBQgJ%^qU3*TG`n!k*h#Uc$OmJLTQr(iH66O#J^=@Nn*5 zwgY}n6|qdJ(W4!LjZ8BǏf`}^dK+VGdCQOhw)+G1W`+{0cNYf_$rP9J-5W!MUJxjjceqJ3~( z>0jh3$V)V5zjfc0)S9-CkEDKK&(ZhUD=kk_ZY&nhM((ZeCiRJ~L z&#C48sIfDW_n$D6dvfjZcZv7ttZ>@y$(!ZZ4aYAp#(u71Z|b8Lhh1{Ef*eBn4%y2m z5Dza2|0TECH9vl$+C3+QuetZ59r)2gVit9rWXun7DzxF$Ui~7k=HEs0&-)$)EBz`*TbN9be**`naaN+1Zb6h;y7PG;+eb!~_NW zt|fstlKSFXUW)HhcTW4G_LjZs>Or{|jP1J(n;Sl;-KAezOHBZ=pTTdK8dEMdQ!ah;gh?8n}NGo)tzWY*nL5$~z9=L`$E zMrz#IqbHUnw$^g;o?m>cbt|8z^QP4^5xWzI*})!n zX0da)jNd#*evVo(u6g;xt|c){W47~0r5KM{BdqyUL$nU~$HH`dwffCR+&gONZ+@;f zaf0{ozu6aqcuwjh47T=4F@5EZt zr(eo%>}8yT|LMzK92mz-|1=3--7!AHeE?(V756W<$C$*qf!1#2M;RN4E83T*Ua|e= za_x*&tW~KCYOKOKWep68_}sdl^Ikn4ehGVeDEmcyp+AG=4#V~*JrK2>uXxpoK(F{uoQYL&Sjk+#x*O0=cC>@vkhV{i8F2z4pxw}e-k_?a@)ez#(3AE6;nuD z=RCW=aa=9U5Aa_*D$!1e+|9W*?AJI)%?;*v@&4ZDolT_&X8?ZW6LBPFWu|kSD@tyI z{g0k9^1=Q&`NQx-%I%>LS21@(ql^3bkoadzzBmw$VE?##x{K7VKDD(;5W(sQ@i2?A1^B;`^h2&6v30f6daR4zEV&bfQOmc_FjiaYYdq`{*IEnVho+-H~es*q=xP zeNW<(ezw2Kwc+=&rszpj^LY%o%EHh$7MBpWlr!U+w7*F_Ry@Hz7jX>l)BYwgRdp@I zo#dH_rLbmxIWE58adl#OcSYg@o-e-Pn)EpNX^}XjTsZQ-;3KJ_A=V)-;XTu<7?_(J zBy#pRU;Bv`NZ=k`ll>i7N19>UDY!sz+Iapy#5v+8ax=tO{Edy@&5S-N??K+Bn1+8# zZx6Yz+QO$0U+nt@OW~g5I_m%MUa)s(#XF^j2acKU6D_79F&XdKS(<*A{uF*&9QSXZ z?s_D7t^AqL=8f<5J<0|0n%YL^1&i~5Q;_3N&59Ap<8@uRew^0^S2Zv|*Rb3eY*Anl zyswyvtbT(1%D!JP7ktEE>|et4%qit2ISWH9-?b`kqc(5SEXu=&JP`LFKhS&goHToA zdfX?vzhFCm$%?gFGqQ)#_vlOzu|e0fy@#zgpPu!5)uL4A8J|GxvYoiGeb^{#uVRKx znmNuhLYs@d^CkB5AUfemO9%dC^h*}dt83quT(wote1rdbk{-Y!Y~5b`%C<_^q#PLM zSvS=bz*0gR+-o{}+IJ*3311N3rTrl;M@Q=%Vi9M}FRGB22zI5-iM~R=R}9LYL~(L* zH87uZc}E{(>-VCIH7@>EyQki`@i{YLxX%pscBj{2ZkMya)hanWI8NR(hNTj1Jl5nE z^Sd$bO>r&pK%bj^4fX}AgG5al|5RBh&MfYY-764#Hx87WnBKtT<+QjrT?1!Q zrzRf@9dc^{+D7lwnmAgw`15lj_i;^%Ez3`%@7TIUe6Lzn-kUg^^(=E|<5goD`*+k5 zHx7}{Eceei$C{%W`^GTxmc>G?uj#MdU&aipo_PU$sXOPEM%=E3o3S_hpZZ3(h!Y+< z{W0>k{=}T6)Wx3Xcbv%DE@e+l=DyViPnxdaj1iyMFTZr<@bLB4QU_gD9Cb8$&UqHj zZcdnz__-Vvb>!3@l(UqV_W=8&i2PNIu9$t;B|aOQ-F?+@ac^?D<)X_`nL75*_~%07 zOx}M_-yw0U+?x}4KF0>WMBgayZCGGL)>73=l*e9v!%p(mVsN1ne%l7t zf0|o4&qaN5=9>^tG?L2~#WR=pdGANN;$5hfXRd8r3f9c~Ylh#EA6~$^_$uN?xyot* z`Hs}XQQO4#B0gq4PA<9}W@<+)d9lQvJgdXGm!wzVEM4EJ7^7>$*cB|3^>KJy$C*bz z+|`_Gr)&DR-Ve5k{n7MYcM>Ze9`EJyJx}w@mqy+BviqN94J4gjHKOgKH+QqI#2#lc zz=;L-^UQPHlQ`RqsHYq6Im3Ek-TQH!jkh?D_{YfOjJrlnT^qUEupNJ49quuA21l97 z^}Y`*2)~XWS`+8EKE*(cY3*Av_BKb8pJwiCT#CJl=j`{YYhk^K{yS`HFL0Xmk>6R% z78`L5+kdV`?)jrC@Ljz*u71dR@}6q2qfX`&!~)gCen%6FZsZ(i89UcoPWWMH%9Pzq)a%DWL8e*09 z?9|KwItTtATyl-N`X8v~v)?LCn>|@0|^ZhmWua4~Bm2v-G-(I#}_paw2 zj)}izE;P2T1Ndi}#k_Q77xK;5xu3VNhh5kgujj;H+ze~__ECyai>zqGo2l+`iVly>icJXeGA&)D`V4K-HN5%1$lXqZK4kaeBpXFWr z_~*oyb0d$Fb7G&%?ibeM_nXDOQxi&G045yRi+DSI5A5v*a)~_b$z0Za!l2xzHN7`* zyp9+oahxijYMlK_|7rCukozBg1UYBzW|5&&TTOdC63H~c_V8l$|2=ZhxVu{PfOiksdhN2-hStyh8#@!zWhH#Tn;PcWyueo_VT zpjt-T@F}^#9OPVq9mRFjgz6e!a2fQ$cyZAlsok`_OUG1j~p9nb8$_1 zck-8pELe-p-5HptxwhKTa^!X#w=yT!NQ{EH(fCk%C>G-{Hgnx(vG#7RTo2#iNNmy~ zV$}xra5wggeNtQF{?&G#P_~Qx)gj)y{gUUlY0o{+rN-6>Hk3F~?eBSu^>;M!e0q=Q z8Q`bp^4;?M+7XABWZK{J4j4rTY<9vbUC&pQC#=l%lQ?yoV&94HEqG#g&UvYezDk@T z*SY;lt}&0Rc_r>ApUqxH=Sf)uaxSpiyv|(sa$Nze((WZGfd_-BC|Iq126IZ{&a}oD!Lk%JE<(z#$Z+@&dxm#*w>uct9Z_mB0;QQ+1 zH;TREV*^{0KZ905*jxRGd|>-TJV&07^O(dNpQWc;uC@Ncc>(hAJYOCTxGHO4K0Z~B z@mAJq!Zhro)b?us+cZC#7+W2=2k~$11EbJKOn&ePFdSosd%1r5z$NT8R*%TPJ-5w2 zz+aCDA1yb@y(wPT>)yxk#fQXk@e|jQ^#V2MtRLwY`jkFPOln_@oTdBVaUBxZT)Ogq zffKgIUZq-(`^a;gHJ~1jpXFn#$CZ53SbRk5xW2wp?QwmF@q_lk*u&bb?@L_KTAo}5 zYhiN4)!p)ZYCGVXU-P$bkk|Gk2LD%JLH4KHD`%fL*WmYd0}J;Z{eQ#*NiSEI1Sv>O7V)w&E>1` zyJLQbvjeEV{H&Mqp|$L+^L-9($itpI5c;LopuG<9DRyZqG0-mZM8E4Gxn>?%%?SK` zJ@)+4nC0q>AvI~m9L!CqrA7~zb$098-wsq;buTge#NbPq7d-mNBXPd{2<9M@A3QIo zRuB5673OP6v$|_sUycy9wU`O({8?w9$eA%mp!Qv6j-#*P9ahNGPy;i~M?NFx+6J#E z8b2}hVu;C@(-n{Wl(jXjT<(xHEaHudel6uzS#!B6I6v~8=tnE^`s(x016Coo$GIxz zKIUTPN8}cv|Koe%c}Bm9wO};*qesd9dS>(lA4y*iE+6@ny}UJBu`*&`^XB;M83$6+s<=Xg!IWKEnRIor-$76*`v zXm0MhwJ(+Y5&hd^d7cNrh##+nf5bTlxc04R>L&dkI0($t?#plg@#poB^n%RGya#c9 zdnL`^&57iGh$Fe*(J2euz`d-Fm-pspX2xcny+wZ;e);yS`Z@NMsHH4-$U5?~XIv6^ zg0qcgb{-e^EH+}VoPVo6JpCtG`9$Gs-&B(5n(h#V!p zSB)SvL9%M;i08oD3B1DZrB0GHHQ7o$7va%=8?4cpRcUI%ceWV@cf z?wE|dZ*p~v;hniA=3pF4URMbgPT%EE@>0P41`uC;!diHPnbd=b=RODf{&%#*`(tO- zb9fkir2fPM>ISc8kDnBOtNye7T=pG14_bU!`zEiZruRT>@dwu6!G}i@lkJbSC$C1n zqP!Zv*ZP{=Na{jiC&97`#Ea|~gM-Yv9USMN;S>0VF_k@K@L2J4&i*(Bji5s8L-6eb z*Oh-Ro=mTK+$a3$iYAO29%b-tnjO~0l-dti)Mu6;C3TG_@?O|!_wip2oM z(#+B@;XN;HB z@0G8m4>f1EXG$MxAC0vPuetk$f5RsXj2eVFtb7dX5zZHOmZJ5P3$J*Zn(B9vFMoaD zd7jgu5idCt+1S{T&7nb-X`e!JRz zNpH}(NY+-=b2e@?Zdtl=26%!xuBm=1uT?+pOlLV-`gb`z>S{h-U5Y)P%srdasV6xS z`}-w-FK6a7{M#XM+-GZVlrfd>=!$lo$wSjD64$WbWnISSFWzDu$DG=0s&nm35a+d; zYoB&`AY2%Gg8Z%Og3zhWn`jYb_=ZNb_ra@B8{!aBfY`dZjge^-jSE zGiox()ez@5A5rHP{c-Ho8^o(MeBYQjpEZqNE8@K^W}nrvFN#=iC7C{@Z-ZNoAzvur zTDw_`X(q^$o*Q#w=5lzPFRbRIb=5~7*@3_4L2Q-g1>0wLXYpKYN+Z|okNvUVLCxYW z?CaNv^VAO+#eR7uW9^k15BqB#>%M{9suTMn@r5qDq7~PCllxK2r2yZQW&|^*jo*^= z9L&p@6&^W4s@GHn&&OUo^9B1RR@^j{b1LI|oeizNuE%#?b-@Fmy3d;@a_>vzt060@@BQT~9Km9s(RkeDx^ zN6)&gidw&Tk(w~(1;*j@gb}x-SP0p~E^B3pr zBTBQvmx51Jb02SE+w1XB=Ir~}!>+5Xtm94m#zn-N4ZQ13{N^{}yQW>e8NAFqzM4Iw z{!*G*<9*6enL2hg_qi2c^k!f|@{6qL+k=C?Pu#ybzq!14%MW+0V~y=#e=d--kRz%=g|{p)xR#Ur;77j5T#&0&AN&pQr19M)%j z{6=wuDIM3wdsG|QemZ|UIUP30`9ZtL)ASeiG|7P}0kd15!xrFwORz6H<2ZUy896i7 zBhZ72++QBorK9J85!C0LmF#;t?edK|dL3Mo#E1>puHE453o~k`*vE$-0Gks#l3!F8 z$L-_ZcuPfKP-?29Wfy;&I8?R7D-Ahyq*+Vxex08MzbE#1(&c^?OyU)I3!lR25-)v% z{r5?5b`4QD z!};Sqi6I-~^YmFmiJ#XJb2-zzSbG@9<>FR8RU7---fepU#os1BDA%Pt?3J^tmaa@a zfS=wm03VkPyJ1|aov|lY->OaMw6zw$*bmIvS!`*(mG%M58{e7cSjq3Q_U*p-e9ku3 z*f8)avAfOKiX!3{akGus)?)C6mooa7OWGE)uG1VVBr;i3X?E4BXXLn+aLy*auMofR zBJooLSnFtPh8kSk*u!JMq87wj@*K~O*Dj(L%Dp!_VqTJCyN?L+L{&#nP$8TuNr z7iXaOIcfel|2NgMn7z#P60W7rxiR6{yT%(Y$33V`ZJkbx#d!znV|#sTVd4PBigH=i zQE|;Y#N?d9jq0UH~daqT$|!W_hS^~x(B!VV3IQaU0LM`zq&8U5A}cb+k|KDCYSb8~2ay zGn|aB#JLMtU%jHfW^F~y1$0m2xtVVt+;Bp?Q#A*Ssf>H6<6=jL^WN0=zJd3ncH)%4 ze0=84zA-P8`=KU}b#{BPd6to@x*v(r=u;TY-unr=`YzhMBXagTpr=mWGzLGE=e{8S z8qE6qjJSF$b8UtY|H)l{Fm!UnNzAj%r~F%wJ7?CsEY72RfXmaggQO}i2X3v7Payi@fVae-Gv@JY`G4f>mUhW}yBi5{aw(?%&$?y#0 zxaZpwCx#;r1YEpGE`;{Vo;dGcK8ZM$+A{W1oyoj!aW;?RZ{oOE2N+{ymm0wV^3O|R)^@_9 zx&*#t-fT>o(Cvf^(Y z=6^>l`unULgN1e9k`LUHHOCi|Y@RJxsit2lV_mDU4 zApU&2QhZn}6wRfdapq5n)%#~%lc_iMZ3jnF1i8&f>T`xiq72M(;*djNm>nVA725miSW<@${dVuiUMkm*zlA+p{`G9+;jzX!K$PuSQJU z)uKGT@8v}e24Sg z)oMEu?Bx8M*t@z_t}&0x-559R{G2*P#w6Cxu))MT)@4SsM@Qfj=My7dLk>HLSjb+m z@zh;UisSe>;^#EW+dg!A%EYOh4<$F-zqN1c)_uuWinlC>k1dye{KS`d4{8*rnH^}G z#b@~#_Juifme`J%WM~|RON!6!7_kA`Px$M;;k(Zt^$6cP6#HY``y6pj0hs5j z;iMa*&WYcp7V+c<^T9S2Vb6NRJ&XT{vpJj9dXvvn98Zi$%{Y33gI_IPB!9`cPCX*$ zP#ZUj`fs0&8?Pd9O9#=2Io&_TJs? z5@Yds+P@%%U@U6imNgX5!H))R<2zC(-F)9`s^M(CjQR`fy*KvwE8hJ^?yWj!y|rNH zs>oTK!)~si)`PvMx9(dOIgWK|x&Lxw)HRi(vHkmH)N8s2c4yC&GlQI$C5B`D(LN{p zAe?n>F0=pQRpf`Q!Sl<(7{&U2#j{_Iy|=gPn2Z{*1BTlx(Sx{RF~4_X?(;cqi6_-x ztpfvULrjv5>u0CT6U)XHC+8wNF#*#ouO5;p{DSo6jG06xiP4TwdM=-u=a|{Z$&(24h7y5 zzgL|TaYFT2?B%f+WAk&)BA>&xc7rAEwO z)>)I(;|d@C!{D*%lfZ#5+Wo?SvCb669?_dTco8o7<5_cALRacC(BvC2|rW=O0JhPr$#Sa9oKP9+G`>v z0DjGHOq#&X=%f!W&X+&7wV{gr?p$njZ))TGwSA9`b$fecjj zBbQTmLTq6ZSkIwxo$0gI!7oiS$XqA(iFnNc!`~vV>y%NSioU48tJNTK4a<{U@WdSW zqMO+})=g#yhSpU2<2%$**X)0Zg+ado%SLT7wwg zYDugybJ7m;UkW?o{&HVDboyrEw_|v=v&k8Dv!6SIpXabU>Lc~a#+!2VFNbg?xtZ844g7vIC*?hOBB zuk^TS$?r_NdJg-tQ}8zy);&aC+bJWqa4Ec8xh3KW@ZiD6d`G3&$@#g&V%^% z^dsYZ;{@NMzHicuC-~c>O{8v~ejy#dfcxr*kA0PA|2%dpFJcdRX}Hc|fkiCo`5#~= zM}mnt8*3&X`!Ej9rgIX5o_mX zn4wC3JC6JB75Ij6q-);yC>ClTfjE+}#N~T>vv2GL`)$NU_CZ_wu%|D5?+p=miNlb` zVhfAnIiPt;OkK!YTY$~JjWtln+H`LC?};bJ{QPWBB95{Kl=3*&3_aOd`Rc~H)~5Yz z?uM3R)?C~9Dd0L;`4Y}vk%wb`r{1p`K^{-sn6t9-D2&_fosqX-d<91-uBpBa*iYnr zNyA}C);hbnp86j0-p!lPNr~geIdFG^+wb|{eOb98ayf`;s6n5Lzx-JKS@3hr6V>EV zlVtgMd-=`PmFDbngs7Ltb*w?CH{$oonK#!{yVaQsSCub}{AJf|?}0fEjJ_K+3Bd=V ze@9LZT!P5mu4uR4wN&Z+Zu|A<1wdoj=k^it(ja`wnfP3Dhg&LVKQ<0q{B__$_DPvX zpwClo&LS2B=2KyBk@CDI?ThhQS+})U(md3fxVW#M(cvJz=nF^hTx?K2 z`|(7+Lu_zRhWElVIwwaQz+U`I)_y2)C+F6AO)(-gStG9(lOg}j%7;;7hdM)6K8(kS ziv#Zxn|kz-TjSo`YwVT4zT}@^tFrbAIS-3{9Q#OY9F6mAmlkKTrwWMIisNtnUVxkU zTXlQjDh768kANC7?k_Pb=ewCl%a@xp`_0Prq{4Cz?taJHwax9rW7+9;kO*G8n_#X@9*6yQ! zXC(IFAoei}ws|~%`vtaXQ>B~`*Ra~i)H;Z>KjuB}r_XU5nBHN5^)c6od-wnwdSBqH zezvcN`3$_HAA%#NCQM;#>tfGVqJ={qA&#^;W>krT*%wI;O`N}n9OQK{lY*Rj=IpQX z%BN1mUhT(T*H*Arh;Ke5@0!8>8pFHzG4N8KgZYU21#B{SS?(8g&ef<9d!dJe80mo5 zjJbPiY|_`m%&F0=|NZg_!Ho;atG~Ba6xggY1=L_?Znv*D5wl>+Q$MnG0W}-# z0f8&QzDo12o!Q|$P0x2v+WThyYYuK4YtO8hhMaJ7YPfman{@%}J9o2h+jGoW4F5Oy zu71|fs`YI?Zk@&cS?dD!;aY>xKg#1(M@v8MJWcJDaftYa{V&!#tVOE#E7xW6gEPT8 zK8<+b&@i|-n;V0v(jy(WF0 z_{@$GpBPKXR}`11Uwt+EFfZ^8IC;ceu7{+(yrgHsMvO7U#LTaKCgwHj)yTQER^|O$ zyK$zKbAs$KF|M&+TZ|$r9p(Ufln$*t6;e4`C0rCr;SJ z{-|Paj^Vl1bD!!ko`S6~&sJ;TB=-9nepfYrUrfwgPQFkh{=z$4p0f`{K8f5I^j}!F zClK3a$(w7)8BUIT;pwi{;zveZQa-;r&(3jD&&2xR!+%%=rnH^)mpC@gbF#k4{NMO4 zF@Niu@W$v22{nr zM(~T)fgcv^yomL)lyxAN`z+RZW8hTUjq4{Z4NS~@f%z!>dnLnsLE^+^aa^5JkAHn& zP2`^F5fkq%k9X>*!ji?CgdIr1%hP0Cxi?_uO;7K z&04TG>TU9xMY-!IZN<*73Y^NGJ9sGMQA^_9$cM3sX{L{S8?|@Z-?Js|Q;cZWZL3{3 zU<+@?Z$b9I>Z#F|WO|HW!wGT#j*D6_nCHenpG|5L5_|6BhGFcIbj`pq8fX> z4Ev@2Qx$9SJ)ZLxV*VHJmhsCX}ruPS^>aYoFKK z{Py>FZcF&wz;l<=@-lm%o_F?6T>r+I8;C=k^PXl5*)QpgJolY5$mmUr>vw7KKJW1* z>~%f(+xED=bKm7w{ciUPd{#X^S5A8+zN!H``7zJ%wZH)`-}4T5>D)Lj4{F-g%j10+ z^MFIfIqGs?&*Oc`zk2kMn!q2-5q`U*jQy_$^4s7}iEAg9Oy3Q@5d9$5<@55M=AD0q zO)uqHzDPd1H~2Pc%6Ys^^VhJSuTkgO&ic2H@)7R${lJj*r)egpv$LP(*=z~y*S+jH z`p26q|DBlO>-bC^i1=$|oZ~D2-?iruSMfad5I?3qS+8kLp~v`t@+^}cjy)jD&)dwq zkem8zcnI&ae;2U7`tS@Ark!Sm*99&u#}&QuINy0az6*V~wG3^DepX-Rv(+b}aljf} zPMzgVYI@GFewY2bkmJ3HF;=o3){r}@o81rJx0d669tk#;aA0vNcu;|lsR=W$du7;) z7MJt{JIV5V)YutyHIa)H4abYrkX>T#VKzG>KwHQ1Lv z*kHMW4aBI!0u!t0y_&sW$hupUyLI0->_z|ZiNkhm;a*S5oOXG_z9z!AabL(gRJZN! ztLDYJDmzE+=t8gn?R+slF>zX~*IUc5KWoKJ9XZz;P+#^?n>fc>lrsW6-+F`?j@Xf$ zTW1r9Ay`9k9;n!nv4Q)|YdpH#g@KyHQW2~j&;3Nsd zv4==3==@Q0StEl2Q&Q*6S!1q4`_atZ)T`+*e-^R%K%S?3m>bzc!-<&}VAD<^HcQ-Z zdCptmS0{{U3Nh`#z?6(F)xKMQ!2;fUXX zj`m-CI(z>l*3v5(XP#Qm@_E{~j)o?&!GCAv2FR~g!`VEdiKk(&ld%W;7R=!!_Rsg? zgVd%^V7>i>KiU^M!{T!4z>r(x&-U@2s=&Pma!>ogo_>w~>HzHg2gIq%=x^#z?d<^k z$@8cw_v2pGH+UgaMKl7T$7P2RD;&S-En2{}al=IPQ z7@jkR2ZM>3Q$Oc@(NkayY@dSpl$?BPfT{oJ=!}?y$HkWI=N88hhcquX@3tO#MZ3Rc z?HLj4+y4E8tQs<|TlCZ_m)8=MxoH`s&di%GpXuLUnfJr?vXf&=4BKy_4_`f*01F{>7e@hW=AS+yga z$E2>2$K{{P%kq41Am4BBRSI_giKi=I zAK82T$?H>Zl(W>uh17FUC)T;@Vp(c0gUyhS|BCf9u0riu^Co&&E4?@E7{_Dwko5vM zOBMFZSkI!zE&5i}XU^mQz=U{K|H3yt7+OQF6LVj&EPaYKYOm>xU^SV=k(^;64(;(f zi;uzP+!uTjc`Q$Ny+5!WF$Xo(&Fj@*@wv$vu4(7_~w$D+=F7?*3ny}&xk zn)}PsKtC+_HriF{{PDN$5j47DjX94V?dZT{?1RG($2rC%owoiV{?_X}`B|E4uigvK zIIt$;B6@M+{_U+3*A?@zMvR?{>yMiH1-7qU+^4l6oC^?{SJ;&t*g&{xRHyYM)9Ug*sDnfhN=j5KLg_076Z#g4?2#rMRS z(JTtCi?|AXO>q1Y4}^Y$I7i&reoVE#bITY**_K4Q*`N z8Q8-Qc%C=JzVoxamTJPA?_l3z)|H$VI4Z$m^KZp_?OjrjLT(%QBYU}y-}}4Z%=la2 zr&Y*(Qi}j?6?pQ=k&mz!3Y(sZJQ-{*$JK|~j_%fp#C9u)^L`AQDW^qys5ZX10l8%* zF$FfNmfv`Kk!wcmm!8vK&cXHuAI5iq?p4gm^7_pBEn^7L4Jf*iq4)fC{lX{OXANgG;+Yq}_%Hae{o@>XW)T}0=NOYXV@te9 zU#yPK@J~Pu&JqRuKOE>8?)&#$ruM9O>87du6P7eYx&s@Jst;)ES=6 z=c9?0>{t3Hv3yT(Ir%V85EBiKSiv9wz zQ0!LZMCCVJ&%JG8e>k^=K?K+|^CM~~Jile^9ed?|8_(7p%DJo7{l$CrXXfcm{dDTS zx@Oe9aSp|vH4DIydd27Ivo-}jppMG*lM*&4KiOUZbS1$3jwZ&J|58H?&?@jPu^06% z&C$esn6Ji~sbyc(a3A(gu8Z^KKgb#Qa@~p?y}zuJiky9Uk3MoZ`)CFJwuZR>C}O8H z6I=}tdl}Tz5nC270xya0{dQoExXE1aSHw>BycczK(k!G4uXu}Rq^^6KkL>JGG>2K6 zHGzZK<0Z~%?Y>XxB4RgZueXg{&t4nzDSwN&sJ(|`9n>3g&Q$XAX_v1c_dbGi%VJ;41(K5>hH38)`qLTpz2%?N%Z06MV88!JE|le6 zig|9uuhn56l7_SSJ2uz4N8lA-u6u$yQx0E{*0Y*U+E2NQp5q!82N3sj&C|0^e38S4 zE{t_Zj?KDX56<9w+V7eU4DhP*Hatt8O%?CDJL_ZxaancFII8hGvEgd)sKetK_}Sh@ z`(VTb8o$eD-R1%-FjkR=CJwo?;^B!~@_AsD(`T*3XB--9=H|-fV1q~VJni49;hITr{m<*Zh_#k<)a`Mbe^xzyqF>yP{WZ=V6UW~E{U+AZVBX552H?bI7H) zf>|Yw&Hc;Pg0XkWxRz#ieh1vU7wh6>{6{_ZqAQq#>t`d+v>R(J@yhglaMA+HQtQB3 zU2W489ZB=&igxqjer9~qf&0(J`ZUMy zF@7QTx`92_i9BdI?^O-$zS#8&zC-?-^AT#pH|;pi-!&imawM^E!ZeIOnzYpL?Zhj) zh@YLya6Yy+`6KH!BS!3|UNs*)^J?ORr17gBEc4aEm&wUd*ZPlzNe9pRpgOTNa81SE z>}Qcv+NX3K*LsBF3fv zJxmOEH1F3rQ#XQlb_vXcS}C}7(!g;xnK6kz2n`70jjrUd>SMnMet!t|GwH3_|7$#B z?~>X(ex`05=X*cSlvD%9o+h=|tSJ}^wYcO$?z275@-=K#7vk&gagG}PYTf8-@|dmQ z`T9Gbv$bRW6graZt>$rDf3$Sv;k-9_%je+tn&DqwBR}X#tkOE}6Rd@2m3pjg_Vu7Bdm!avr+x?vI661-4^MxcRwX6B`U;4Zci%dkQgY-@wr10?8?od$a4dzYs5t zVXb%fanh-=EtR_o8WRGyVH!o=U?-&+6jyY9Z~4&vqGu#u^k*_oAoZaMj0bJO@; zY9Nd0iIEz&P*=u2+#h&?YtNa*_KaxL)Q&LUH;*>glV>7->De=G`X#3$Hp!ar%P~g# zFlzVaYxY6d6JZ_)<`l<`PtYdMno~P#i5h?8S$;NOGk1^+q!zK?WgbVqOi$Ce$P2(; zVs4ih3)n*BT-Fr02lOJ&X5D;R?3`k_64KJ8yT-Nj?ZD%(J@-bAUw(typ7&+#KT;=L3`cH^`{=hz?uuOAekJ2a zF){m+;CJD_&%rMqpOIH%?&h3s`^dyD$cf{-oHHda#o1eOY~;_7(`3b?ZW|a#X~usH%`9RgRL`E#Hr}sT(7hj-7aSXV zr185$Slc6m<7CakISj5(pRIEx&TaE-)Ee}I`aU#^!-qPb$XbB;)ZQN-A>S+tTfO3@ zyTblkQ{Y)(I}4*u;oMA~7rtNtImC7N?J<#$Tes4`%V9!e}DUU4EhY+Ig*asCxxa)%Q8__;5!YRB*+_N`cR(Km@L$O|%8wLi$% zRUg!)#Z7!ZGE?=(6LnFoj4+R%&7w<^EpnAD-Y0$ss-oB0VSKL$-@5O7%mq9Jr1vh9EYqgOW$-Lks;*MH!6g4)7@yyo|7uAHVTlGu@ z*pBmqj*pzz9N9ir<9u@jxx|bZBCl7+^+eWWea>^#rID-e>?duP{C_k<@wr2-p|C!k zRWyoqw21F9M;HO7B0r&?{9!n;e>pbES;HeDZ!%|hzN&pLlONoi`*Ph#_Fc+t#eeQB zUdrFqge|pS;Ot%Nh%rvazbqj~tRY{JWA-|^?>2DGQzAEErVa04WaK7i@7hG{F`9K) z63?hhi=o7yX&#VuJ-Pk%*OKSx{esJo$DV0)>Inty!~Q#7Qf6hb-qk@ z*3w*jPd%U8#NYZ%?C*f@#Ish1WEk%{&7?NpVgIs68hFQP=9;zI>9e#6xyWDAZ2C%a zk_LQS-%Ok4X^yac<>)G~_w09Q#`;{rJvYR2*njbIo}FBj{^a%5?7arCjt=;^r9AgW zVxATpTNhZ0vvNjFUBUCN=9w%du1k8na&H!WwE`?rO_&wrz^PxUdH!bd;-%O;c`r3! zjw!d5e_~ID`YLiKt}0J+Tg;=U&q_Kf@>|TStclZiL0l_-vN`wS7t4q}7Gu{}!;h_k zhnF;3pFN{izGa+m&fj-V_f}4+!ReYw8lXq7~ z?EX4y;Vp9BdiLI8{Ki^x`&Gp5i-=ja5f?RJYvvGRzek*~8XuJOBA7!HI0o1`Yi=pF z@8x)3YWsY-?se>JL(XUDj3K!n_N-CgCst@+Z`5P2*G29xKJTn`dvlCMw>E6%w^!lE zTM^6G#dEgjl>R#QQ#E^IAGWEH-&!5GO5=A~e5U)in&Vac#%ki6_lUh~_~8Ua_xtxQEosqu=y7%VV*JNW2S;B>wHq}yTw2$PCqnRCR%sGkTOOqem&YJ%W9!n{;uKK8zT+()KhPhJM z*?ri=5`4zn*v-$tWYYX@IV7iD{xY_#9zXseF_byGI?NyNd`hrQyLh(tEH8~UZ_L2^ z4NS?pKw;~b;`6F!_Qbux0l|*FkAGc(?N*OEk3C%(?@`=!|HZEnd#U&PAvQeC!1B4t zpAmObZ=t4l;?LNd0HzfC-Pv2uUsr>F@_BxPKP!vl;+CxA*uU0>lIGYmpvro%NbDbLfp)pSG?EXWzVeI8tTb3<=5sWa$w{_m@oLZ&b5(?rC)11|Dh(@62A>i=#(X+|2$;{iZpunjUcV;(N`P z)#}l=sySo*U0*q1_$2a)hQN(ef69*d>|83IJNrFp)T?D=zl^$g?nm{XT<7uwo9vIZ z4|<(qZCe-E`{N4iLv{EvxZLsCoL7G&Rwl35`StQ)!00m*t$n!S`OY>N0Ji4!7d&w-Ia8X0X%Cb!gYl1i=HD)9 z89BasK5`*EPTWtuu6rD(mmZ9weVni6u(679nsJ~!H+$ARU;P>J9ODz~)V@13K8Qy% zfpsi+;wgShal{X3C2*Z${Kc!-kEeO3CufXpibvi>yqEfVtUVDsMhxcpO*JF2CV8#$ zYRb-eoV8>uA)bFH?|LNfLamiz)@3(^X|KLPsu@= z(y>p(*z{iV{9Qi}WW^%P)5#06=9S_P)Hkx$e60n8-(=NzbdTRS^U|#ItlbN82B=?Q z8<+8WZfEAKc}Few_9?goBUo2;U{9wqm*iyjRgHN!x+0^n@2l}mXTgm*h5N3LoCw|x z@!1Oe#gweMo4OhHNr}VQzh+O0IkkKq>-cguuGxDmHnj|m-$CG+pR(t-(|ZeFSc>w$hnL=I<|l@elMl`(YbDC+?hCiIxw(^;`ElJqt-E7Ayje$3XVgCy|$- zql1t55j(k;UV}k_ldumfnNxvp`Wg)JPiWd4o73*m7dXawxc14aG0RN*$lb_6f&=3$ zDq;;n+9BM(`nr=V?7wi2$TRUbUcRR@@!L~{=1p?NIsZ+1Tfq%}!cIL_;e06hKX57n z15pQnXBs_C@-D1R%S|f%Q)h72VSyDmKUe)3`@(8^cg7c=5O{@nvb_&U!?O#yeD7@2 z+$CxcS#cqGT*T$kSCw>T)(3yaTI7N!ULkKii#YjI_Tzm~i;erASSU`=Ya zfX|GPL#3_@dxhVACK#OatQQul?I3@~=VK3-^P%`Hp$$v^%6fjJP)x(~!7PI(W8ABb zvG|GlGhlp0Vj5y5&U5kJ$O*ASvwqQ`b#9AX9qpcav+5UlP3NTe;l}b94D>IZDuHOvC*$9_2|P8z*<>NTlpqh_*Lff$0=i}eVeTLrV)V*QI7 z*e|yK;%>gja(QdU{LTpi6V8dli<@~5YGCpk%c)UgHvZ7bIq@Cb^s$?bvAWSl_AVt4Z#0aa6G^ z@mTd_rqB9&;9&L@PM>vQRxX@1DRi^5-lyj?&m_Lf-avA{th^Cx$6FhIADFG5$@SxW zbr|Fh`A!SKc97;zr9>k~1?Y!k-@ z9geL1gW4jnhO8K-v6FloIV##@ILqL%qs#}wzQMBq!`#C8S4NK&{p7rtM_5ZIMP9&j z#$FY${tnmjV)OK$LvI& ze-(c8NX|cxxZ)%1|IP~DF?MeSxx)pO*bH)k27d29qEF439Gg(+{mVIFb`rm31a`ib zc=CV8{V?N%m~c&@zNQyE8TF&oFS2hGUQm&`;@Wn&Lov(AT0Hg6A~{H26W)1foBzzu z;rFNcQ0hp#wvDS17r*#z#1CRnV4ZxQv&@XUuIF8;hwQv3 z=PDM1Ey=Z*%Gx}coL%kTXL#rBA~tZ|w=GZt%&FTuw2ipM***Ff&sS$p9NTNk zY5({)Cy;ZbnXPJos5yg=j`PKu)Stl@#yRqWoYQ2CE%}@MR_7NAH5`7UL`D@O^u*ZzpD~#hY{5cWJ$K!4up0?&FF5 z=0@Hszt?llAEg%J0OP≪DOH-(hZBuyX*%$^uVNZ|B>AOL(UZ*u#A8rz$W9sdebpT%c&JK?m(-!6FKBI3Yw9qI>Qq@Tri z!GmNk9^jtlgSk&3);&mUU5X7>vpN^JBKpOwd*>0GyFHFASr>VbyewyMyJoK0JI#3N z=i|o1dm_fR_ut+k<5)3UXFSP=NgVjbILCa-J`3M9cw6L9t_`&6$TfBa1}MI~`MG!C zu5ZFOE@KU~Vh=S$?z7@1b;#`-tIAz@rMi6f!&PG+4k3P6Lo8Me)^G&zL%Em@*R?0Q zHqNn+a{Kq|@Y~hwhjxJvjGIgPb z_t^U&*G0_m{$*RS!-q$XR`tzl_V8iEW94x_V!8JC*dt?4tGo#D6znK|)82=5*oec) zr55qu)yqACzbzq#eh;76Dq~H5`mAl>M=ii1OQ89QD1xj#w8F!!sL*@3Lp7g_wL` z1?t{E`pC<1e(9gopswV7G{o~k?}fZhz0*(FCyU~Iu_<+F?4d*Zjb~DYeG~)y0^64j zoKXE?_?>}2IPcpW$39bY5ZB@KSx@47Yl#8O>HdyAtPN~j+a*_1J1Iv?Y|VTM{RVu( za%|`V{Buuo$vS+{26BZo<7xc`o5;DUc+N*)MiE zC!HC3JXou1V{K?xC(T$IpXDkKZ$LwQu4Nsp{i_PqWpIH+7MIMOro5oMv z%=hJyHz%%_+%q+i#AEGY6gQatnA$aiu#K}bOH2EQY$<616y>%Xw*RSdQ7Wk3* zYz1qwfqgN6JzmQ1uP0yVMeb9=UR_6i*PZoV!n1gXXVDwntCnZlz~VVisM-{qV<$)56^ma^vO z;afUV|5e-e@5Br}BbK;k?_>BVXJEa8f4(j7oYX@!oAcX7EFm6ZFO_kHbDFG|HSJ|_ z9*;YtDxY~1;(qo?-QJ-i??3HX??*0;9AJHf=U-L+7p~J5zi)5&@8W#5)QlxuhhAS_ zW=%@J=B!EQjEnzRt9I6ypZ%^$GY)0Xz6w5lCN|?&@mZdy98158To9j``qrICIc{1D z;)waIkN)KJ!vnKYv)3MfeZ9JN#$(nToe{Z%-ZbBxJy6DP;#z9a?0#WHoUczeR#D5y zH6W+XI6>_o?iIT@pXdHZu-l|xuY?Ro3%B|FRvHt%2b!T8dzaxkEUoi*Ve7Q@Dhq#|H#37N7lXn-8FMmW#)*J4@ zSbXC5tck0c{Zxd%`N@2Wx!z)H2ct&M+y>U@d-Q4zB2OQa^Bs|27n(Dg@7r(n<+{SS zH`Z6w=;ce}S1Q0Na_X|)O&^wcoqg*~v%Fi)Yk@Bvj1QB8)Qt6VEPGw;o#wo|lktn` z*~;NE=e=~avB7asuXoM+u3ZByCl6tb{WWV}l^l3_(6j2SI%A0YjC1VAWDZGu7Kfou z9`DXL9Bi6+(SDO5#NGoV|1xG4gA#kRUM~;C{Mx#z_?|oyaa4IAVnOy>*>fj1M!f<# zTHc$V)f)GA6^}fbwVJeO?7i}{x&Nb&497!)t*<^@cqmB#lG-w-PiEJ0|(F! zxc<%M&1L*vpU=2yZP+h)S#wP5S{)_&dLTX;$kz1Ao zmV4!u?~Bux+eagBiXM{+f0z78HI8nanK&n6r&pDai|cd$(YK|Z4A@ZxeNe=*Z!+9zJWr1pqd1KOx(&uAHIKrYtCTbe~q=DapBRWVZWWS_bD zk+c8QYb>qyBs2f(;dQ{}R&PcNNS2EY1ke_DEAb&=V zv~?W0wQ9UyKj~%eTYc)T;irr-oZXpx*5g*qse7ZeE>Hz4Yw}1b4 z{9x*fXNF(+Uw!&LxkvGJN5=VWhy00LcxU9W_72-8<-1VZ&73arXN*JI#=Fqxs$*dN zP7K8QuknVlmj2P}=TEtb>m{s1Z6NhDJYQa`7^Cl0eGi|%>tXqMzvtfC5ObDe&*ZVF z1Lir}KJ%&cx9G7EqrL%#+6y~%6z_B$cDI^6asv5d9k#rNnB#cj?G2GXilsX%R*ni7 zMCQyztB99th^0@&er_fwN;)$Gh9_+n`4ah4mPL+y<&{G@Kg}{0OE*W5U!fkd^P8M~ z?Ajy#<6SsIc?5ex{T1~?inzW!ge~l^q14|NlOwJouF5B#t%&Pu&u^|=#l71{kmg@Z z>9{c`r&moFXV=PIY}33dej^wqc6AXk#SUWW5s~xSKVaYY!+$7?dtkrfgPgOojr%wW z-?11UvWn+5j6Gq$zy@N5#Entk20kWrI{Q8vu{r&)x8?{N@UvaWKT7!DTlfua*efNh zgB`>+2ifNd3o_T3{9w{+wU^&nQR-NVCzxxfHzL=L-_D+F$l-7CgRk+<{4BO#8lTl% zx}@#PJog4-)32kCLrh8CgK^W!*)#GedJs>(5xKK{7HVa9P5s5}$Lh!z8guF=9J{@S zXWhuV@6EZ(cy{Xc9GOw8L_A7vs&k6d?3-rfs!8v~eyGpqtmHi;4$LKOQ%?u|670jI z6)fju&zep6gespS7|=5Ax0*Gu47<|+mR8O4X<$z}gG(%>j(aPQ@T@Dhk+L@iTm7mi~XB#vW}{W2lnCP zvb=LOiVx+PsSmsw{*|?R`8jp?mwl`O^Jwe!&550o7ArF-u!)P|9>^=QSJz&udcLp8RYvHdijuPuSCWynFS9zSLh6CvA$);-BE%{NC@taB`7*%3Zl}=0fln`?B_9 zn_gsJ$+@yGSZ$WAte?-hP93(ao_E|LbK2!ivycBpPO<}`q&C*` zo;A%`du8|%HCOi&S0s&MdFJ*jxo(Z&H$Rv7U~=H^U$!*Xx%q;1>hc?&3;!iQ1V6!l z|0b}xOe^FY~uR&!$%Mw5F>vb*p7LVK2bi2JV|RD z=7v|5|C8U8&bNnAe_`%sE@qE|zQ#U5`C9rTH6O$_wcGY}%5T?SFuyizwddHwXnyTy z`$mo7#lVTl*%Qeh$#rBN7%{c+z&J2}HFlN|SN7n3ll@P_qNR@IAh?v{9zJ)Ywyu8QN7mU&i7dC%23_3B$(vMfFuZ>Yzm?@$u}ttv1>c~&Qb zLoCKe$ORgV{aDO=4(HLu-GEn1;DQ>bXd> z=Qcmrn>C&^QmkbgtJsh1d}niQ=NR5NGmqc$O2&R965SjzNS{R113?=i7p82i}k^keOr%e;yoycYbsU&J2H(rTVRj2Kl+;{w(~ zKi>6g*x2s;#uFkAw68#I1ZRI3-)c9^<;_#9X~H9p{6(G^T=zI`OitZ3YwcA#NpC>b zdTy#M_Rq=zkk`Sx&05Pd?_fT6cJ~XZ2G?W!^XNsKNIs|^ITQZ8b3&`JSAUE8qxwbk zvol9yc#gG+4g7ENkx@B0lRGc^Rm?oKAK%`8dI)lGa(~XLM{++6{I<^2_ReQtsYN-M zJo$X=Z<>3qo}L_U->-Yh*aL2V;1c%3(&v;lpS35>nMCHQ=o9fgla?tlRz^J*b75mh z^;JIp%|Lv?_r!damE?k~^84?LkAAAM?40Amp8I?KtnKx<&)Hr%=K)y< zx7Msq3$<15c|UvhFEL-(Yno3HuZ16{j#nWsMZE~?sy=^lMSEM|_3-?Y-mEor=X$H< zLQKJ06gOBHIvZjdaI(nP`m(owvNm1lZ%=yo2g;d6jIZ^#tgL97gN$uOr{x@Mhx$S* zV;&99o4Bq&^_+AM_MNLCU<|MAQK!XN-M#4b)qZrnIfuwz60oJL{0;gU$d|9qsy(93 zks~Ay16E~@mX$}Lz6zYOta+AkAoefboq7Gf{$Iq57<*6PmBu_5>v8)pel9SXG<)cF zFr2fnOQYE9BLef0zdsu2aaHJ}H+EU>r-HTPP>Q9u{g>g=)pPAE_mW2Cw%C zmQo>x;2z{}$9Rs~tz0wa8vi^uxyXC=IQ)Sk_nJB_YBIP6?62W@7MWLiZ`4%c9D8Ai z;{$V$)8gDMu?~M1*dMm!Z1@tRi1(%jw?VCVaSw3`d*WQ%;!(^T!iN7XK4VX@lLw>E z%Uq0paX1VYVPE=yw=4`klItyh%0u*3CC(E3-QYM`6BMWNo>L!a89K3J*dy(zPi+s} zz*tDm61}9vS3eL3J{$TW))&;0bxpeFsWoyuaX-lSgEu7?1yWP2|8|!;`{04#t~Ag8 z4!e{?b7o**1@cK|d~#gQoE?7?nnmC@*w`n)*7{)+zY)tS_c_=b>r5rJkHj&=zT|1h zIdqOWz8F7x0DHQ#0vpV8P>*O#>|ggIJxa0W;qtH!t7DJIePWNIx11k;3zk?YHszdX z_G#Jn?}x^oQj10mL7fMCuM(%FSH>D|lm3kU(EYpWnPW2Q`npGq!~3VC*c!2e{`TuO62F3H;0CRUHGd2mAHhiKF6$C0lB z4w+SlL>n&WjlKtB#nr?vH->$7_M-2RUIEr{9Wl(k!QVie5P$p%HtIxtM^#`oVo=7( z{3iC=sJM>rQEe0Aed4ElLA!%=6!e4?9pKufzFA^mu?PoEtSn zduZ&Zlc!<-nz*idCi;3fBHUMx@cZcZVqXUYR-lFkn(uLZ#Z437@eahd8$V1XPVYth zY47LR*stLc1K|J2y9Wj~&dl||*u}}c{+Wy9u&Isa+?EmXS#5mvSFMlO!!mB#7;?14 zm%v|P_exnKFSC9-gJC$&=lPv|*%zg}hnK+(hGOfI#;h2W zu?PE^ec3T$iAzWSlNhp3Tt`j*MPCi!H>DcH+P+7Gf9L&04WjY8S6J`=74edogY!L| z_a_#CZ3P!buX|3wp&^&^K_t&(kE0`>!1JANBKK9FZW zm*Wloea2aq_N3dZ>-px1azZ8+yb?Ku^NTk>H;?zKzCoW1xqRdi_Om?t$m_iOq}SSQ z*ivkkeLlvk*~o?Q!QdFnv5$$T;o5*V%5&Qr+zEB_oc-!?H9cIze(rVe5#X{(Z-iM! z#Q%F^P1+-@=EJq^Uop0zp0+a17gPRx&bRSdJZHf04}ybXFPNGP#+>Y*xTe?t_4J#G zYd(m&+xesZGmg6si4TbJzKi^2#Z6BV+pLZ}W%F}S|AK2-N3(asTGiv#4`}Cz-<u$^gFJBOu$H81Y8pS@8R$F2K214nLy*r5Fs zg{_x^5q%4`m?a+ce0wxjMQ&2lyA|(0X-Ygze~>*It`mDOovG?fV!6A;BbUW@Szl92 zK)#397f*0ziuk9W<%`Ju_}%U|h=ZJA)C}J(U*7rShehtA#;iIL;z$dgScQ!;->krf z)evj7zz@o?-^l&7h&)Km6VGWoWD$FC2bkJnQ7gZs?OI}_7J(sn5AxXU6G0P{-;hU~ z;J#QFF{BzJ_M1G6{;D{@gt8sn&vN#2nj5apxqC#6NKH{OgkMizPkgcz-|{+pD&^E@ z5oD}or`fv|*uDnhu4dSlH^`|Pa^UV-_#@6_LNo zkpPF|Su}E=E3qT??mf-AQzN&O^)LtDo3s(sK6R~{?`UuB{n6Gri$uKo9dm4K@k_SH%q{Lc5qJvGOgy7 zhkceKhpr}T(*D7I_`ak80v-WYTEa8kmFu*%KXy3v0f|FWi;4Bq?$SZT((~ee)Or

O)imD?kun19v`{I%LC;sZ|+ zw{+usN_kFy#%A})7^~cqdy(Tu2hM=bBYVAT#y%ALm*ts=n@uQtgzKw?E+%$6zr72% zi@n&V@wpGr$==>)_`R*;I-}9ra`uGZYoAWjOktik&#!Mr>~8AV2Z$37kMA0E*|qqV zqauc}59r>Hx&$6#FO2i!j9b(YbAG|Jt6yWicjvivV@;;H-ibS>R==8bM@8%b_aDF6 z3Y_E(>}`+We&|o-MydtsGxYsBn>BGJ|0llQTr6pWxQ}K(HjlM36g$)-a07XX_QN`} zz&eh#IoE*tx(lA@h3`o5A>5ic-x&wa15$Iqy7SNT%OY>Rq^&c#rexJDbxy=3ZKnsf z!5$^^S#t+xA2_oH8yIoEdWp?vIL(n+f5EerU`LT3iA$)t zYCRO2jt)iIn7x15{=XK;XN1r~HUb3$i)GA@pbIW%3L z{tQ843rO336)OgBqda=<$2)m$A+(pMjct z9=#(u>mXM(5Zu(;1D9fuP$42b+u-m>e|?=?35e+jp%%K!iX literal 0 HcmV?d00001 diff --git a/src/rc_lidar/caijian.py b/src/rc_lidar/caijian.py new file mode 100644 index 0000000..1ade55c --- /dev/null +++ b/src/rc_lidar/caijian.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2, PointField +import numpy as np +import struct +from sklearn.cluster import DBSCAN + +class LidarFilterNode(Node): + def __init__(self): + super().__init__('caijian_node') + self.publisher_ = self.create_publisher(PointCloud2, '/livox/lidar_filtered', 10) + self.subscription = self.create_subscription( + PointCloud2, + '/livox/lidar', + self.filter_callback, + 10) + self.get_logger().info('caijian_node started, numpy filtering z in [1.5,3]m, distance<=12m, remove isolated points') + + def filter_callback(self, msg): + num_points = msg.width * msg.height + data = np.frombuffer(msg.data, dtype=np.uint8) + points = np.zeros((num_points, 4), dtype=np.float32) # x, y, z, intensity + + for i in range(num_points): + offset = i * msg.point_step + x = struct.unpack_from('f', data, offset)[0] + y = struct.unpack_from('f', data, offset + 4)[0] + z = struct.unpack_from('f', data, offset + 8)[0] + intensity = struct.unpack_from('f', data, offset + 12)[0] + points[i] = [x, y, z, intensity] + + z_mask = (points[:,2] >= 1.5) & (points[:,2] <= 3.0) + dist_mask = np.linalg.norm(points[:,:3], axis=1) <= 12.0 + mask = z_mask & dist_mask + filtered_points = points[mask] + + # 使用DBSCAN去除孤立点 + if filtered_points.shape[0] > 0: + clustering = DBSCAN(eps=0.3, min_samples=5).fit(filtered_points[:,:3]) + core_mask = clustering.labels_ != -1 + filtered_points = filtered_points[core_mask] + + fields = [ + PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name='intensity', offset=12, datatype=PointField.FLOAT32, count=1), + ] + filtered_points_list = filtered_points.tolist() + import sensor_msgs_py.point_cloud2 as pc2 + filtered_msg = pc2.create_cloud(msg.header, fields, filtered_points_list) + self.publisher_.publish(filtered_msg) + +def main(args=None): + rclpy.init(args=args) + node = LidarFilterNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/circlr.py b/src/rc_lidar/circlr.py new file mode 100644 index 0000000..7942083 --- /dev/null +++ b/src/rc_lidar/circlr.py @@ -0,0 +1,90 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import PointStamped +from sensor_msgs_py import point_cloud2 as pc2 +import numpy as np +from sklearn.cluster import DBSCAN +import time + +def statistical_outlier_removal(points, k=20, std_ratio=2.0): + from sklearn.neighbors import NearestNeighbors + nbrs = NearestNeighbors(n_neighbors=k+1).fit(points) + distances, _ = nbrs.kneighbors(points) + mean_dist = np.mean(distances[:, 1:], axis=1) + threshold = np.mean(mean_dist) + std_ratio * np.std(mean_dist) + mask = mean_dist < threshold + return points[mask] + +class HoopFinder(Node): + def __init__(self): + super().__init__('find_hoop') + self.sub = self.create_subscription( + PointCloud2, + '/livox/lidar_filtered', + self.callback, + 10) + self.pub = self.create_publisher(PointStamped, '/hoop_position', 10) + self.buffer = [] + self.start_time = None + self.hoop_history = [] + + def callback(self, msg): + # 采集0.4秒内的点云 + if self.start_time is None: + self.start_time = time.time() + for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True): + self.buffer.append([p[0], p[1], p[2], p[3]]) + if time.time() - self.start_time < 0.4: + return + + points = np.array(self.buffer) + self.buffer = [] + self.start_time = None + + # 高度滤波 + filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)] + if len(filtered) == 0: + return + + # 统计离群点滤波 + filtered = statistical_outlier_removal(filtered[:,:3], k=20, std_ratio=2.0) + + # DBSCAN聚类 + clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered) + labels = clustering.labels_ + unique_labels = set(labels) + hoop_pos = None + max_cluster_size = 0 + + for label in unique_labels: + if label == -1: + continue + cluster = filtered[labels == label] + if len(cluster) > max_cluster_size: + max_cluster_size = len(cluster) + hoop_pos = np.mean(cluster, axis=0) + + # 均值滤波输出 + if hoop_pos is not None: + self.hoop_history.append(hoop_pos) + if len(self.hoop_history) > 5: + self.hoop_history.pop(0) + smooth_pos = np.mean(self.hoop_history, axis=0) + pt = PointStamped() + pt.header = msg.header + pt.point.x = float(smooth_pos[0]) + pt.point.y = float(smooth_pos[1]) + pt.point.z = float(smooth_pos[2]) + self.pub.publish(pt) + self.get_logger().info(f"Hoop position (smoothed): {smooth_pos}") + +def main(args=None): + rclpy.init(args=args) + node = HoopFinder() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/find.py b/src/rc_lidar/find.py new file mode 100644 index 0000000..9aa330b --- /dev/null +++ b/src/rc_lidar/find.py @@ -0,0 +1,59 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from geometry_msgs.msg import PointStamped +from sensor_msgs_py import point_cloud2 as pc2 +import numpy as np +from sklearn.cluster import DBSCAN + +class HoopFinder(Node): + def __init__(self): + super().__init__('find_hoop') + self.sub = self.create_subscription( + PointCloud2, + '/livox/lidar', + self.callback, + 10) + self.pub = self.create_publisher(PointStamped, '/hoop_position', 10) + + def callback(self, msg): + points = [] + for p in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True): + points.append([p[0], p[1], p[2], p[3]]) + points = np.array(points) + filtered = points[(points[:,2] > 1.0) & (points[:,2] < 3.0)] + if len(filtered) == 0: + return + + clustering = DBSCAN(eps=0.3, min_samples=10).fit(filtered[:,:3]) + labels = clustering.labels_ + unique_labels = set(labels) + hoop_pos = None + max_cluster_size = 0 + + for label in unique_labels: + if label == -1: + continue + cluster = filtered[labels == label] + if len(cluster) > max_cluster_size: + max_cluster_size = len(cluster) + hoop_pos = np.mean(cluster[:,:3], axis=0) + + if hoop_pos is not None: + pt = PointStamped() + pt.header = msg.header + pt.point.x = float(hoop_pos[0]) + pt.point.y = float(hoop_pos[1]) + pt.point.z = float(hoop_pos[2]) + self.pub.publish(pt) + self.get_logger().info(f"Hoop position: {hoop_pos}") + +def main(args=None): + rclpy.init(args=args) + node = HoopFinder() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/juxing.py b/src/rc_lidar/juxing.py new file mode 100644 index 0000000..67a2b8f --- /dev/null +++ b/src/rc_lidar/juxing.py @@ -0,0 +1,113 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +import numpy as np +import open3d as o3d +import sensor_msgs_py.point_cloud2 as pc2 +from visualization_msgs.msg import Marker +import std_msgs.msg +import geometry_msgs.msg + +class RectangleDetector(Node): + def __init__(self): + super().__init__('rectangle_detector') + self.subscription = self.create_subscription( + PointCloud2, + '/livox/lidar_filtered', + self.pointcloud_callback, + 10 + ) + self.marker_pub = self.create_publisher(Marker, '/rectangle_marker', 10) + # 可选：发布原始点云 + self.cloud_pub = self.create_publisher(PointCloud2, '/rectangle_cloud', 10) + + # 目标矩形尺寸（单位：米） + self.length = 1.8 + self.width = 1.05 + self.thickness = 0.04 + + + def publish_marker(self, bbox): + marker = Marker() + marker.header.frame_id = "livox_frame" + marker.header.stamp = self.get_clock().now().to_msg() + marker.ns = "rectangle_corners" + marker.id = 0 + marker.type = Marker.SPHERE_LIST # 或 Marker.POINTS + marker.action = Marker.ADD + + # 获取8个旋转后的角点 + corners = np.asarray(bbox.get_box_points()) + for c in corners: + pt = geometry_msgs.msg.Point() + pt.x, pt.y, pt.z = c + marker.points.append(pt) + + marker.scale.x = 0.08 # 球半径或点大小 + marker.scale.y = 0.08 + marker.scale.z = 0.08 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + marker.lifetime.sec = 1 + marker.lifetime.nanosec = 0 + self.marker_pub.publish(marker) + # ...existing code... +# ...existing code... + + def pointcloud_callback(self, msg): + points_list = [] + for p in pc2.read_points(msg, skip_nans=True): + try: + points_list.append([p[0], p[1], p[2]]) + except Exception as e: + self.get_logger().warn(f'点异常: {e}, 内容: {p}') + if len(points_list) == 0: + self.get_logger().info('点云为空') + return + points = np.array(points_list) + + # 发布原始点云到RViz + self.cloud_pub.publish(msg) + + # 转换为Open3D点云对象 + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + + # 添加体素滤波，滤波分辨率可调 + voxel_size = 0.03 # 3cm分辨率 + pcd = pcd.voxel_down_sample(voxel_size=voxel_size) + + # 使用RANSAC拟合矩形框（近似为平面+边界筛选） + plane_model, inliers = pcd.segment_plane( + distance_threshold=0.02, + ransac_n=3, + num_iterations=2000 + ) + inlier_cloud = pcd.select_by_index(inliers) + # bbox = inlier_cloud.get_axis_aligned_bounding_box() + bbox = inlier_cloud.get_oriented_bounding_box() # 修改为贴合实际方向 + extent = bbox.extent + center = bbox.center + + self.get_logger().info(f'滤波后bbox中心: {center}, 尺寸: {extent}') + + dims = sorted(extent) + if (abs(dims[0] - self.thickness) < 1.0 and + abs(dims[1] - self.width) < 1.0 and + abs(dims[2] - self.length) < 1.0): + self.get_logger().info(f'检测到目标矩形框，位置: {center}, 尺寸: {extent}') + self.publish_marker(bbox) + else: + self.get_logger().info('未检测到目标矩形框') + +def main(args=None): + rclpy.init(args=args) + node = RectangleDetector() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/pcd2pgm.py b/src/rc_lidar/pcd2pgm.py new file mode 100644 index 0000000..79d4fae --- /dev/null +++ b/src/rc_lidar/pcd2pgm.py @@ -0,0 +1,75 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from nav_msgs.msg import OccupancyGrid +import numpy as np +import struct +import time + +class PointCloudToGrid(Node): + def __init__(self): + super().__init__('pointcloud_to_grid') + self.subscription = self.create_subscription( + PointCloud2, + '/livox/lidar_filtered', + self.pointcloud_callback, + 10) + self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10) + self.grid_size = 2000 + self.resolution = 0.02 + self.origin_x = -20.0 + self.origin_y = -20.0 + self.points_buffer = [] + self.last_header = None + # 定时器每0.5秒触发一次 + self.timer = self.create_timer(0.5, self.publish_grid) + + def pointcloud_callback(self, msg): + points = self.pointcloud2_to_xyz_array(msg) + self.points_buffer.append(points) + self.last_header = msg.header # 保存最新header用于地图消息 + + def publish_grid(self): + if not self.points_buffer: + return + # 合并0.5秒内所有点 + all_points = np.concatenate(self.points_buffer, axis=0) + grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8) + for x, y, z in all_points: + if z < 2.0: + ix = int((x - self.origin_x) / self.resolution) + iy = int((y - self.origin_y) / self.resolution) + if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size: + grid[iy, ix] = 100 + grid_msg = OccupancyGrid() + if self.last_header: + grid_msg.header = self.last_header + grid_msg.info.resolution = self.resolution + grid_msg.info.width = self.grid_size + grid_msg.info.height = self.grid_size + grid_msg.info.origin.position.x = self.origin_x + grid_msg.info.origin.position.y = self.origin_y + grid_msg.data = grid.flatten().tolist() + self.publisher.publish(grid_msg) + self.points_buffer.clear() # 清空缓存 + + def pointcloud2_to_xyz_array(self, cloud_msg): + # 解析 PointCloud2 数据为 numpy 数组 + fmt = 'fff' # x, y, z + point_step = cloud_msg.point_step + data = cloud_msg.data + points = [] + for i in range(0, len(data), point_step): + x, y, z = struct.unpack_from(fmt, data, i) + points.append([x, y, z]) + return np.array(points) + +def main(args=None): + rclpy.init(args=args) + node = PointCloudToGrid() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/save_pcd.py b/src/rc_lidar/save_pcd.py new file mode 100644 index 0000000..25301c3 --- /dev/null +++ b/src/rc_lidar/save_pcd.py @@ -0,0 +1,55 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +import numpy as np +import open3d as o3d +import time + +class PointCloudSaver(Node): + def __init__(self): + super().__init__('pcd_saver') + self.subscription = self.create_subscription( + PointCloud2, + '/livox/lidar_filtered', + self.listener_callback, + 10) + self.point_clouds = [] + self.start_time = time.time() + self.timer = self.create_timer(3.0, self.save_and_exit) + self.saving = False + + def listener_callback(self, msg): + if not self.saving: + pc = self.pointcloud2_to_xyz_array(msg) + if pc is not None: + self.point_clouds.append(pc) + + def pointcloud2_to_xyz_array(self, cloud_msg): + # 仅支持 x, y, z 均为 float32 且无 padding 的点云 + import struct + points = [] + point_step = cloud_msg.point_step + for i in range(cloud_msg.width * cloud_msg.height): + offset = i * point_step + x, y, z = struct.unpack_from('fff', cloud_msg.data, offset) + points.append([x, y, z]) + return np.array(points) + + def save_and_exit(self): + if not self.saving: + self.saving = True + if self.point_clouds: + all_points = np.vstack(self.point_clouds) + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(all_points) + o3d.io.write_point_cloud("output.pcd", pcd) + self.get_logger().info("Saved output.pcd") + rclpy.shutdown() + +def main(args=None): + rclpy.init(args=args) + saver = PointCloudSaver() + rclpy.spin(saver) + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/simple_icp.py b/src/rc_lidar/simple_icp.py new file mode 100644 index 0000000..c1dffdc --- /dev/null +++ b/src/rc_lidar/simple_icp.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from sensor_msgs_py import point_cloud2 +from geometry_msgs.msg import PoseStamped +import open3d as o3d +import numpy as np +from transforms3d.quaternions import mat2quat + +class PointCloudLocalization(Node): + def __init__(self): + super().__init__('point_cloud_localizer') + + # 加载参考点云地图 (PCD文件) + self.reference_map = o3d.io.read_point_cloud("/home/robofish/RC2025/lankuang.pcd") # 替换为你的PCD文件路径 + if not self.reference_map.has_points(): + self.get_logger().error("Failed to load reference map!") + rclpy.shutdown() + + # 预处理参考地图 + self.reference_map = self.reference_map.voxel_down_sample(voxel_size=0.05) + self.reference_map.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)[0] + + # 创建ICP对象 + self.icp = o3d.pipelines.registration.registration_icp + self.threshold = 0.5 # 匹配距离阈值 (米) + self.trans_init = np.identity(4) # 初始变换矩阵 + + # 订阅激光雷达点云 + self.subscription = self.create_subscription( + PointCloud2, + '/livox/lidar_filtered', + self.lidar_callback, + 10) + + # 发布估计位置 + self.pose_pub = self.create_publisher(PoseStamped, '/estimated_pose', 10) + + self.get_logger().info("Point Cloud Localization Node Initialized!") + + def ros_pc2_to_o3d(self, ros_cloud): + """将ROS PointCloud2转换为Open3D点云""" + # 提取xyz坐标 + points = point_cloud2.read_points(ros_cloud, field_names=("x", "y", "z"), skip_nans=True) + xyz = np.array([ [p[0], p[1], p[2]] for p in points ], dtype=np.float32) + + if xyz.shape[0] == 0: + return None + + # 创建Open3D点云 + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(xyz) + return pcd + + def preprocess_pointcloud(self, pcd): + """点云预处理""" + # 降采样 + pcd = pcd.voxel_down_sample(voxel_size=0.03) + + # 移除离群点 + pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=1.0) + + # 移除地面 (可选) + # plane_model, inliers = pcd.segment_plane(distance_threshold=0.1, ransac_n=3, num_iterations=100) + # pcd = pcd.select_by_index(inliers, invert=True) + + return pcd + + def lidar_callback(self, msg): + """处理新的激光雷达数据""" + # 转换为Open3D格式 + current_pcd = self.ros_pc2_to_o3d(msg) + if current_pcd is None: + self.get_logger().warn("Received empty point cloud!") + return + + # 预处理当前点云 + current_pcd = self.preprocess_pointcloud(current_pcd) + + # 执行ICP配准 + reg_result = self.icp( + current_pcd, self.reference_map, self.threshold, + self.trans_init, + o3d.pipelines.registration.TransformationEstimationPointToPoint(), + o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=50) + ) + + # 更新变换矩阵 + self.trans_init = reg_result.transformation + + # 提取位置和方向 + translation = reg_result.transformation[:3, 3] + rotation_matrix = reg_result.transformation[:3, :3] + + # 转换为四元数 + quaternion = mat2quat(reg_result.transformation[:3, :3]) # 注意返回顺序为 [w, x, y, z] + + # 发布位姿 + pose_msg = PoseStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = "livox_frame" + pose_msg.pose.position.x = translation[0] + pose_msg.pose.position.y = translation[1] + pose_msg.pose.position.z = translation[2] + pose_msg.pose.orientation.x = quaternion[1] # x + pose_msg.pose.orientation.y = quaternion[2] # y + pose_msg.pose.orientation.z = quaternion[3] # z + pose_msg.pose.orientation.w = quaternion[0] # w + + self.pose_pub.publish(pose_msg) + self.get_logger().info(f"Estimated Position: x={translation[0]:.2f}, y={translation[1]:.2f}, z={translation[2]:.2f}") + +def main(args=None): + rclpy.init(args=args) + node = PointCloudLocalization() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/xiamian.py b/src/rc_lidar/xiamian.py new file mode 100644 index 0000000..79d4fae --- /dev/null +++ b/src/rc_lidar/xiamian.py @@ -0,0 +1,75 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 +from nav_msgs.msg import OccupancyGrid +import numpy as np +import struct +import time + +class PointCloudToGrid(Node): + def __init__(self): + super().__init__('pointcloud_to_grid') + self.subscription = self.create_subscription( + PointCloud2, + '/livox/lidar_filtered', + self.pointcloud_callback, + 10) + self.publisher = self.create_publisher(OccupancyGrid, '/lidar_grid', 10) + self.grid_size = 2000 + self.resolution = 0.02 + self.origin_x = -20.0 + self.origin_y = -20.0 + self.points_buffer = [] + self.last_header = None + # 定时器每0.5秒触发一次 + self.timer = self.create_timer(0.5, self.publish_grid) + + def pointcloud_callback(self, msg): + points = self.pointcloud2_to_xyz_array(msg) + self.points_buffer.append(points) + self.last_header = msg.header # 保存最新header用于地图消息 + + def publish_grid(self): + if not self.points_buffer: + return + # 合并0.5秒内所有点 + all_points = np.concatenate(self.points_buffer, axis=0) + grid = np.zeros((self.grid_size, self.grid_size), dtype=np.int8) + for x, y, z in all_points: + if z < 2.0: + ix = int((x - self.origin_x) / self.resolution) + iy = int((y - self.origin_y) / self.resolution) + if 0 <= ix < self.grid_size and 0 <= iy < self.grid_size: + grid[iy, ix] = 100 + grid_msg = OccupancyGrid() + if self.last_header: + grid_msg.header = self.last_header + grid_msg.info.resolution = self.resolution + grid_msg.info.width = self.grid_size + grid_msg.info.height = self.grid_size + grid_msg.info.origin.position.x = self.origin_x + grid_msg.info.origin.position.y = self.origin_y + grid_msg.data = grid.flatten().tolist() + self.publisher.publish(grid_msg) + self.points_buffer.clear() # 清空缓存 + + def pointcloud2_to_xyz_array(self, cloud_msg): + # 解析 PointCloud2 数据为 numpy 数组 + fmt = 'fff' # x, y, z + point_step = cloud_msg.point_step + data = cloud_msg.data + points = [] + for i in range(0, len(data), point_step): + x, y, z = struct.unpack_from(fmt, data, i) + points.append([x, y, z]) + return np.array(points) + +def main(args=None): + rclpy.init(args=args) + node = PointCloudToGrid() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/rc_lidar/zhaoban.py b/src/rc_lidar/zhaoban.py new file mode 100644 index 0000000..556027e --- /dev/null +++ b/src/rc_lidar/zhaoban.py @@ -0,0 +1,51 @@ +import numpy as np +from sklearn.cluster import DBSCAN +from scipy.optimize import leastsq +import matplotlib.pyplot as plt + +def fit_circle(x, y): + # 拟合圆的函数 + def calc_R(xc, yc): + return np.sqrt((x - xc)**2 + (y - yc)**2) + def f(c): + Ri = calc_R(*c) + return Ri - Ri.mean() + center_estimate = np.mean(x), np.mean(y) + center, _ = leastsq(f, center_estimate) + radius = calc_R(*center).mean() + return center[0], center[1], radius + +def find_circle(points, eps=0.5, min_samples=10): + # 聚类 + clustering = DBSCAN(eps=eps, min_samples=min_samples).fit(points) + labels = clustering.labels_ + # 只取最大簇 + unique, counts = np.unique(labels[labels != -1], return_counts=True) + if len(unique) == 0: + raise ValueError("未找到有效聚类") + main_cluster = unique[np.argmax(counts)] + cluster_points = points[labels == main_cluster] + x, y = cluster_points[:, 0], cluster_points[:, 1] + # 拟合圆 + xc, yc, r = fit_circle(x, y) + return xc, yc, r, cluster_points + +if __name__ == "__main__": + # 示例数据 + np.random.seed(0) + angle = np.linspace(0, 2 * np.pi, 100) + x = 5 + 3 * np.cos(angle) + np.random.normal(0, 0.1, 100) + y = 2 + 3 * np.sin(angle) + np.random.normal(0, 0.1, 100) + points = np.vstack((x, y)).T + + xc, yc, r, cluster_points = find_circle(points) + print(f"圆心: ({xc:.2f}, {yc:.2f}), 半径: {r:.2f}") + + # 可视化 + plt.scatter(points[:, 0], points[:, 1], label='所有点') + plt.scatter(cluster_points[:, 0], cluster_points[:, 1], label='聚类点') + circle = plt.Circle((xc, yc), r, color='r', fill=False, label='拟合圆') + plt.gca().add_patch(circle) + plt.legend() + plt.axis('equal') + plt.show() \ No newline at end of file diff --git a/src/rc_lidar/zhaoyuan.py b/src/rc_lidar/zhaoyuan.py new file mode 100644 index 0000000..e69de29 diff --git a/src/rm_driver/rm_serial_driver/script/R2_Serial.py b/src/rm_driver/rm_serial_driver/script/R2_Serial.py index c9069d1..d9896bb 100644 --- a/src/rm_driver/rm_serial_driver/script/R2_Serial.py +++ b/src/rm_driver/rm_serial_driver/script/R2_Serial.py @@ -135,6 +135,9 @@ class AimDataSerial(Node): packet.extend(struct.pack(' - -# NOTE: The build lifecycle on Travis.ci is something like this: -# before_install -# install -# before_script -# script -# after_success or after_failure -# after_script -# OPTIONAL before_deploy -# OPTIONAL deploy -# OPTIONAL after_deploy - -################################################################################ - -# Use ubuntu trusty (14.04) with sudo privileges. -dist: trusty -sudo: required -language: - - generic -cache: - - apt - -# Configuration variables. All variables are global now, but this can be used to -# trigger a build matrix for different ROS distributions if desired. -env: - global: - - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] - - CI_SOURCE_PATH=$(pwd) - - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall - - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options - - ROS_PARALLEL_JOBS='-j8 -l6' - matrix: - - ROS_DISTRO=indigo - - ROS_DISTRO=jade - - -################################################################################ - -# Install system dependencies, namely a very barebones ROS setup. -before_install: - - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" - - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - - sudo apt-get update -qq - - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin - - source /opt/ros/$ROS_DISTRO/setup.bash - # Prepare rosdep to install dependencies. - - sudo rosdep init - - rosdep update - -# Create a catkin workspace with the package under integration. -install: - - mkdir -p ~/catkin_ws/src - - cd ~/catkin_ws/src - - catkin_init_workspace - # Create the devel/setup.bash (run catkin_make with an empty workspace) and - # source it to set the path variables. - - cd ~/catkin_ws - - catkin_make - - source devel/setup.bash - # Add the package under integration to the workspace using a symlink. - - cd ~/catkin_ws/src - - ln -s $CI_SOURCE_PATH . - -# Install all dependencies, using wstool and rosdep. -# wstool looks for a ROSINSTALL_FILE defined in the environment variables. -before_script: - # source dependencies: install using wstool. - - cd ~/catkin_ws/src - - wstool init - - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi - - wstool up - # package depdencies: install using rosdep. - - cd ~/catkin_ws - - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO - -# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to -# catkin_make. -script: - - cd ~/catkin_ws - - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) - # Testing: Use both run_tests (to see the output) and test (to error out). - - catkin_make run_tests # This always returns 0, but looks pretty. - - catkin_make test # This will return non-zero if a test fails. - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/README.md b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/README.md deleted file mode 100644 index 5028ed4..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/README.md +++ /dev/null @@ -1,30 +0,0 @@ -costmap_converter ROS Package -============================= - -A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types - -Build status of the *master* branch: -- ROS Buildfarm Kinetic: [![Kinetic Build Status](http://build.ros.org/buildStatus/icon?job=Kdev__costmap_converter__ubuntu_xenial_amd64)](http://build.ros.org/job/Kdev__costmap_converter__ubuntu_xenial_amd64/) -- ROS Buildfarm Indigo: [![Indigo Build Status](http://build.ros.org/buildStatus/icon?job=Idev__costmap_converter__ubuntu_trusty_amd64)](http://build.ros.org/job/Idev__costmap_converter__ubuntu_trusty_amd64/) - - -### Contributors - -- Christoph Rösmann -- Franz Albers (*CostmapToDynamicObstacles* plugin) -- Otniel Rinaldo - - -### License - -The *costmap_converter* package is licensed under the BSD license. -It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed. - -Some third-party dependencies are included that are licensed under different terms: - - *MultitargetTracker*, GNU GPLv3, https://github.com/Smorodov/Multitarget-tracker - (partially required for the *CostmapToDynamicObstacles* plugin) - -All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details. - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CHANGELOG.rst b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CHANGELOG.rst deleted file mode 100644 index 0b7e1b5..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CHANGELOG.rst +++ /dev/null @@ -1,105 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package costmap_converter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.2 (2020-06-22) ------------------- -* OpenCV 4 compatibility fix -* Contributors: daviddudas - -0.1.1 (2020-01-25) ------------------- -* Fixed ament plugin export -* Revert release-mode for cmake build -* Contributors: Christoph Rösmann - -0.1.0 (2020-01-23) ------------------- -* Port to ROS2 (thanks to Vinnam Kim and stevemacenski) -* Messages moved to a separate package -* Contributors: Christoph Rösmann, Vinnam Kim, stevemacenski - -0.0.12 (2019-12-02) -------------------- -* CostmapToPolygons: Simplification of the polygon by Douglas-Peucker algorithm (reduces the density of points in the polygon). -* Bugfixes -* Contributors: Rainer Kümmerle - -0.0.11 (2019-10-26) -------------------- -* rostest integration to avoid running a roscore separately for unit testing -* Contributors: Christoph Rösmann - -0.0.10 (2019-10-26) -------------------- -* Runtime improvements for CostmapToPolygonsDBSMCCH (`#12 `_) - * Grid lookup for regionQuery - * use a grid structure for looking up nearest neighbors - * parameters in a struct - * guard the parameters by drawing a copy from dynamic reconfigure - * Adding some test cases for regionQuery and dbScan - * Avoid computing sqrt at the end of convexHull2 - * Add doxygen comments for the neighbor lookup - * Change the param read to one liners - * Add test on empty map for dbScan -* Contributors: Rainer Kümmerle - -0.0.9 (2018-05-28) ------------------- -* Moved plugin loader for static costmap conversion to BaseCostmapToDynamicObstacles. - The corresponding ROS parameter `static_converter_plugin` is now defined in the CostmapToDynamicObstacles namespace. -* Contributors: Christoph Rösmann - -0.0.8 (2018-05-17) ------------------- -* Standalone converter subscribes now to costmap updates. Fixes `#1 `_ -* Adds radius field for circular obstacles to ObstacleMsg -* Stacked costmap conversion (`#7 `_). - E.g., it is now possible combine a dynamic obstacle and static obstacle converter plugin. -* Contributors: Christoph Rösmann, Franz Albers - -0.0.7 (2017-09-20) ------------------- -* Fixed some compilation issues (C++11 compiler flags and opencv2 on indigo/jade). -* Dynamic obstacle plugin: obstacle velocity is now published for both x and y coordinates rather than the absolute value - -0.0.6 (2017-09-18) ------------------- -* This pull request adds the costmap to dynamic obstacles plugin (written by Franz Albers). - It detects the dynamic foreground of the costmap (based on the temporal evolution of the costmap) - including blobs representing the obstacles. Furthermore, Kalman-based tracking is applied to estimate - the current velocity for each obstacle. - **Note, this plugin is still experimental.** -* New message types are introduced: costmap\_converter::ObstacleMsg and costmap\_converter::ObstacleArrayMsg. - These types extend the previous polygon representation by additional velocity, orientation and id information. -* The API has been extended to provide obstacles via the new ObstacleArrayMsg type instead of vector of polygons. -* Contributors: Franz Albers, Christoph Rösmann - -0.0.5 (2016-02-01) ------------------- -* Major changes regarding the line detection based on the convex hull - (it should be much more robust now). -* Concave hull plugin added. -* The cluster size can now be limited from above using a specific parameter. - This implicitly avoids large clusters forming a 'L' or 'U'. -* All parameters can now be adjusted using dynamic_reconfigure (rqt_reconfigure). -* Some parameter names changed. -* Line plugin based on ransac: line inliers must now be placed inbetween the start and end of a line. - -0.0.4 (2016-01-11) ------------------- -* Fixed conversion from map to world coordinates if the costmap is not quadratic. - -0.0.3 (2015-12-23) ------------------- -* The argument list of the initialize method requires a nodehandle from now on. This facilitates the handling of parameter namespaces for multiple instantiations of the plugin. -* This change is pushed immediately as a single release to avoid API breaks (since version 0.0.2 is not on the official repos up to now). - -0.0.2 (2015-12-22) ------------------- -* Added a plugin for converting the costmap to lines using ransac - -0.0.1 (2015-12-21) ------------------- -* First release of the package including a pluginlib interface, two plugins (costmap to polygons and costmap to lines) and a standalone conversion node. - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CMakeLists.txt b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CMakeLists.txt deleted file mode 100644 index 90429ca..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/CMakeLists.txt +++ /dev/null @@ -1,97 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(costmap_converter) - -# Set to Release in order to speed up the program significantly -set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(class_loader REQUIRED) -find_package(costmap_converter_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(OpenCV REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) - -set(dependencies - class_loader - costmap_converter_msgs - cv_bridge - geometry_msgs - nav2_costmap_2d - tf2 - tf2_geometry_msgs - tf2_ros - OpenCV - pluginlib - rclcpp -) - -include_directories( - include -) - -add_library(costmap_converter SHARED - src/costmap_to_polygons.cpp - src/costmap_to_polygons_concave.cpp - src/costmap_to_lines_convex_hull.cpp - src/costmap_to_lines_ransac.cpp - src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp - src/costmap_to_dynamic_obstacles/background_subtractor.cpp - src/costmap_to_dynamic_obstacles/blob_detector.cpp - src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp - src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp - src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp -) - -ament_target_dependencies(costmap_converter - ${dependencies} -) - -add_executable(standalone_converter src/costmap_converter_node.cpp) - -target_link_libraries(standalone_converter - costmap_converter -) -ament_target_dependencies(standalone_converter - ${dependencies} -) - -install(TARGETS costmap_converter - DESTINATION lib -) -install(TARGETS standalone_converter - DESTINATION bin -) - -install(DIRECTORY include/ - DESTINATION include/ -) - -ament_export_include_directories(include) -ament_export_libraries(costmap_converter) -ament_export_dependencies(${dependencies}) -pluginlib_export_plugin_description_file(costmap_converter costmap_converter_plugins.xml) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_costmap_polygons test/costmap_polygons.cpp) - if(TARGET test_costmap_polygons) - target_link_libraries(test_costmap_polygons costmap_converter) - endif() - ament_target_dependencies(test_costmap_polygons ${dependencies}) -endif() - -ament_package() diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg deleted file mode 100755 index 7e18a9e..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToDynamicObstacles.cfg +++ /dev/null @@ -1,136 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - -################################################################## -###################### Foreground detection ###################### -gen.add("alpha_slow", double_t, 0, - "Foreground detection: Learning rate of the slow filter", - 0.3, 0.0, 1.0) - -gen.add("alpha_fast", double_t, 0, - "Foreground detection: Learning rate of the fast filter", - 0.85, 0.0, 1.0) - -gen.add("beta", double_t, 0, - "Foreground detection: Weighting coefficient between a pixels value and the mean of its nearest neighbors", - 0.85, 0.0, 1.0) - -gen.add("min_sep_between_slow_and_fast_filter", int_t, 0, - "Foreground detection: Minimal difference between the fast and the slow filter to recognize a obstacle as dynamic", - 80, 0, 255) - -gen.add("min_occupancy_probability", int_t, 0, - "Foreground detection: Minimal value of the fast filter to recognize a obstacle as dynamic", - 180, 0, 255) - -gen.add("max_occupancy_neighbors", int_t, 0, - "Foreground detection: Maximal mean value of the nearest neighbors of a pixel in the slow filter", - 80, 0, 255) - -gen.add("morph_size", int_t, 0, - "Foreground detection: Size of the structuring element for the closing operation", - 1, 0, 10) - -gen.add("publish_static_obstacles", bool_t, 0, - "Include static obstacles as single-point polygons", - True) - -############################################################ -###################### Blob detection ###################### - -# These parameters are commented out, because the input image for the blob detection is already binary -> irrelevant -#gen.add("threshold_step", double_t, 0, -# "Blob detection: Distance between neighboring thresholds", -# 256.0, 0.0, 256.0) -# -#gen.add("min_threshold", double_t, 0, -# "Blob detection: Convert the source image to binary images by applying several thresholds, starting at min_threshold", -# 1, 0, 255) -# -#gen.add("max_threshold", double_t, 0, -# "Blob detection: Convert the source image to binary images by applying several thresholds, ending at max_threshold", -# 255, 0, 255) -# -#gen.add("min_repeatability", int_t, 0, -# "Blob detection: Minimal number of detections of a blob in the several thresholds to be considered as real blob", -# 1, 1, 10) -# -gen.add("min_distance_between_blobs", double_t, 0, - "Blob detection: Minimal distance between centers of two blobs to be considered as seperate blobs", - 10, 0.0, 300.0) - -gen.add("filter_by_area", bool_t, 0, - "Blob detection: Filter blobs based on number of pixels", - True) - -gen.add("min_area", int_t, 0, - "Blob detection: Minimal number of pixels a blob consists of", - 3, 0, 300) - -gen.add("max_area", int_t, 0, - "Blob detection: Maximal number of pixels a blob consists of", - 300, 0, 300) - -gen.add("filter_by_circularity", bool_t, 0, - "Blob detection: Filter blobs based on their circularity", - True) - -gen.add("min_circularity", double_t, 0, - "Blob detection: Minimal circularity value (0 in case of a line)", - 0.2, 0.0, 1.0) - -gen.add("max_circularity", double_t, 0, - "Blob detection: Maximal circularity value (1 in case of a circle)", - 1.0, 0.0, 1.0) - -gen.add("filter_by_inertia", bool_t, 0, - "Blob detection: Filter blobs based on their inertia ratio", - True) - -gen.add("min_inertia_ratio", double_t, 0, - "Blob detection: Minimal inertia ratio", - 0.2, 0.0, 1.0) - -gen.add("max_inertia_ratio", double_t, 0, - "Blob detection: Maximal inertia ratio", - 1.0, 0.0, 1.0) - -gen.add("filter_by_convexity", bool_t, 0, - "Blob detection: Filter blobs based on their convexity (Blob area / area of its convex hull)", - False) - -gen.add("min_convexity", double_t, 0, - "Blob detection: Minimum convexity ratio", - 0.0, 0.0, 1.0) - -gen.add("max_convexity", double_t, 0, - "Blob detection: Maximal convexity ratio", - 1.0, 0.0, 1.0) - -################################################################ -#################### Tracking ################################## -gen.add("dt", double_t, 0, - "Tracking: Time for one timestep of the kalman filter", - 0.2, 0.1, 3.0) - -gen.add("dist_thresh", double_t, 0, - "Tracking: Maximum distance between two points to be considered in the assignment problem", - 20.0, 0.0, 150.0) - -gen.add("max_allowed_skipped_frames", int_t, 0, - "Tracking: Maximum number of frames a object is tracked while it is not seen", - 3, 0, 10) - -gen.add("max_trace_length", int_t, 0, - "Tracking: Maximum number of Points in a objects trace", - 10, 1, 100) - -exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToDynamicObstacles")) diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg deleted file mode 100755 index 1dd917c..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSMCCH.cfg +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -gen.add("cluster_max_distance", double_t, 0, - "Parameter for DB_Scan, maximum distance to neighbors [m]", - 0.4, 0.0, 10.0) - -gen.add("cluster_min_pts", int_t, 0, - "Parameter for DB_Scan: minimum number of points that define a cluster", - 2, 1, 20) - -gen.add("cluster_max_pts", int_t, 0, - "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)", - 30, 2, 200) - -gen.add("convex_hull_min_pt_separation", double_t, 0, - "Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)", - 0.1, 0.0, 10.0) - -gen.add("support_pts_max_dist", double_t, 0, - "Minimum distance from a point to the line to be counted as support point", - 0.3, 0.0, 10.0) - -gen.add("support_pts_max_dist_inbetween", double_t, 0, - "A line is only defined, if the distance between two consecutive support points is less than this treshold. Set to 0 in order to deactivate this check.", - 1.0, 0.0, 10.0) - -gen.add("min_support_pts", int_t, 0, - "Minimum number of support points to represent a line", - 2, 0, 50) - -exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSMCCH")) diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg deleted file mode 100755 index 2c5b322..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToLinesDBSRANSAC.cfg +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -gen.add("cluster_max_distance", double_t, 0, - "Parameter for DB_Scan, maximum distance to neighbors [m]", - 0.4, 0.0, 10.0) - -gen.add("cluster_min_pts", int_t, 0, - "Parameter for DB_Scan: minimum number of points that define a cluster", - 2, 1, 20) - -gen.add("cluster_max_pts", int_t, 0, - "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)", - 30, 2, 200) - -gen.add("ransac_inlier_distance", double_t, 0, - "Maximum distance to the line segment for inliers", - 0.2, 0.0, 10.0) - -gen.add("ransac_min_inliers", int_t, 0, - "Minimum numer of inliers required to form a line", - 10, 0, 100) - -gen.add("ransac_no_iterations", int_t, 0, - "Number of ransac iterations", - 2000, 1, 10000) - -gen.add("ransac_remainig_outliers", int_t, 0, - "Repeat ransac until the number of outliers is as specified here", - 3, 0, 50) - -gen.add("ransac_convert_outlier_pts", bool_t, 0, - "Convert remaining outliers to single points.", - True) - -gen.add("ransac_filter_remaining_outlier_pts", bool_t, 0, - "Filter the interior of remaining outliers and keep only keypoints of their convex hull", - False) - -gen.add("convex_hull_min_pt_separation", double_t, 0, - "Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)", - 0.1, 0.0, 10.0) - - -exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToLinesDBSRANSAC")) diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg deleted file mode 100755 index 7e4d49f..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSConcaveHull.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -gen.add("cluster_max_distance", double_t, 0, - "Parameter for DB_Scan, maximum distance to neighbors [m]", - 0.4, 0.0, 10.0) - -gen.add("cluster_min_pts", int_t, 0, - "Parameter for DB_Scan: minimum number of points that define a cluster", - 2, 1, 20) - -gen.add("cluster_max_pts", int_t, 0, - "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)", - 30, 2, 200) - -gen.add("convex_hull_min_pt_separation", double_t, 0, - "Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)", - 0.1, 0.0, 10.0) - -gen.add("concave_hull_depth", double_t, 0, - "Smaller depth: sharper surface, depth -> high value: convex hull", - 2.0, 0.0, 100.0) - -exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSConcaveHull")) diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg deleted file mode 100755 index d5cc11a..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/cfg/dynamic_reconfigure/CostmapToPolygonsDBSMCCH.cfg +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -gen.add("cluster_max_distance", double_t, 0, - "Parameter for DB_Scan, maximum distance to neighbors [m]", - 0.4, 0.0, 10.0) - -gen.add("cluster_min_pts", int_t, 0, - "Parameter for DB_Scan: minimum number of points that define a cluster", - 2, 1, 20) - -gen.add("cluster_max_pts", int_t, 0, - "Parameter for DB_Scan: maximum number of points that define a cluster (limit cluster size to avoid large L- and U-shapes)", - 30, 2, 200) - -gen.add("convex_hull_min_pt_separation", double_t, 0, - "Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all)", - 0.1, 0.0, 10.0) - -exit(gen.generate("costmap_converter", "standalone_converter", "CostmapToPolygonsDBSMCCH")) diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/costmap_converter_plugins.xml b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/costmap_converter_plugins.xml deleted file mode 100644 index 6930cd1..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/costmap_converter_plugins.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - - This class converts costmap2d obstacles into a vector of convex polygons. - Clustering is performed with DBScan. Convex polygons are detected using the Monotone Chain Convex Hull algorithm. - - - - - - This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). - Clustering is performed with DBScan. Clusters are transformed into convex polygons using the Monotone Chain Convex Hull algorithm. - The resulting keypoints are used for possible line candidates. - A line is only defined if there exist a specified number of support points between each keypoint pair. - - - - - - This class converts costmap2d obstacles into a vector of lines (represented as polygon msg). - Clustering is performed with DBScan. For each cluster RANSAC is applied sequentally to fit multiple line models. - - - - - - This class converts costmap2d obstacles into a vector of non-convex (concave) polygons. - - - - - - This class detects and tracks obstacles from a sequence of costmaps. - - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_converter_interface.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_converter_interface.h deleted file mode 100644 index 6e46375..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_converter_interface.h +++ /dev/null @@ -1,389 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016 - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann, Otniel Rinaldo - *********************************************************************/ - -#ifndef COSTMAP_CONVERTER_INTERFACE_H_ -#define COSTMAP_CONVERTER_INTERFACE_H_ - -//#include -#include -#include - -#include -#include -#include -#include -#include -#include - -namespace costmap_converter -{ - -//! Typedef for a shared dynamic obstacle container -typedef costmap_converter_msgs::msg::ObstacleArrayMsg::SharedPtr ObstacleArrayPtr; -//! Typedef for a shared dynamic obstacle container (read-only access) -typedef costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr ObstacleArrayConstPtr; - -//! Typedef for a shared polygon container -typedef std::shared_ptr> PolygonContainerPtr; -//! Typedef for a shared polygon container (read-only access) -typedef std::shared_ptr> PolygonContainerConstPtr; - - -/** - * @class BaseCostmapToPolygons - * @brief This abstract class defines the interface for plugins that convert the costmap into polygon types - * - * Plugins must accept a nav2_costmap_2d::Costmap2D datatype as information source. - * The interface provides two different use cases: - * 1. Manual call to conversion routines: setCostmap2D(), compute() and getPolygons() - * (in subsequent calls setCostmap2D() can be substituted by updateCostmap2D()) - * 2. Repeatedly process costmap with a specific rate (startWorker() and stopWorker()): - * Make sure that the costmap is valid while the worker is active (you can specify your own spinner or activate a threaded spinner). - * Costmaps can be obtained by invoking getPolygons(). - */ -class BaseCostmapToPolygons -{ -public: - - /** - * @brief Initialize the plugin - * @param nh Nodehandle that defines the namespace for parameters - */ - virtual void initialize(rclcpp::Node::SharedPtr nh) { - nh_ = nh; - } - - /** - * @brief Destructor - */ - virtual ~BaseCostmapToPolygons() - { - stopWorker(); - } - - - /** - * @brief Pass a pointer to the costap to the plugin. - * @warning The plugin should handle the costmap's mutex locking. - * @sa updateCostmap2D - * @param costmap Pointer to the costmap2d source - */ - virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap) = 0; - - /** - * @brief Get updated data from the previously set Costmap2D - * @warning The plugin should handle the costmap's mutex locking. - * @sa setCostmap2D - */ - virtual void updateCostmap2D() = 0; - - /** - * @brief This method performs the actual work (conversion of the costmap to polygons) - */ - virtual void compute() = 0; - - /** - * @brief Get a shared instance of the current polygon container - * - * If this method is not implemented by any subclass (plugin) the returned shared - * pointer is empty. - * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance! - * @warning The underlying plugin must ensure that this method is thread safe. - * @return Shared instance of the current polygon container - */ - virtual PolygonContainerConstPtr getPolygons(){return PolygonContainerConstPtr();} - - /** - * @brief Get a shared instance of the current obstacle container - * If this method is not overwritten by the underlying plugin, the obstacle container just imports getPolygons(). - * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance! - * @warning The underlying plugin must ensure that this method is thread safe. - * @return Shared instance of the current obstacle container - * @sa getPolygons - */ - virtual ObstacleArrayConstPtr getObstacles() - { - ObstacleArrayPtr obstacles = std::make_shared(); - PolygonContainerConstPtr polygons = getPolygons(); - if (polygons) - { - for (const geometry_msgs::msg::Polygon& polygon : *polygons) - { - obstacles->obstacles.emplace_back(); - obstacles->obstacles.back().polygon = polygon; - } - } - return obstacles; - } - - /** - * @brief Set name of robot's odometry topic - * - * Some plugins might require odometry information - * to compensate the robot's ego motion - * @param odom_topic topic name - */ - virtual void setOdomTopic(const std::string& odom_topic) { (void)odom_topic; } - - /** - * @brief Determines whether an additional plugin for subsequent costmap conversion is specified - * - * @return false, since all plugins for static costmap conversion are independent - */ - virtual bool stackedCostmapConversion() {return false;} - - /** - * @brief Instantiate a worker that repeatedly coverts the most recent costmap to polygons. - * The worker is implemented as a timer event that is invoked at a specific \c rate. - * The passed \c costmap pointer must be valid at the complete time and must be lockable. - * By specifying the argument \c spin_thread the timer event is invoked in a separate - * thread and callback queue or otherwise it is called from the global callback queue (of the - * node in which the plugin is used). - * @param rate The rate that specifies how often the costmap should be updated - * @param costmap Pointer to the underlying costmap (must be valid and lockable as long as the worker is active - * @param spin_thread if \c true,the timer is invoked in a separate thread, otherwise in the default callback queue) - */ - void startWorker(rclcpp::Rate::SharedPtr rate, nav2_costmap_2d::Costmap2D* costmap, bool spin_thread = false) - { - setCostmap2D(costmap); - - if (spin_thread_) - { - { - std::lock_guard terminate_lock(terminate_mutex_); - need_to_terminate_ = true; - } - spin_thread_->join(); - delete spin_thread_; - } - - if (spin_thread) - { - RCLCPP_DEBUG(nh_->get_logger(), "Spinning up a thread for the CostmapToPolygons plugin"); - need_to_terminate_ = false; - - worker_timer_ = nh_->create_wall_timer( - rate->period(), - std::bind(&BaseCostmapToPolygons::workerCallback, this)); - spin_thread_ = new std::thread(std::bind(&BaseCostmapToPolygons::spinThread, this)); - } - else - { - worker_timer_ = nh_->create_wall_timer( - rate->period(), - std::bind(&BaseCostmapToPolygons::workerCallback, this)); - spin_thread_ = nullptr; - } - } - - /** - * @brief Stop the worker that repeatedly converts the costmap to polygons - */ - void stopWorker() - { - if (worker_timer_) worker_timer_->cancel(); - if (spin_thread_) - { - { - std::lock_guard terminate_lock(terminate_mutex_); - need_to_terminate_ = true; - } - spin_thread_->join(); - delete spin_thread_; - } - } - -protected: - - /** - * @brief Protected constructor that should be called by subclasses - */ - BaseCostmapToPolygons() : //nh_("~costmap_to_polygons"), - nh_(nullptr), - spin_thread_(nullptr), need_to_terminate_(false) {} - - /** - * @brief Blocking method that checks for new timer events (active if startWorker() is called with spin_thread enabled) - */ - void spinThread() - { - rclcpp::WallRate r(10); - while (rclcpp::ok()) - { - { - std::lock_guard terminate_lock(terminate_mutex_); - if (need_to_terminate_) - break; - rclcpp::spin_some(nh_); - r.sleep(); - } - } - } - - /** - * @brief The callback of the worker that performs the actual work (updating the costmap and converting it to polygons) - */ - void workerCallback() - { - updateCostmap2D(); - compute(); - } - - rclcpp::Logger getLogger() const - { - return nh_->get_logger(); - } - - rclcpp::Time now() const - { - return nh_->now(); - } - -private: - rclcpp::TimerBase::SharedPtr worker_timer_; - rclcpp::Node::SharedPtr nh_; - std::thread* spin_thread_; - std::mutex terminate_mutex_; - bool need_to_terminate_; -}; - - -/** - * @class BaseCostmapToDynamicPolygons - * @brief This class extends the BaseCostmapToPolygongs class for dynamic costmap conversion plugins in order to incorporate a subsequent costmap converter plugin for static obstacles - * - * This class should not be instantiated. - */ -class BaseCostmapToDynamicObstacles : public BaseCostmapToPolygons -{ -public: - - /** - * @brief Load underlying static costmap conversion plugin via plugin-loader - * @param plugin_name Exact class name of the plugin to be loaded, e.g. - * costmap_converter::CostmapToPolygonsDBSMCCH - * @param nh_parent NodeHandle which is extended by the namespace of the static conversion plugin - */ - void loadStaticCostmapConverterPlugin(const std::string& plugin_name, rclcpp::Node::SharedPtr nh_parent) - { - try - { - static_costmap_converter_ = static_converter_loader_.createSharedInstance(plugin_name); - - if(std::dynamic_pointer_cast(static_costmap_converter_)) - { - throw pluginlib::PluginlibException("The specified plugin for static costmap conversion is a dynamic plugin. Specify a static plugin."); - } -// std::string raw_plugin_name = static_converter_loader_.getName(plugin_name); - static_costmap_converter_->initialize(nh_parent); - setStaticCostmapConverterPlugin(static_costmap_converter_); - RCLCPP_INFO(getLogger(), "CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles %s loaded.", plugin_name.c_str()); - } - catch(const pluginlib::PluginlibException& ex) - { - RCLCPP_WARN(getLogger(), "CostmapToDynamicObstacles: The specified costmap converter plugin cannot be loaded. " - "Continuing without subsequent conversion of static obstacles. Error message: %s", ex.what()); - static_costmap_converter_.reset(); - } - } - - /** - * @brief Set the underlying plugin for subsequent costmap conversion of the static background of the costmap - * @param static_costmap_converter shared pointer to the static costmap conversion plugin - */ - void setStaticCostmapConverterPlugin(std::shared_ptr static_costmap_converter) - { - static_costmap_converter_ = static_costmap_converter; - } - - /** - * @brief Set the costmap for the underlying plugin - * @param static_costmap Costmap2D, which contains the static part of the original costmap - */ - void setStaticCostmap(std::shared_ptr static_costmap) - { - static_costmap_converter_->setCostmap2D(static_costmap.get()); - } - - /** - * @brief Apply the underlying plugin for static costmap conversion - */ - void convertStaticObstacles() - { - static_costmap_converter_->compute(); - } - - /** - * @brief Get the converted polygons from the underlying plugin - * @return Shared instance of the underlying plugins polygon container - */ - PolygonContainerConstPtr getStaticPolygons() - { - return static_costmap_converter_->getPolygons(); - } - - /** - * @brief Determines whether an additional plugin for subsequent costmap conversion is specified - * - * @return true, if a plugin for subsequent costmap conversion is specified - */ - bool stackedCostmapConversion() - { - if(static_costmap_converter_) - return true; - else - return false; - } - -protected: - /** - * @brief Protected constructor that should be called by subclasses - */ - BaseCostmapToDynamicObstacles() : static_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), static_costmap_converter_() {} - -private: - pluginlib::ClassLoader static_converter_loader_; - std::shared_ptr static_costmap_converter_; -}; - - -} - - - -#endif // end COSTMAP_CONVERTER_INTERFACE_H_ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h deleted file mode 100644 index 3e3d716..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/background_subtractor.h +++ /dev/null @@ -1,115 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017 - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following code makes use of the OpenCV library. - * OpenCV is licensed under the terms of the 3-clause BSD License. - * - * Authors: Franz Albers, Christoph Rösmann - *********************************************************************/ - -#ifndef BACKGROUNDSUBTRACTOR_H_ -#define BACKGROUNDSUBTRACTOR_H_ - -#include - -/** - * @class BackgroundSubtractor - * @brief Perform background subtraction to extract the "moving" foreground - * - * This class is based on OpenCV's background subtraction class cv::BackgroundSubtractor. - * It has been modified in order to incorporate a specialized bandpass filter. - * - * See http://docs.opencv.org/3.2.0/d7/df6/classcv_1_1BackgroundSubtractor.html for the original class. - */ -class BackgroundSubtractor -{ -public: - struct Params - { - double alpha_slow; //!< Filter constant (learning rate) of the slow filter part - double alpha_fast; //!< Filter constant (learning rate) of the fast filter part - double beta; - double min_sep_between_fast_and_slow_filter; - double min_occupancy_probability; - double max_occupancy_neighbors; - int morph_size; - }; - - //! Constructor that accepts a specific parameter configuration - BackgroundSubtractor(const Params& parameters); - - /** - * @brief Computes a foreground mask - * @param[in] image Next video frame - * @param[out] fg_mask Foreground mask as an 8-bit binary image - * @param[in] shift_x Translation along the x axis between the current and previous image - * @param[in] shift_y Translation along the y axis between the current and previous image - */ - void apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x = 0, int shift_y = 0); - - /** - * @brief OpenCV Visualization - * @param name Id/name of the opencv window - * @param image Image to be visualized - */ - void visualize(const std::string& name, const cv::Mat& image); - - /** - * @brief Export vector of matrices to yaml file - * @remarks This method is intended for debugging purposes - * @param filename Desired filename including path and excluding file suffix - * @param mat_vec Vector of cv::Mat to be exported - */ - void writeMatToYAML(const std::string& filename, const std::vector& mat_vec); - - //! Update internal parameters - void updateParameters(const Params& parameters); - -private: - //! Transform/shift all internal matrices/grids according to a given translation vector - void transformToCurrentFrame(int shift_x, int shift_y); - - cv::Mat occupancy_grid_fast_; - cv::Mat occupancy_grid_slow_; - cv::Mat current_frame_; - - int previous_shift_x_; - int previous_shift_y_; - - Params params_; -}; - -#endif // BACKGROUNDSUBTRACTOR_H_ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h deleted file mode 100644 index ac9b80a..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h +++ /dev/null @@ -1,111 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017 - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following code makes use of the OpenCV library. - * OpenCV is licensed under the terms of the 3-clause BSD License. - * - * Authors: Franz Albers, Christoph Rösmann - *********************************************************************/ - -#ifndef BLOBDETECTOR_H_ -#define BLOBDETECTOR_H_ - -// Basically the OpenCV SimpleBlobDetector, extended with getContours() - -#include - -/** - * @class BlobDetector - * @brief Detect blobs in image (specialized for dynamic obstacles in the costmap) - * - * This class is based on OpenCV's blob detector cv::SimpleBlobDetector. - * It has been modified and specialized for dynamic obstacle tracking in the costmap: - * -> The modified version also returns contours of the blob. - * - * See http://docs.opencv.org/trunk/d0/d7a/classcv_1_1SimpleBlobDetector.html for the original class. - */ -class BlobDetector : public cv::SimpleBlobDetector -{ -public: - //! Default constructor which optionally accepts custom parameters - BlobDetector(const cv::SimpleBlobDetector::Params& parameters = cv::SimpleBlobDetector::Params()); - - //! Create shared instance of the blob detector with given parameters - static cv::Ptr create(const BlobDetector::Params& params); - - /** - * @brief Detects keypoints in an image and extracts contours - * - * In contrast to the original detect method, this extended version - * also extracts contours. Contours can be accessed by getContours() - * after invoking this method. - * - * @todo The mask option is currently not implemented. - * - * @param image image - * @param keypoints The detected keypoints. - * @param mask Mask specifying where to look for keypoints (optional). It must be a 8-bit integer - * matrix with non-zero values in the region of interest. - */ - virtual void detect(const cv::Mat& image, std::vector& keypoints, - const cv::Mat& mask = cv::Mat()); - - /** - * @brief Access contours extracted during detection stage - * @return Read-only reference to the contours set of the previous detect() run - */ - const std::vector>& getContours() { return contours_; } - - //! Update internal parameters - void updateParameters(const cv::SimpleBlobDetector::Params& parameters); - -protected: - struct Center - { - cv::Point2d location; - double radius; - double confidence; - }; - - virtual void findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector

& centers, - std::vector>& cur_contours) const; - - std::vector> contours_; - - Params params_; -}; - -#endif // BLOBDETECTOR_H_ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h deleted file mode 100644 index 0c9924b..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.h +++ /dev/null @@ -1,212 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017 - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following code makes use of the OpenCV library. - * OpenCV is licensed under the terms of the 3-clause BSD License. - * - * Authors: Franz Albers, Christoph Rösmann - *********************************************************************/ - -#ifndef COSTMAP_TO_DYNAMIC_OBSTACLES_H_ -#define COSTMAP_TO_DYNAMIC_OBSTACLES_H_ - -// ROS -#include -#include -#include -#include - -// OpenCV -#include -#include -#include - -// dynamic reconfigure -//#include -//#include - -// Own includes -#include -#include -#include - -// STL -#include - -namespace costmap_converter { - -/** - * @class CostmapToDynamicObstacles - * @brief This class converts the costmap_2d into dynamic obstacles. - * - * Static obstacles are treated as point obstacles. - */ -class CostmapToDynamicObstacles : public BaseCostmapToDynamicObstacles -{ -public: - /** - * @brief Constructor - */ - CostmapToDynamicObstacles(); - - /** - * @brief Destructor - */ - virtual ~CostmapToDynamicObstacles(); - - /** - * @brief Initialize the plugin - * @param nh Nodehandle that defines the namespace for parameters - */ - virtual void initialize(rclcpp::Node::SharedPtr nh); - - /** - * @brief This method performs the actual work (conversion of the costmap to - * obstacles) - */ - virtual void compute(); - - /** - * @brief Pass a pointer to the costmap to the plugin. - * @sa updateCostmap2D - * @param costmap Pointer to the costmap2d source - */ - virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap); - - /** - * @brief Get updated data from the previously set Costmap2D - * @sa setCostmap2D - */ - virtual void updateCostmap2D(); - - /** - * @brief Get a shared instance of the current obstacle container - * @remarks If compute() or startWorker() has not been called before, this - * method returns an empty instance! - * @return Shared instance of the current obstacle container - */ - ObstacleArrayConstPtr getObstacles(); - - /** - * @brief Set name of robot's odometry topic - * - * @warning The method must be called before initialize() - * - * Some plugins might require odometry information - * to compensate the robot's ego motion - * @param odom_topic topic name - */ - virtual void setOdomTopic(const std::string& odom_topic) - { - odom_topic_ = odom_topic; - } - - /** - * @brief OpenCV Visualization - * @param name Id/name of the opencv window - * @param image Image to be visualized - */ - void visualize(const std::string& name, const cv::Mat& image); - -protected: - /** - * @brief Converts the estimated velocity of a tracked object to m/s and - * returns it - * @param idx Index of the tracked object in the tracker - * @return Point_t, which represents velocity in [m/s] of object(idx) in x,y,z - * coordinates - */ - Point_t getEstimatedVelocityOfObject(unsigned int idx); - - /** - * @brief Gets the last observed contour of a object and converts it from [px] - * to [m] - * @param[in] idx Index of the tracked object in the tracker - * @param[out] contour vector of Point_t, which represents the last observed contour in [m] - * in x,y,z coordinates - */ - void getContour(unsigned int idx, std::vector& contour); - - /** - * @brief Thread-safe update of the internal obstacle container (that is - * shared using getObstacles() from outside this - * class) - * @param obstacles Updated obstacle container - */ - void updateObstacleContainer(ObstacleArrayPtr obstacles); - -private: - std::mutex mutex_; - nav2_costmap_2d::Costmap2D* costmap_; - cv::Mat costmap_mat_; - ObstacleArrayPtr obstacles_; - cv::Mat fg_mask_; - std::unique_ptr bg_sub_; - cv::Ptr blob_det_; - std::vector keypoints_; - std::unique_ptr tracker_; - rclcpp::Subscription::SharedPtr odom_sub_; - Point_t ego_vel_; - - std::string odom_topic_ = "/odom"; - bool publish_static_obstacles_ = true; - -// dynamic_reconfigure::Server* -// dynamic_recfg_; //!< Dynamic reconfigure server to allow config -// //! modifications at runtime - - /** - * @brief Callback for the odometry messages of the observing robot. - * - * Used to convert the velocity of obstacles to the /map frame. - * @param msg The Pointer to the nav_msgs::Odometry of the observing robot - */ - void odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - - /** - * @brief Callback for the dynamic_reconfigure node. - * - * This callback allows to modify parameters dynamically at runtime without - * restarting the node - * @param config Reference to the dynamic reconfigure config - * @param level Dynamic reconfigure level - */ -// void reconfigureCB(CostmapToDynamicObstaclesConfig &config, uint32_t level); -}; - -} // end namespace costmap_converter - -#endif /* COSTMAP_TO_DYNAMIC_OBSTACLES_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h deleted file mode 100644 index ca0f01e..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.h +++ /dev/null @@ -1,96 +0,0 @@ -// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3 -// Refer to README.md in this directory. - -#pragma once -#include "Kalman.h" -#include "HungarianAlg.h" -#include "defines.h" -#include -#include -#include -#include - -// -------------------------------------------------------------------------- -class CTrack -{ -public: - CTrack(const Point_t& p, const std::vector& contour, track_t dt, size_t trackID) - : track_id(trackID), - skipped_frames(0), - prediction(p), - lastContour(contour), - KF(p, dt) - { - } - - track_t CalcDist(const Point_t& p) - { - Point_t diff = prediction - p; - return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z); - } - - void Update(const Point_t& p, const std::vector& contour, bool dataCorrect, size_t max_trace_length) - { - KF.Prediction(); - prediction = KF.Update(p, dataCorrect); - - if (dataCorrect) - { - lastContour = contour; - } - - if (trace.size() > max_trace_length) - { - trace.erase(trace.begin(), trace.end() - max_trace_length); - } - - trace.push_back(prediction); - } - - // Returns contour in [px], not in [m]! - std::vector getLastContour() const - { - return lastContour; - } - - // Returns velocity in [px/s], not in [m/s]! - Point_t getEstimatedVelocity() const - { - return KF.LastVelocity; - } - - std::vector trace; - size_t track_id; - size_t skipped_frames; - -private: - Point_t prediction; - std::vector lastContour; // Contour liegt immer auf ganzen Pixeln -> Integer Punkt - TKalmanFilter KF; -}; - -// -------------------------------------------------------------------------- -class CTracker -{ -public: - struct Params{ - track_t dt; // time for one step of the filter - track_t dist_thresh;// distance threshold. Pairs of points with higher distance are not considered in the assignment problem - int max_allowed_skipped_frames; // Maximum number of frames the track is maintained without seeing the object in the measurement data - int max_trace_length; // Maximum trace length - }; - - CTracker(const Params& parameters); - ~CTracker(void); - - std::vector> tracks; - void Update(const std::vector& detectedCentroid, const std::vector > &contour); - - void updateParameters(const Params ¶meters); - -private: - // Aggregated parameters for the tracker object - Params params; - // ID for the upcoming CTrack object - size_t NextTrackID; -}; diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h deleted file mode 100644 index 0985ae7..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.h +++ /dev/null @@ -1,60 +0,0 @@ -// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3 -// Refer to README.md in this directory. - -#include -#include -#include -#include -#include "defines.h" - -// http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm - -typedef std::vector assignments_t; -typedef std::vector distMatrix_t; - -class AssignmentProblemSolver -{ -private: - // -------------------------------------------------------------------------- - // Computes the optimal assignment (minimum overall costs) using Munkres algorithm. - // -------------------------------------------------------------------------- - void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, - size_t nOfColumns); - void buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows, size_t nOfColumns); - void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, - size_t nOfRows); - void step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, - bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); - void step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, - bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); - void step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, - bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); - void step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, - bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, - size_t col); - void step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, - bool* coveredColumns, bool* coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); - // -------------------------------------------------------------------------- - // Computes a suboptimal solution. Good for cases with many forbidden assignments. - // -------------------------------------------------------------------------- - void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, - size_t nOfColumns); - // -------------------------------------------------------------------------- - // Computes a suboptimal solution. Good for cases with many forbidden assignments. - // -------------------------------------------------------------------------- - void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, - size_t nOfColumns); - -public: - enum TMethod - { - optimal, - many_forbidden_assignments, - without_forbidden_assignments - }; - - AssignmentProblemSolver(); - ~AssignmentProblemSolver(); - track_t Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, assignments_t& assignment, - TMethod Method = optimal); -}; diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h deleted file mode 100755 index 34a5319..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.h +++ /dev/null @@ -1,20 +0,0 @@ -// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3 -// Refer to README.md in this directory. - -#pragma once -#include "defines.h" -#include - -// http://www.morethantechnical.com/2011/06/17/simple-kalman-filter-for-tracking-using-opencv-2-2-w-code/ -class TKalmanFilter -{ -public: - TKalmanFilter(Point_t p, track_t deltatime = 0.2); - ~TKalmanFilter(); - void Prediction(); - Point_t Update(Point_t p, bool DataCorrect); - cv::KalmanFilter* kalman; - track_t dt; - Point_t LastPosition; // contour in [px] - Point_t LastVelocity; // velocity in [px/s] -}; diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md deleted file mode 100644 index 3fb4614..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/README.md +++ /dev/null @@ -1,13 +0,0 @@ -Multitarget Tracker -=================== - -This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker]. -It is utilized for the *CostmapToDynamicObstacles* plugin. - -The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt]. -The package itself depends on other third party packages with different licenses (refer to the package repository). - -TODO: Include the whole package as external CMake project. - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h deleted file mode 100644 index 203004f..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_dynamic_obstacles/multitarget_tracker/defines.h +++ /dev/null @@ -1,9 +0,0 @@ -// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3 -// Refer to README.md in this directory. - -#pragma once -#include - -typedef float track_t; -typedef cv::Point3_ Point_t; -#define Mat_t CV_32FC diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_convex_hull.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_convex_hull.h deleted file mode 100644 index 7e409f3..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_convex_hull.h +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann, Otniel Rinaldo - *********************************************************************/ - -#ifndef COSTMAP_TO_LINES_CONVEX_HULL_H_ -#define COSTMAP_TO_LINES_CONVEX_HULL_H_ - -#include -#include - -// dynamic reconfigure -//#include - -namespace costmap_converter -{ - -/** - * @class CostmapToLinesDBSMCCH - * @brief This class converts the costmap_2d into a set of lines (and points) - * - * The conversion is performed in three stages: - * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN) - * Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei, - * A density-based algorithm for discovering clusters in large spatial databases with noise. - * Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. - * - * 2. In the subsequent stage, the convex hull of each cluster is determined using the monotone chain algorithm aka Andrew's algorithm: - * C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 ) - * Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979). - * - * 3. In the third step extract lines from each polygon (convex hull) if there exist at least a predefined number of support points. - * - * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line) - */ - class CostmapToLinesDBSMCCH : public CostmapToPolygonsDBSMCCH - { - public: - - /** - * @brief Constructor - */ - CostmapToLinesDBSMCCH(); - - /** - * @brief Destructor - */ - virtual ~CostmapToLinesDBSMCCH(); - - /** - * @brief Initialize the plugin - * @param nh Nodehandle that defines the namespace for parameters - */ - virtual void initialize(rclcpp::Node::SharedPtr nh); - - /** - * @brief This method performs the actual work (conversion of the costmap to polygons) - */ - virtual void compute(); - - - protected: - - /** - * @brief Extract points and lines from a given cluster by checking all keypoint pairs of the convex hull for a minimum number of support points - * @param cluster list of points in the cluster - * @param polygon convex hull of the cluster \c cluster - * @param[out] lines back_inserter object to a sequence of polygon msgs (new lines will be pushed back) - */ - void extractPointsAndLines(std::vector& cluster, const geometry_msgs::msg::Polygon& polygon, std::back_insert_iterator< std::vector > lines); - - - - protected: - - double support_pts_max_dist_inbetween_; - double support_pts_max_dist_; - int min_support_pts_; - - private: - - /** - * @brief Callback for the dynamic_reconfigure node. - * - * This callback allows to modify parameters dynamically at runtime without restarting the node - * @param config Reference to the dynamic reconfigure config - * @param level Dynamic reconfigure level - */ -// void reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level); - -// dynamic_reconfigure::Server* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime - - }; - - - - - - - - -} //end namespace teb_local_planner - -#endif /* COSTMAP_TO_LINES_CONVEX_HULL_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_ransac.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_ransac.h deleted file mode 100644 index 26d494c..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_lines_ransac.h +++ /dev/null @@ -1,187 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef COSTMAP_TO_LINES_RANSAC_H_ -#define COSTMAP_TO_LINES_RANSAC_H_ - -#include -#include -#include - -#include - -//#include - -namespace costmap_converter -{ - -/** - * @class CostmapToLinesDBSRANSAC - * @brief This class converts the costmap_2d into a set of lines (and points) - * - * The conversion is performed in two stages: - * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN) - * Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei, - * A density-based algorithm for discovering clusters in large spatial databases with noise. - * Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. - * - * 2. The RANSAC algorithm is used to find straight line segment models (https://en.wikipedia.org/wiki/RANSAC) - * RANSAC is called repeatedly to find multiple lines per cluster until the number of inliners is below a specific threshold. - * In that case the remaining outliers are used as points or keypoints of their convex hull are used as points (depending on a paramter). - * The latter one applies as a filter. The convex assumption is not strict in practice, since the remaining regions/cluster (line inliers are removed) - * should be dense and small. For details about the convex hull algorithm, refer to costmap_converter::CostmapToPolygonsDBSMCCH. - * Resulting lines of RANSAC are currently not refined by linear regression. - * - * The output is a container of polygons (but each polygon does only contain a single vertex (point) or two vertices (line) - */ - class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH - { - public: - - /** - * @brief Constructor - */ - CostmapToLinesDBSRANSAC(); - - /** - * @brief Destructor - */ - virtual ~CostmapToLinesDBSRANSAC(); - - /** - * @brief Initialize the plugin - * @param nh Nodehandle that defines the namespace for parameters - */ - virtual void initialize(rclcpp::Node::SharedPtr nh); - - /** - * @brief This method performs the actual work (conversion of the costmap to polygons) - */ - virtual void compute(); - - - /** - * @brief Check if the candidate point is an inlier. - * @param point generic 2D point type defining the reference point - * @param line_start generic 2D point type defining the start of the line - * @param line_end generic 2D point type defining the end of the line - * @param min_distance minimum distance allowed - * @tparam Point generic point type that should provide (writable) x and y member fiels. - * @tparam LinePoint generic point type that should provide (writable) x and y member fiels. - * @return \c true if inlier, \c false otherwise - */ - template - static bool isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance); - - protected: - - double ransac_inlier_distance_; //!< Maximum distance to the line segment for inliers - int ransac_min_inliers_; //!< Minimum numer of inliers required to form a line - int ransac_no_iterations_; //!< Number of ransac iterations - int ransac_remainig_outliers_; //!< Repeat ransac until the number of outliers is as specified here - bool ransac_convert_outlier_pts_; //!< If \c true, convert remaining outliers to single points. - bool ransac_filter_remaining_outlier_pts_; //!< If \c true, filter the interior of remaining outliers and keep only keypoints of their convex hull - - - std::mt19937 rnd_generator_; //!< Random number generator for ransac with default seed - - - /** - * @brief Find a straight line segment in a point cloud with RANSAC (without linear regression). - * @param data set of 2D data points - * @param inlier_distance maximum distance that inliers must satisfy - * @param no_iterations number of RANSAC iterations - * @param min_inliers minimum number of inliers to return true - * @param[out] best_model start and end of the best straight line segment - * @param[out] inliers inlier keypoints are written to this container [optional] - * @param[out] outliers outlier keypoints are written to this container [optional] - * @return \c false, if \c min_inliers is not satisfied, \c true otherwise - */ - bool lineRansac(const std::vector& data, double inlier_distance, int no_iterations, int min_inliers, - std::pair& best_model, std::vector* inliers = NULL, - std::vector* outliers = NULL); - - /** - * @brief Perform a simple linear regression in order to fit a straight line 'y = slope*x + intercept' - * @param data set of 2D data points - * @param[out] slope The slope of the fitted line - * @param[out] intercept The intercept / offset of the line - * @param[out] mean_x_out mean of the x-values of the data [optional] - * @param[out] mean_y_out mean of the y-values of the data [optional] - * @return \c true, if a valid line has been fitted, \c false otherwise. - */ - bool linearRegression(const std::vector& data, double& slope, double& intercept, - double* mean_x_out = NULL, double* mean_y_out = NULL); - - - -// void adjustLineLength(const std::vector& data, const KeyPoint& linept1, const KeyPoint& linept2, -// KeyPoint& line_start, KeyPoint& line_end); - - private: - - /** - * @brief Callback for the dynamic_reconfigure node. - * - * This callback allows to modify parameters dynamically at runtime without restarting the node - * @param config Reference to the dynamic reconfigure config - * @param level Dynamic reconfigure level - */ -// void reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level); - -// dynamic_reconfigure::Server* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime - }; - - - -template -bool CostmapToLinesDBSRANSAC::isInlier(const Point& point, const LinePoint& line_start, const LinePoint& line_end, double min_distance) -{ - bool is_inbetween = false; - double distance = computeDistanceToLineSegment(point, line_start, line_end, &is_inbetween); - if (!is_inbetween) - return false; - if (distance <= min_distance) - return true; - return false; -} - - -} //end namespace teb_local_planner - -#endif /* COSTMAP_TO_LINES_RANSAC_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons.h deleted file mode 100644 index 049fd10..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons.h +++ /dev/null @@ -1,335 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann, Otniel Rinaldo - *********************************************************************/ - -#ifndef COSTMAP_TO_POLYGONS_H_ -#define COSTMAP_TO_POLYGONS_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// dynamic reconfigure -//#include -//#include - - -namespace costmap_converter -{ - -/** - * @class CostmapToPolygonsDBSMCCH - * @brief This class converts the costmap_2d into a set of convex polygons (and points) - * - * The conversion is performed in two stages: - * 1. Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN) - * Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei, - * A density-based algorithm for discovering clusters in large spatial databases with noise. - * Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. - * - * 2. In the subsequent stage, clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm: - * C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 ) - * Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979). - * - * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex) - */ -class CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons -{ - public: - - /** - * @struct KeyPoint - * @brief Defines a keypoint in metric coordinates of the map - */ - struct KeyPoint - { - //! Default constructor - KeyPoint() : x(0.0), y(0.0) {} - //! Constructor with point initialization - KeyPoint(double x_, double y_) : x(x_), y(y_) {} - double x; //!< x coordinate [m] - double y; //!< y coordinate [m] - - //! Convert keypoint to geometry_msgs::msg::Point message type - void toPointMsg(geometry_msgs::msg::Point& point) const {point.x=x; point.y=y; point.z=0;} - //! Convert keypoint to geometry_msgs::msg::Point32 message type - void toPointMsg(geometry_msgs::msg::Point32& point) const {point.x=x; point.y=y; point.z=0;} - }; - - /** - * @struct Parameters - * @brief Defines the parameters of the algorithm - */ - struct Parameters - { - Parameters() : max_distance_(0.4), min_pts_(2), max_pts_(30), min_keypoint_separation_(0.1) {} - // DBSCAN parameters - double max_distance_; //!< Parameter for DB_Scan, maximum distance to neighbors [m] - int min_pts_; //!< Parameter for DB_Scan: minimum number of points that define a cluster - int max_pts_; //!< Parameter for DB_Scan: maximum number of points that define a cluster (to avoid large L- and U-shapes) - - // convex hull parameters - double min_keypoint_separation_; //!< Clear keypoints of the convex polygon that are close to each other [distance in meters] (0: keep all) - }; - - /** - * @brief Constructor - */ - CostmapToPolygonsDBSMCCH(); - - /** - * @brief Destructor - */ - virtual ~CostmapToPolygonsDBSMCCH(); - - /** - * @brief Initialize the plugin - * @param nh Nodehandle that defines the namespace for parameters - */ - virtual void initialize(rclcpp::Node::SharedPtr nh) override; - - /** - * @brief This method performs the actual work (conversion of the costmap to polygons) - */ - virtual void compute(); - - /** - * @brief Pass a pointer to the costap to the plugin. - * @sa updateCostmap2D - * @param costmap Pointer to the costmap2d source - */ - virtual void setCostmap2D(nav2_costmap_2d::Costmap2D* costmap); - - /** - * @brief Get updated data from the previously set Costmap2D - * @sa setCostmap2D - */ - virtual void updateCostmap2D(); - - - /** - * @brief Convert a generi point type to a geometry_msgs::msg::Polygon - * @param point generic 2D point type - * @param[out] polygon already instantiated polygon that will be filled with a single point - * @tparam Point generic point type that should provide (writable) x and y member fiels. - */ - template< typename Point> - static void convertPointToPolygon(const Point& point, geometry_msgs::msg::Polygon& polygon) - { - polygon.points.resize(1); - polygon.points.front().x = point.x; - polygon.points.front().y = point.y; - polygon.points.front().z = 0; - } - - /** - * @brief Get a shared instance of the current polygon container - * @remarks If compute() or startWorker() has not been called before, this method returns an empty instance! - * @return Shared instance of the current polygon container - */ - PolygonContainerConstPtr getPolygons(); - - - - protected: - - /** - * @brief DBSCAN algorithm for clustering - * - * Clusters in the costmap are collected using the DBSCAN Algorithm (https://en.wikipedia.org/wiki/DBSCAN) - * Reference: Ester, Martin; Kriegel, Hans-Peter; Sander, Jörg; Xu, Xiaowei, - * A density-based algorithm for discovering clusters in large spatial databases with noise. - * Proceedings of the Second International Conference on Knowledge Discovery and Data Mining. AAAI Press. 1996. pp. 226–231. ISBN 1-57735-004-9. - * - * @param[out] clusters clusters will added to this output-argument (a sequence of keypoints for each cluster) - * the first cluster (clusters[0]) will contain all noise points (that does not fulfil the min_pts condition - */ - void dbScan(std::vector< std::vector >& clusters); - - /** - * @brief Helper function for dbScan to search for neighboring points - * - * @param curr_index index to the current item in \c occupied_cells - * @param[out] neighbor_indices list of neighbors (indices of \c occupied cells) - */ - void regionQuery(int curr_index, std::vector& neighbor_indices); - - /** - * @brief helper function for adding a point to the lookup data structures - */ - void addPoint(double x, double y) - { - int idx = occupied_cells_.size(); - occupied_cells_.emplace_back(x, y); - int cx, cy; - pointToNeighborCells(occupied_cells_.back(), cx, cy); - int nidx = neighborCellsToIndex(cx, cy); - if (nidx >= 0) - neighbor_lookup_[nidx].push_back(idx); - } - - /** - * @brief Compute the convex hull for a single cluster (monotone chain algorithm) - * - * Clusters are converted into convex polgons using the monotone chain algorithm aka Andrew's algorithm: - * C++ implementation example: http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull ( GNU Free Documentation License 1.2 ) - * Reference: A. M. Andrew, "Another Efficient Algorithm for Convex Hulls in Two Dimensions", Info. Proc. Letters 9, 216-219 (1979). - * @remarks The algorithm seems to cut edges, thus we give a try to convexHull2(). - * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...) - * @remarks The last point is the same as the first one - * @param cluster list of keypoints that should be converted into a polygon - * @param[out] polygon the resulting convex polygon - */ - void convexHull(std::vector& cluster, geometry_msgs::msg::Polygon& polygon); - - /** - * @brief Compute the convex hull for a single cluster - * - * Clusters are converted into convex polgons using an algorithm provided here: - * https://bitbucket.org/vostreltsov/concave-hull/overview - * The convex hull algorithm is part of the concave hull algorithm. - * The license is WTFPL 2.0 and permits any usage. - * - * @remarks The last point is the same as the first one - * @param cluster list of keypoints that should be converted into a polygon - * @param[out] polygon the resulting convex polygon - * @todo Evaluate and figure out whether convexHull() or convexHull2() is better suited (performance, quality, ...) - */ - void convexHull2(std::vector& cluster, geometry_msgs::msg::Polygon& polygon); - - /** - * @brief Simplify a polygon by removing points. - * - * We apply the Douglas-Peucker Algorithm to simplify the edges of the polygon. - * Internally, the parameter min_keypoint_separation is used for deciding whether - * a point is considered close to an edge and to be merged into the line. - */ - void simplifyPolygon(geometry_msgs::msg::Polygon& polygon); - - /** - * @brief 2D Cross product of two vectors defined by two points and a common origin - * @param O Origin - * @param A First point - * @param B Second point - * @tparam P1 2D Point type with x and y member fields - * @tparam P2 2D Point type with x and y member fields - * @tparam P3 2D Point type with x and y member fields - */ - template - long double cross(const P1& O, const P2& A, const P3& B) - { - return (long double)(A.x - O.x) * (long double)(B.y - O.y) - (long double)(A.y - O.y) * (long double)(B.x - O.x); - } - - - /** - * @brief Thread-safe update of the internal polygon container (that is shared using getPolygons() from outside this class) - * @param polygons Updated polygon container - */ - void updatePolygonContainer(PolygonContainerPtr polygons); - - - std::vector occupied_cells_; //!< List of occupied cells in the current map (updated by updateCostmap2D()) - - std::vector > neighbor_lookup_; //! array of cells for neighbor lookup - int neighbor_size_x_; //! size of the neighbour lookup in x (number of cells) - int neighbor_size_y_; //! size of the neighbour lookup in y (number of cells) - double offset_x_; //! offset [meters] in x for the lookup grid - double offset_y_; //! offset [meters] in y for the lookup grid - - /** - * @brief convert a 2d cell coordinate into the 1D index of the array - * @param cx the x index of the cell - * @param cy the y index of the cell - */ - int neighborCellsToIndex(int cx, int cy) - { - if (cx < 0 || cx >= neighbor_size_x_ || cy < 0 || cy >= neighbor_size_y_) - return -1; - return cy * neighbor_size_x_ + cx; - } - - /** - * @brief compute the cell indices of a keypoint - * @param kp key point given in world coordinates [m, m] - * @param cx output cell index in x direction - * @param cy output cell index in y direction - */ - void pointToNeighborCells(const KeyPoint& kp, int& cx, int& cy) - { - cx = int((kp.x - offset_x_) / parameter_.max_distance_); - cy = int((kp.y - offset_y_) / parameter_.max_distance_); - } - - - Parameters parameter_; //< active parameters throughout computation - Parameters parameter_buffered_; //< the buffered parameters that are offered to dynamic reconfigure - std::mutex parameter_mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance - - private: - - /** - * @brief Callback for the dynamic_reconfigure node. - * - * This callback allows to modify parameters dynamically at runtime without restarting the node - * @param config Reference to the dynamic reconfigure config - * @param level Dynamic reconfigure level - */ - //void reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level); - - - PolygonContainerPtr polygons_; //!< Current shared container of polygons - std::mutex mutex_; //!< Mutex that keeps track about the ownership of the shared polygon instance - - //dynamic_reconfigure::Server* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime - - nav2_costmap_2d::Costmap2D *costmap_; //!< Pointer to the costmap2d - -}; - - -} //end namespace teb_local_planner - -#endif /* COSTMAP_TO_POLYGONS_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons_concave.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons_concave.h deleted file mode 100644 index 4e062ea..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/costmap_to_polygons_concave.h +++ /dev/null @@ -1,202 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef COSTMAP_TO_POLYGONS_CONCAVE_H_ -#define COSTMAP_TO_POLYGONS_CONCAVE_H_ - -#include -#include - -// dynamic reconfigure -//#include -//#include - - -namespace costmap_converter -{ - -/** - * @class CostmapToPolygonsDBSConcaveHull - * @brief This class converts the costmap_2d into a set of non-convex polygons (and points) - * - * All single points that do not define a cluster (noise) are also treated as polygon (represented as a single vertex) - * @todo change the class hierarchy to a clearer and more generic one - */ -class CostmapToPolygonsDBSConcaveHull : public CostmapToPolygonsDBSMCCH -{ - public: - - - - /** - * @brief Constructor - */ - CostmapToPolygonsDBSConcaveHull(); - - /** - * @brief Destructor - */ - virtual ~CostmapToPolygonsDBSConcaveHull(); - - /** - * @brief Initialize the plugin - * @param nh Nodehandle that defines the namespace for parameters - */ - virtual void initialize(rclcpp::Node::SharedPtr nh); - - - /** - * @brief This method performs the actual work (conversion of the costmap to polygons) - */ - virtual void compute(); - - protected: - - - /** - * @brief Compute the concave hull for a single cluster - * - * @remarks The last point is the same as the first one - * @param cluster list of keypoints that should be converted into a polygon - * @param depth Smaller depth: sharper surface, depth -> high value: convex hull - * @param[out] polygon the resulting convex polygon - */ - void concaveHull(std::vector& cluster, double depth, geometry_msgs::msg::Polygon& polygon); - - void concaveHullClusterCut(std::vector& cluster, double depth, geometry_msgs::msg::Polygon& polygon); - - template - std::size_t findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector& cluster, const std::vector& hull, bool* found); - - - template - bool checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end); - - template - bool checkLineIntersection(const std::vector& hull, const Point1& current_line_start, - const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end); - - double concave_hull_depth_; - - private: - - /** - * @brief Callback for the dynamic_reconfigure node. - * - * This callback allows to modify parameters dynamically at runtime without restarting the node - * @param config Reference to the dynamic reconfigure config - * @param level Dynamic reconfigure level - */ -// void reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level); - -// dynamic_reconfigure::Server* dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime - - -}; - - -template -std::size_t CostmapToPolygonsDBSConcaveHull::findNearestInnerPoint(PointLine line_start, PointLine line_end, const std::vector& cluster, const std::vector& hull, bool* found) -{ - std::size_t nearsest_idx = 0; - double distance = 0; - *found = false; - - for (std::size_t i = 0; i < cluster.size(); ++i) - { - // Skip points that are already in the hull - if (std::find_if( hull.begin(), hull.end(), std::bind(isApprox2d, std::placeholders::_1, std::cref(cluster[i]), 1e-5) ) != hull.end() ) - continue; - - double dist = computeDistanceToLineSegment(cluster[i], line_start, line_end); - bool skip = false; - for (int j = 0; !skip && j < (int)hull.size() - 1; ++j) - { - double dist_temp = computeDistanceToLineSegment(cluster[i], hull[j], hull[j + 1]); - skip |= dist_temp < dist; - } - if (skip) - continue; - - if (!(*found) || dist < distance) - { - nearsest_idx = i; - distance = dist; - *found = true; - } - } - return nearsest_idx; -} - - -template -bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const Point1& line1_start, const Point2& line1_end, const Point3& line2_start, const Point4& line2_end) -{ - double dx1 = line1_end.x - line1_start.x; - double dy1 = line1_end.y - line1_start.y; - double dx2 = line2_end.x - line2_start.x; - double dy2 = line2_end.y - line2_start.y; - double s = (-dy1 * (line1_start.x - line2_start.x) + dx1 * (line1_start.y - line2_start.y)) / (-dx2 * dy1 + dx1 * dy2); - double t = ( dx2 * (line1_start.y - line2_start.y) - dy2 * (line1_start.x - line2_start.x)) / (-dx2 * dy1 + dx1 * dy2); - return (s > 0 && s < 1 && t > 0 && t < 1); -} - - -template -bool CostmapToPolygonsDBSConcaveHull::checkLineIntersection(const std::vector& hull, const Point1& current_line_start, - const Point2& current_line_end, const Point3& test_line_start, const Point4& test_line_end) -{ - for (int i = 0; i < (int)hull.size() - 2; i++) - { - if (isApprox2d(current_line_start, hull[i], 1e-5) && isApprox2d(current_line_end, hull[i+1], 1e-5)) - { - continue; - } - - if (checkLineIntersection(test_line_start, test_line_end, hull[i], hull[i+1])) - { - return true; - } - } - return false; -} - - -} //end namespace teb_local_planner - -#endif /* COSTMAP_TO_POLYGONS_CONCAVE_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/misc.h b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/misc.h deleted file mode 100644 index b3500a7..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/include/costmap_converter/misc.h +++ /dev/null @@ -1,157 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef MISC_H_ -#define MISC_H_ - -#include -#include - -namespace costmap_converter -{ - -/** - * @brief Calculate the distance between a point and a straight line (with infinite length) - * @param point generic 2D point type defining the reference point - * @param line_pt1 generic 2D point as part of the line - * @param line_pt2 generic 2D point as part of the line - * @tparam Point generic point type that should provide x and y member fiels. - * @tparam LinePoint generic point type that should provide x and y member fiels. - * @return (minimum) euclidean distance to the line segment - */ -template -inline double computeDistanceToLine(const Point& point, const LinePoint& line_pt1, const LinePoint& line_pt2) -{ - double dx = line_pt2.x - line_pt1.x; - double dy = line_pt2.y - line_pt1.y; - - double length = std::sqrt(dx*dx + dy*dy); - - if (length>0) - return std::abs(dy * point.x - dx * point.y + line_pt2.x * line_pt1.y - line_pt2.y * line_pt1.x) / length; - - return std::sqrt(std::pow(point.x - line_pt1.x, 2) + std::pow(point.y - line_pt1.y, 2)); -} - - -/** - * @brief Calculate the squared distance between a point and a straight line segment - * @param point generic 2D point type defining the reference point - * @param line_start generic 2D point type defining the start of the line - * @param line_end generic 2D point type defining the end of the line - * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional] - * @tparam Point generic point type that should provide x and y member fiels. - * @tparam LinePoint generic point type that should provide x and y member fiels. - * @return (minimum) squared euclidean distance to the line segment - */ -template -inline double computeSquaredDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL) -{ - double dx = line_end.x - line_start.x; - double dy = line_end.y - line_start.y; - - double length_sqr = dx*dx + dy*dy; - - double u = 0; - - if (length_sqr > 0) - u = ((point.x - line_start.x) * dx + (point.y - line_start.y) * dy) / length_sqr; - - if (is_inbetween) - *is_inbetween = (u>=0 && u<=1); - - if (u <= 0) - return std::pow(point.x-line_start.x,2) + std::pow(point.y-line_start.y,2); - - if (u >= 1) - return std::pow(point.x-line_end.x,2) + std::pow(point.y-line_end.y,2); - - return std::pow(point.x - (line_start.x+u*dx) ,2) + std::pow(point.y - (line_start.y+u*dy),2); -} - -/** - * @brief Calculate the distance between a point and a straight line segment - * @param point generic 2D point type defining the reference point - * @param line_start generic 2D point type defining the start of the line - * @param line_end generic 2D point type defining the end of the line - * @param[out] is_inbetween write \c true, if the point is placed inbetween start and end [optional] - * @tparam Point generic point type that should provide x and y member fiels. - * @tparam LinePoint generic point type that should provide x and y member fiels. - * @return (minimum) euclidean distance to the line segment - */ -template -inline double computeDistanceToLineSegment(const Point& point, const LinePoint& line_start, const LinePoint& line_end, bool* is_inbetween=NULL) -{ - return std::sqrt(computeSquaredDistanceToLineSegment(point, line_start, line_end, is_inbetween)); -} - - -/** - * @brief Calculate the distance between two 2d points - * @param pt1 generic 2D point - * @param pt2 generic 2D point - * @tparam Point1 generic point type that should provide x and y member fiels. - * @tparam Point2 generic point type that should provide x and y member fiels. - * @return euclidean distance to the line segment - */ -template -inline double norm2d(const Point1& pt1, const Point2& pt2) -{ - return std::sqrt( std::pow(pt2.x - pt1.x, 2) + std::pow(pt2.y - pt1.y, 2) ); -} - -/** - * @brief Check if two points are approximately defining the same one - * @param pt1 generic 2D point - * @param pt2 generic 2D point - * @param threshold define the minimum threshold |pt1.x-pt2.y| < tresh && |pt1.y-pt2.y| < tresh - * @tparam Point1 generic point type that should provide x and y member fiels. - * @tparam Point2 generic point type that should provide x and y member fiels. - * @return \c true, if similar, \c false otherwise - */ -template -inline bool isApprox2d(const Point1& pt1, const Point2& pt2, double threshold) -{ - return ( std::abs(pt2.x-pt1.x) - - - costmap_converter - 0.1.2 - - A ros package that includes plugins and nodes to convert occupied costmap2d cells to primitive types. - - - Christoph Rösmann - - Christoph Rösmann - Franz Albers - Otniel Rinaldo - - BSD - - http://wiki.ros.org/costmap_converter - - ament_cmake - - class_loader - costmap_converter_msgs - cv_bridge - geometry_msgs - nav2_costmap_2d - tf2 - pluginlib - rclcpp - - - ament_cmake_gtest - - - - ament_cmake - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_converter_node.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_converter_node.cpp deleted file mode 100644 index e72a30f..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_converter_node.cpp +++ /dev/null @@ -1,264 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann, Otniel Rinaldo - *********************************************************************/ -#include - -#include - -#include -#include -#include -#include - -#include -#include - -class CostmapStandaloneConversion : public rclcpp::Node { - public: - CostmapStandaloneConversion(const std::string node_name) - : rclcpp::Node(node_name), - converter_loader_("costmap_converter", - "costmap_converter::BaseCostmapToPolygons") { - costmap_ros_ = - std::make_shared("converter_costmap"); - costmap_thread_ = std::make_unique( - [](rclcpp_lifecycle::LifecycleNode::SharedPtr node) { - rclcpp::spin(node->get_node_base_interface()); - }, - costmap_ros_); - rclcpp_lifecycle::State state; - costmap_ros_->on_configure(state); - costmap_ros_->on_activate(state); - - n_ = std::shared_ptr(this, [](rclcpp::Node *) {}); - // load converter plugin from parameter server, otherwise set default - - std::string converter_plugin = - "costmap_converter::CostmapToPolygonsDBSMCCH"; - - declare_parameter("converter_plugin", - rclcpp::ParameterValue(converter_plugin)); - - get_parameter_or("converter_plugin", converter_plugin, - converter_plugin); - - try { - converter_ = converter_loader_.createSharedInstance(converter_plugin); - } catch (const pluginlib::PluginlibException &ex) { - RCLCPP_ERROR(get_logger(), - "The plugin failed to load for some reason. Error: %s", - ex.what()); - rclcpp::shutdown(); - return; - } - - RCLCPP_INFO(get_logger(), "Standalone costmap converter: %s loaded.", - converter_plugin.c_str()); - - std::string obstacles_topic = "costmap_obstacles"; - declare_parameter("obstacles_topic", - rclcpp::ParameterValue(obstacles_topic)); - get_parameter_or("obstacles_topic", obstacles_topic, - obstacles_topic); - - std::string polygon_marker_topic = "costmap_polygon_markers"; - declare_parameter("polygon_marker_topic", - rclcpp::ParameterValue(polygon_marker_topic)); - get_parameter_or("polygon_marker_topic", polygon_marker_topic, - polygon_marker_topic); - - obstacle_pub_ = - create_publisher( - obstacles_topic, 1000); - marker_pub_ = create_publisher( - polygon_marker_topic, 10); - - occupied_min_value_ = 100; - declare_parameter("occupied_min_value", - rclcpp::ParameterValue(occupied_min_value_)); - get_parameter_or("occupied_min_value", occupied_min_value_, - occupied_min_value_); - - std::string odom_topic = "/odom"; - declare_parameter("odom_topic", rclcpp::ParameterValue(odom_topic)); - get_parameter_or("odom_topic", odom_topic, odom_topic); - - if (converter_) { - converter_->setOdomTopic(odom_topic); - converter_->initialize(shared_from_this()); - converter_->startWorker(std::make_shared(5), - costmap_ros_->getCostmap(), false); - } - - pub_timer_ = n_->create_wall_timer( - std::chrono::milliseconds(200), - std::bind(&CostmapStandaloneConversion::publishCallback, this)); - } - - void publishCallback() { - costmap_converter::ObstacleArrayConstPtr obstacles = - converter_->getObstacles(); - - if (!obstacles) return; - - obstacle_pub_->publish(*obstacles); - - frame_id_ = costmap_ros_->getGlobalFrameID(); - - publishAsMarker(frame_id_, *obstacles); - } - - void publishAsMarker( - const std::string &frame_id, - const std::vector &polygonStamped) { - visualization_msgs::msg::Marker line_list; - line_list.header.frame_id = frame_id; - line_list.header.stamp = now(); - line_list.ns = "Polygons"; - line_list.action = visualization_msgs::msg::Marker::ADD; - line_list.pose.orientation.w = 1.0; - - line_list.id = 0; - line_list.type = visualization_msgs::msg::Marker::LINE_LIST; - - line_list.scale.x = 0.1; - line_list.color.g = 1.0; - line_list.color.a = 1.0; - - for (std::size_t i = 0; i < polygonStamped.size(); ++i) { - for (int j = 0; j < (int)polygonStamped[i].polygon.points.size() - 1; - ++j) { - geometry_msgs::msg::Point line_start; - line_start.x = polygonStamped[i].polygon.points[j].x; - line_start.y = polygonStamped[i].polygon.points[j].y; - line_list.points.push_back(line_start); - geometry_msgs::msg::Point line_end; - line_end.x = polygonStamped[i].polygon.points[j + 1].x; - line_end.y = polygonStamped[i].polygon.points[j + 1].y; - line_list.points.push_back(line_end); - } - // close loop for current polygon - if (!polygonStamped[i].polygon.points.empty() && - polygonStamped[i].polygon.points.size() != 2) { - geometry_msgs::msg::Point line_start; - line_start.x = polygonStamped[i].polygon.points.back().x; - line_start.y = polygonStamped[i].polygon.points.back().y; - line_list.points.push_back(line_start); - if (line_list.points.size() % 2 != 0) { - geometry_msgs::msg::Point line_end; - line_end.x = polygonStamped[i].polygon.points.front().x; - line_end.y = polygonStamped[i].polygon.points.front().y; - line_list.points.push_back(line_end); - } - } - } - marker_pub_->publish(line_list); - } - - void publishAsMarker( - const std::string &frame_id, - const costmap_converter_msgs::msg::ObstacleArrayMsg &obstacles) { - visualization_msgs::msg::Marker line_list; - line_list.header.frame_id = frame_id; - line_list.header.stamp = now(); - line_list.ns = "Polygons"; - line_list.action = visualization_msgs::msg::Marker::ADD; - line_list.pose.orientation.w = 1.0; - - line_list.id = 0; - line_list.type = visualization_msgs::msg::Marker::LINE_LIST; - - line_list.scale.x = 0.1; - line_list.color.g = 1.0; - line_list.color.a = 1.0; - - for (const auto &obstacle : obstacles.obstacles) { - for (int j = 0; j < (int)obstacle.polygon.points.size() - 1; ++j) { - geometry_msgs::msg::Point line_start; - line_start.x = obstacle.polygon.points[j].x; - line_start.y = obstacle.polygon.points[j].y; - line_list.points.push_back(line_start); - geometry_msgs::msg::Point line_end; - line_end.x = obstacle.polygon.points[j + 1].x; - line_end.y = obstacle.polygon.points[j + 1].y; - line_list.points.push_back(line_end); - } - // close loop for current polygon - if (!obstacle.polygon.points.empty() && - obstacle.polygon.points.size() != 2) { - geometry_msgs::msg::Point line_start; - line_start.x = obstacle.polygon.points.back().x; - line_start.y = obstacle.polygon.points.back().y; - line_list.points.push_back(line_start); - if (line_list.points.size() % 2 != 0) { - geometry_msgs::msg::Point line_end; - line_end.x = obstacle.polygon.points.front().x; - line_end.y = obstacle.polygon.points.front().y; - line_list.points.push_back(line_end); - } - } - } - marker_pub_->publish(line_list); - } - - private: - pluginlib::ClassLoader - converter_loader_; - std::shared_ptr converter_; - - rclcpp::Node::SharedPtr n_; - std::shared_ptr costmap_ros_; - std::unique_ptr costmap_thread_; - rclcpp::Publisher::SharedPtr - obstacle_pub_; - rclcpp::Publisher::SharedPtr marker_pub_; - rclcpp::TimerBase::SharedPtr pub_timer_; - - std::string frame_id_; - int occupied_min_value_; -}; - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - - auto convert_process = - std::make_shared("costmap_converter"); - - rclcpp::spin(convert_process); - - return 0; -} diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/background_subtractor.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/background_subtractor.cpp deleted file mode 100644 index 37256bc..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/background_subtractor.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include -//#include -#include - -BackgroundSubtractor::BackgroundSubtractor(const Params ¶meters): params_(parameters) -{ -} - -void BackgroundSubtractor::apply(const cv::Mat& image, cv::Mat& fg_mask, int shift_x, int shift_y) -{ - current_frame_ = image; - - // occupancy grids are empty only once in the beginning -> initialize variables - if (occupancy_grid_fast_.empty() && occupancy_grid_slow_.empty()) - { - occupancy_grid_fast_ = current_frame_; - occupancy_grid_slow_ = current_frame_; - previous_shift_x_ = shift_x; - previous_shift_y_ = shift_y; - return; - } - - // Shift previous occupancy grid to new location (match current_frame_) - int shift_relative_to_previous_pos_x_ = shift_x - previous_shift_x_; - int shift_relative_to_previous_pos_y_ = shift_y - previous_shift_y_; - previous_shift_x_ = shift_x; - previous_shift_y_ = shift_y; - - // if(shift_relative_to_previous_pos_x_ != 0 || shift_relative_to_previous_pos_y_ != 0) - transformToCurrentFrame(shift_relative_to_previous_pos_x_, shift_relative_to_previous_pos_y_); - - // cvv::debugFilter(occupancy_grid_fast_, currentFrame_, CVVISUAL_LOCATION); - - // Calculate normalized sum (mean) of nearest neighbors - int size = 3; // 3, 5, 7, .... - cv::Mat nearest_neighbor_mean_fast(occupancy_grid_fast_.size(), CV_8UC1); - cv::Mat nearest_neighbor_mean_slow(occupancy_grid_slow_.size(), CV_8UC1); - cv::boxFilter(occupancy_grid_fast_, nearest_neighbor_mean_fast, -1, cv::Size(size, size), cv::Point(-1, -1), true, - cv::BORDER_REPLICATE); - cv::boxFilter(occupancy_grid_slow_, nearest_neighbor_mean_slow, -1, cv::Size(size, size), cv::Point(-1, -1), true, - cv::BORDER_REPLICATE); - // cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE); - // cv::GaussianBlur(occupancy_grid_fast_, nearest_neighbor_mean_fast, cv::Size(size,size), 1, 1, cv::BORDER_REPLICATE); - - // compute time mean value for each pixel according to learningrate alpha - // occupancy_grid_fast_ = beta*(alpha_fast*current_frame_ + (1.0-alpha_fast)*occupancy_grid_fast_) + (1-beta)*nearest_neighbor_mean_fast; - cv::addWeighted(current_frame_, params_.alpha_fast, occupancy_grid_fast_, (1 - params_.alpha_fast), 0, occupancy_grid_fast_); - cv::addWeighted(occupancy_grid_fast_, params_.beta, nearest_neighbor_mean_fast, (1 - params_.beta), 0, occupancy_grid_fast_); - // occupancy_grid_slow_ = beta*(alpha_slow*current_frame_ + (1.0-alpha_slow)*occupancy_grid_slow) + (1-beta)*nearest_neighbor_mean_slow; - cv::addWeighted(current_frame_, params_.alpha_slow, occupancy_grid_slow_, (1 - params_.alpha_slow), 0, occupancy_grid_slow_); - cv::addWeighted(occupancy_grid_slow_, params_.beta, nearest_neighbor_mean_slow, (1 - params_.beta), 0, occupancy_grid_slow_); - - // 1) Obstacles should be detected after a minimum response of the fast filter - // occupancy_grid_fast_ > min_occupancy_probability - cv::threshold(occupancy_grid_fast_, occupancy_grid_fast_, params_.min_occupancy_probability, 0 /*unused*/, cv::THRESH_TOZERO); - // 2) Moving obstacles have a minimum difference between the responses of the slow and fast filter - // occupancy_grid_fast_-occupancy_grid_slow_ > min_sep_between_fast_and_slow_filter - cv::threshold(occupancy_grid_fast_ - occupancy_grid_slow_, fg_mask, params_.min_sep_between_fast_and_slow_filter, 255, - cv::THRESH_BINARY); - // 3) Dismiss static obstacles - // nearest_neighbor_mean_slow < max_occupancy_neighbors - cv::threshold(nearest_neighbor_mean_slow, nearest_neighbor_mean_slow, params_.max_occupancy_neighbors, 255, cv::THRESH_BINARY_INV); - cv::bitwise_and(nearest_neighbor_mean_slow, fg_mask, fg_mask); - - //visualize("Current frame", currentFrame_); - cv::Mat setBorderToZero = cv::Mat(current_frame_.size(), CV_8UC1, 0.0); - int border = 5; - setBorderToZero(cv::Rect(border, border, current_frame_.cols-2*border, current_frame_.rows-2*border)) = 255; - - cv::bitwise_and(setBorderToZero, fg_mask, fg_mask); - - // cv::imwrite("/home/albers/Desktop/currentFrame.png", currentFrame_); - // visualize("Foreground mask", fgMask); - - // Closing Operation - cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, - cv::Size(2*params_.morph_size + 1, 2*params_.morph_size + 1), - cv::Point(params_.morph_size, params_.morph_size)); - cv::dilate(fg_mask, fg_mask, element); - cv::dilate(fg_mask, fg_mask, element); - cv::erode(fg_mask, fg_mask, element); -} - -void BackgroundSubtractor::transformToCurrentFrame(int shift_x, int shift_y) -{ - // TODO: initialize with current_frame (first observed image) rather than zeros - - // Translate (shift) image in costmap coordinates - // in cv::Mat: shift X -> to the left; shift y -> to the top - cv::Mat temp_fast, temp_slow; - cv::Mat translation_mat = (cv::Mat_(2, 3, CV_64F) << 1, 0, -shift_x, 0, 1, -shift_y); - cv::warpAffine(occupancy_grid_fast_, temp_fast, translation_mat, occupancy_grid_fast_.size()); // can't operate in-place - cv::warpAffine(occupancy_grid_slow_, temp_slow, translation_mat, occupancy_grid_slow_.size()); // can't operate in-place - - // cvv::debugFilter(occupancy_grid_fast_, temp_fast); - - occupancy_grid_fast_ = temp_fast; - occupancy_grid_slow_ = temp_slow; -} - -void BackgroundSubtractor::visualize(const std::string& name, const cv::Mat& image) -{ - if (!image.empty()) - { - cv::Mat im = image.clone(); - cv::flip(im, im, 0); - cv::imshow(name, im); - cv::waitKey(1); - } -} - -void BackgroundSubtractor::updateParameters(const Params ¶meters) -{ - params_ = parameters; -} diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/blob_detector.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/blob_detector.cpp deleted file mode 100755 index c7352f4..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/blob_detector.cpp +++ /dev/null @@ -1,193 +0,0 @@ -#include -#include -#include - -BlobDetector::BlobDetector(const SimpleBlobDetector::Params& parameters) : params_(parameters) {} - -cv::Ptr BlobDetector::create(const cv::SimpleBlobDetector::Params& params) -{ - return cv::Ptr (new BlobDetector(params)); // compatibility with older versions - //return cv::makePtr(params); -} - -void BlobDetector::detect(const cv::Mat& image, std::vector& keypoints, const cv::Mat&) -{ - // TODO: support mask - contours_.clear(); - - keypoints.clear(); - cv::Mat grayscale_image; - if (image.channels() == 3) - cv::cvtColor(image, grayscale_image, cv::COLOR_BGR2GRAY); - else - grayscale_image = image; - - if (grayscale_image.type() != CV_8UC1) - { - //CV_Error(cv::Error::StsUnsupportedFormat, "Blob detector only supports 8-bit images!"); - std::cerr << "Blob detector only supports 8-bit images!\n"; - } - - std::vector> centers; - std::vector> contours; - for (double thresh = params_.minThreshold; thresh < params_.maxThreshold; thresh += params_.thresholdStep) - { - cv::Mat binarized_image; - cv::threshold(grayscale_image, binarized_image, thresh, 255, cv::THRESH_BINARY); - - std::vector
cur_centers; - std::vector> cur_contours, new_contours; - findBlobs(grayscale_image, binarized_image, cur_centers, cur_contours); - std::vector> new_centers; - for (std::size_t i = 0; i < cur_centers.size(); ++i) - { - bool isNew = true; - for (std::size_t j = 0; j < centers.size(); ++j) - { - double dist = cv::norm(centers[j][centers[j].size() / 2].location - cur_centers[i].location); - isNew = dist >= params_.minDistBetweenBlobs && dist >= centers[j][centers[j].size() / 2].radius && - dist >= cur_centers[i].radius; - if (!isNew) - { - centers[j].push_back(cur_centers[i]); - - size_t k = centers[j].size() - 1; - while (k > 0 && centers[j][k].radius < centers[j][k - 1].radius) - { - centers[j][k] = centers[j][k - 1]; - k--; - } - centers[j][k] = cur_centers[i]; - - break; - } - } - if (isNew) - { - new_centers.push_back(std::vector
(1, cur_centers[i])); - new_contours.push_back(cur_contours[i]); - } - } - std::copy(new_centers.begin(), new_centers.end(), std::back_inserter(centers)); - std::copy(new_contours.begin(), new_contours.end(), std::back_inserter(contours)); - } - - for (size_t i = 0; i < centers.size(); ++i) - { - if (centers[i].size() < params_.minRepeatability) - continue; - cv::Point2d sum_point(0, 0); - double normalizer = 0; - for (std::size_t j = 0; j < centers[i].size(); ++j) - { - sum_point += centers[i][j].confidence * centers[i][j].location; - normalizer += centers[i][j].confidence; - } - sum_point *= (1. / normalizer); - cv::KeyPoint kpt(sum_point, (float)(centers[i][centers[i].size() / 2].radius)); - keypoints.push_back(kpt); - contours_.push_back(contours[i]); - } -} - -void BlobDetector::findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector
& centers, - std::vector>& cur_contours) const -{ - (void)image; - centers.clear(); - cur_contours.clear(); - - std::vector> contours; - cv::Mat tmp_binary_image = binary_image.clone(); - cv::findContours(tmp_binary_image, contours, cv::RETR_LIST, cv::CHAIN_APPROX_SIMPLE); - - for (std::size_t contour_idx = 0; contour_idx < contours.size(); ++contour_idx) - { - Center center; - center.confidence = 1; - cv::Moments moms = cv::moments(cv::Mat(contours[contour_idx])); - if (params_.filterByArea) - { - double area = moms.m00; - if (area < params_.minArea || area >= params_.maxArea) - continue; - } - - if (params_.filterByCircularity) - { - double area = moms.m00; - double perimeter = cv::arcLength(cv::Mat(contours[contour_idx]), true); - double ratio = 4 * CV_PI * area / (perimeter * perimeter); - if (ratio < params_.minCircularity || ratio >= params_.maxCircularity) - continue; - } - - if (params_.filterByInertia) - { - double denominator = std::sqrt(std::pow(2 * moms.mu11, 2) + std::pow(moms.mu20 - moms.mu02, 2)); - const double eps = 1e-2; - double ratio; - if (denominator > eps) - { - double cosmin = (moms.mu20 - moms.mu02) / denominator; - double sinmin = 2 * moms.mu11 / denominator; - double cosmax = -cosmin; - double sinmax = -sinmin; - - double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin; - double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax; - ratio = imin / imax; - } - else - { - ratio = 1; - } - - if (ratio < params_.minInertiaRatio || ratio >= params_.maxInertiaRatio) - continue; - - center.confidence = ratio * ratio; - } - - if (params_.filterByConvexity) - { - std::vector hull; - cv::convexHull(cv::Mat(contours[contour_idx]), hull); - double area = cv::contourArea(cv::Mat(contours[contour_idx])); - double hullArea = cv::contourArea(cv::Mat(hull)); - double ratio = area / hullArea; - if (ratio < params_.minConvexity || ratio >= params_.maxConvexity) - continue; - } - - if (moms.m00 == 0.0) - continue; - center.location = cv::Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00); - - if (params_.filterByColor) - { - if (binary_image.at(cvRound(center.location.y), cvRound(center.location.x)) != params_.blobColor) - continue; - } - - // compute blob radius - { - std::vector dists; - for (std::size_t point_idx = 0; point_idx < contours[contour_idx].size(); ++point_idx) - { - cv::Point2d pt = contours[contour_idx][point_idx]; - dists.push_back(cv::norm(center.location - pt)); - } - std::sort(dists.begin(), dists.end()); - center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.; - } - - centers.push_back(center); - cur_contours.push_back(contours[contour_idx]); - } -} - -void BlobDetector::updateParameters(const Params& parameters) -{ - params_ = parameters; -} diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp deleted file mode 100644 index 5848124..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/costmap_to_dynamic_obstacles.cpp +++ /dev/null @@ -1,489 +0,0 @@ -#include - -#include -#include -#include -#include - -PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToDynamicObstacles, costmap_converter::BaseCostmapToPolygons) - -namespace costmap_converter -{ - -CostmapToDynamicObstacles::CostmapToDynamicObstacles() : BaseCostmapToDynamicObstacles() -{ - ego_vel_.x = ego_vel_.y = ego_vel_.z = 0; - costmap_ = nullptr; -// dynamic_recfg_ = nullptr; -} - -CostmapToDynamicObstacles::~CostmapToDynamicObstacles() -{ -// if(dynamic_recfg_ != nullptr) -// delete dynamic_recfg_; -} - -void CostmapToDynamicObstacles::initialize(rclcpp::Node::SharedPtr nh) -{ - BaseCostmapToPolygons::initialize(nh); - - costmap_ = nullptr; - - // We need the odometry from the robot to compensate the ego motion -// rclcpp::Node::SharedPtr gn; // create odom topic w.r.t. global node handle - odom_sub_ = nh->create_subscription( - odom_topic_, - rclcpp::SystemDefaultsQoS(), - std::bind(&CostmapToDynamicObstacles::odomCallback, this, std::placeholders::_1)); - - nh->get_parameter_or("publish_static_obstacles", publish_static_obstacles_, publish_static_obstacles_); - - ////////////////////////////////// - // Foreground detection parameters - BackgroundSubtractor::Params bg_sub_params; - - bg_sub_params.alpha_slow = 0.3; - nh->get_parameter_or("alpha_slow", bg_sub_params.alpha_slow, bg_sub_params.alpha_slow); - - bg_sub_params.alpha_fast = 0.85; - nh->get_parameter_or("alpha_fast", bg_sub_params.alpha_fast, bg_sub_params.alpha_fast); - - bg_sub_params.beta = 0.85; - nh->get_parameter_or("beta", bg_sub_params.beta, bg_sub_params.beta); - - bg_sub_params.min_occupancy_probability = 180; - nh->get_parameter_or("min_occupancy_probability", bg_sub_params.min_occupancy_probability, bg_sub_params.min_occupancy_probability); - - bg_sub_params.min_sep_between_fast_and_slow_filter = 80; - nh->get_parameter_or("min_sep_between_slow_and_fast_filter", bg_sub_params.min_sep_between_fast_and_slow_filter, bg_sub_params.min_sep_between_fast_and_slow_filter); - - bg_sub_params.max_occupancy_neighbors = 100; - nh->get_parameter_or("max_occupancy_neighbors", bg_sub_params.max_occupancy_neighbors, bg_sub_params.max_occupancy_neighbors); - - bg_sub_params.morph_size = 1; - nh->get_parameter_or("morph_size", bg_sub_params.morph_size, bg_sub_params.morph_size); - - bg_sub_ = std::unique_ptr(new BackgroundSubtractor(bg_sub_params)); - - //////////////////////////// - // Blob detection parameters - BlobDetector::Params blob_det_params; - - blob_det_params.filterByColor = true; // actually filterByIntensity, always true - blob_det_params.blobColor = 255; // Extract light blobs - blob_det_params.thresholdStep = 256; // Input for blob detection is already a binary image - blob_det_params.minThreshold = 127; - blob_det_params.maxThreshold = 255; - blob_det_params.minRepeatability = 1; - - blob_det_params.minDistBetweenBlobs = 10; - nh->get_parameter_or("min_distance_between_blobs", blob_det_params.minDistBetweenBlobs, blob_det_params.minDistBetweenBlobs); - - blob_det_params.filterByArea = true; - nh->get_parameter_or("filter_by_area", blob_det_params.filterByArea, blob_det_params.filterByArea); - - blob_det_params.minArea = 3; // Filter out blobs with less pixels - nh->get_parameter_or("min_area", blob_det_params.minArea, blob_det_params.minArea); - - blob_det_params.maxArea = 300; - nh->get_parameter_or("max_area", blob_det_params.maxArea, blob_det_params.maxArea); - - blob_det_params.filterByCircularity = true; // circularity = 4*pi*area/perimeter^2 - nh->get_parameter_or("filter_by_circularity", blob_det_params.filterByCircularity, blob_det_params.filterByCircularity); - - blob_det_params.minCircularity = 0.2; - nh->get_parameter_or("min_circularity", blob_det_params.minCircularity, blob_det_params.minCircularity); - - blob_det_params.maxCircularity = 1; // maximal 1 (in case of a circle) - nh->get_parameter_or("max_circularity", blob_det_params.maxCircularity, blob_det_params.maxCircularity); - - blob_det_params.filterByInertia = true; // Filter blobs based on their elongation - nh->get_parameter_or("filter_by_intertia", blob_det_params.filterByInertia, blob_det_params.filterByInertia); - - blob_det_params.minInertiaRatio = 0.2; // minimal 0 (in case of a line) - nh->get_parameter_or("min_inertia_ratio", blob_det_params.minInertiaRatio, blob_det_params.minInertiaRatio); - - blob_det_params.maxInertiaRatio = 1; // maximal 1 (in case of a circle) - nh->get_parameter_or("max_intertia_ratio", blob_det_params.maxInertiaRatio, blob_det_params.maxInertiaRatio); - - blob_det_params.filterByConvexity = false; // Area of the Blob / Area of its convex hull - nh->get_parameter_or("filter_by_convexity", blob_det_params.filterByConvexity, blob_det_params.filterByConvexity); - - blob_det_params.minConvexity = 0; // minimal 0 - nh->get_parameter_or("min_convexity", blob_det_params.minConvexity, blob_det_params.minConvexity); - - blob_det_params.maxConvexity = 1; // maximal 1 - nh->get_parameter_or("max_convexity", blob_det_params.maxConvexity, blob_det_params.maxConvexity); - - blob_det_ = BlobDetector::create(blob_det_params); - - //////////////////////////////////// - // Tracking parameters - CTracker::Params tracker_params; - tracker_params.dt = 0.2; - nh->get_parameter_or("dt", tracker_params.dt, tracker_params.dt); - - tracker_params.dist_thresh = 60.0; - nh->get_parameter_or("dist_thresh", tracker_params.dist_thresh, tracker_params.dist_thresh); - - tracker_params.max_allowed_skipped_frames = 3; - nh->get_parameter_or("max_allowed_skipped_frames", tracker_params.max_allowed_skipped_frames, tracker_params.max_allowed_skipped_frames); - - tracker_params.max_trace_length = 10; - nh->get_parameter_or("max_trace_length", tracker_params.max_trace_length, tracker_params.max_trace_length); - - tracker_ = std::unique_ptr(new CTracker(tracker_params)); - - - //////////////////////////////////// - // Static costmap conversion parameters - std::string static_converter_plugin = "costmap_converter::CostmapToPolygonsDBSMCCH"; - nh->get_parameter_or("static_converter_plugin", static_converter_plugin, static_converter_plugin); - loadStaticCostmapConverterPlugin(static_converter_plugin, nh); - - -// setup dynamic reconfigure -// dynamic_recfg_ = new dynamic_reconfigure::Server(nh); -// dynamic_reconfigure::Server::CallbackType cb = boost::bind(&CostmapToDynamicObstacles::reconfigureCB, this, _1, _2); -// dynamic_recfg_->setCallback(cb); -} - -void CostmapToDynamicObstacles::compute() -{ - if (costmap_mat_.empty()) - return; - - /////////////////////////// Foreground detection //////////////////////////////////// - // Dynamic obstacles are separated from static obstacles - int origin_x = round(costmap_->getOriginX() / costmap_->getResolution()); - int origin_y = round(costmap_->getOriginY() / costmap_->getResolution()); - // ROS_INFO("Origin x [m]: %f Origin_y [m]: %f", costmap_->getOriginX(), costmap_->getOriginY()); - // ROS_INFO("Origin x [px]: %d \t Origin_y [px]: %d", originX, originY); - - bg_sub_->apply(costmap_mat_, fg_mask_, origin_x, origin_y); - - // if no foreground object is detected, no ObstacleMsgs need to be published - if (fg_mask_.empty()) - return; - - cv::Mat bg_mat; - if (publish_static_obstacles_) - { - // Get static obstacles - bg_mat = costmap_mat_ - fg_mask_; - // visualize("bg_mat", bg_mat); - } - - - /////////////////////////////// Blob detection ///////////////////////////////////// - // Centers and contours of Blobs are detected - blob_det_->detect(fg_mask_, keypoints_); - std::vector> contours = blob_det_->getContours(); - - - ////////////////////////////// Tracking //////////////////////////////////////////// - // Objects are assigned to objects from previous frame based on Hungarian Algorithm - // Object velocities are estimated using a Kalman Filter - std::vector detected_centers(keypoints_.size()); - for (size_t i = 0; i < keypoints_.size(); i++) - { - detected_centers.at(i).x = keypoints_.at(i).pt.x; - detected_centers.at(i).y = keypoints_.at(i).pt.y; - detected_centers.at(i).z = 0; // Currently unused! - } - - tracker_->Update(detected_centers, contours); - - - ///////////////////////////////////// Output /////////////////////////////////////// - /* - cv::Mat fg_mask_with_keypoints = cv::Mat::zeros(fg_mask.size(), CV_8UC3); - cv::cvtColor(fg_mask, fg_mask_with_keypoints, cv::COLOR_GRAY2BGR); - - for (int i = 0; i < (int)tracker_->tracks.size(); ++i) - cv::rectangle(fg_mask_with_keypoints, cv::boundingRect(tracker_->tracks[i]->getLastContour()), - cv::Scalar(0, 0, 255), 1); - - //visualize("fgMaskWithKeyPoints", fgMaskWithKeypoints); - //cv::imwrite("/home/albers/Desktop/fgMask.png", fgMask); - //cv::imwrite("/home/albers/Desktop/fgMaskWithKeyPoints.png", fgMaskWithKeypoints); - */ - - //////////////////////////// Fill ObstacleContainerPtr ///////////////////////////// - ObstacleArrayPtr obstacles(new costmap_converter_msgs::msg::ObstacleArrayMsg); - // header.seq is automatically filled - obstacles->header.stamp = now(); - obstacles->header.frame_id = "/map"; //Global frame /map - - // For all tracked objects - for (unsigned int i = 0; i < (unsigned int)tracker_->tracks.size(); ++i) - { - geometry_msgs::msg::Polygon polygon; - - // TODO directly create polygon inside getContour and avoid copy - std::vector contour; - getContour(i, contour); // this method also transforms map to world coordinates - - // convert contour to polygon - for (const Point_t& pt : contour) - { - polygon.points.emplace_back(); - polygon.points.back().x = pt.x; - polygon.points.back().y = pt.y; - polygon.points.back().z = 0; - } - - obstacles->obstacles.emplace_back(); - obstacles->obstacles.back().polygon = polygon; - - // Set obstacle ID - obstacles->obstacles.back().id = tracker_->tracks.at(i)->track_id; - - // Set orientation - geometry_msgs::msg::QuaternionStamped orientation; - - Point_t vel = getEstimatedVelocityOfObject(i); - double yaw = std::atan2(vel.y, vel.x); - //ROS_INFO("yaw: %f", yaw); - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - obstacles->obstacles.back().orientation = tf2::toMsg(q); - - // Set velocity - geometry_msgs::msg::TwistWithCovariance velocities; - //velocities.twist.linear.x = std::sqrt(vel.x*vel.x + vel.y*vel.y); - //velocities.twist.linear.y = 0; - velocities.twist.linear.x = vel.x; - velocities.twist.linear.y = vel.y; // TODO(roesmann): don't we need to consider the transformation between opencv's and costmap's coordinate frames? - velocities.twist.linear.z = 0; - velocities.twist.angular.x = 0; - velocities.twist.angular.y = 0; - velocities.twist.angular.z = 0; - - // TODO: use correct covariance matrix - velocities.covariance = {1, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1}; - - obstacles->obstacles.back().velocities = velocities; - } - - ////////////////////////// Static obstacles //////////////////////////// - if (publish_static_obstacles_) - { - uchar* img_data = bg_mat.data; - int width = bg_mat.cols; - int height = bg_mat.rows; - int stride = bg_mat.step; - - if (stackedCostmapConversion()) - { - // Create new costmap with static obstacles (background) - std::shared_ptr static_costmap(new nav2_costmap_2d::Costmap2D(costmap_->getSizeInCellsX(), - costmap_->getSizeInCellsY(), - costmap_->getResolution(), - costmap_->getOriginX(), - costmap_->getOriginY())); - for(int i = 0; i < height; i++) - { - for(int j = 0; j < width; j++) - { - static_costmap->setCost(j, i, img_data[i * stride + j]); - } - } - - // Apply static obstacle conversion plugin - setStaticCostmap(static_costmap); - convertStaticObstacles(); - - // Store converted static obstacles for publishing - auto static_polygons = getStaticPolygons(); - for (auto it = static_polygons->begin(); it != static_polygons->end(); ++it) - { - obstacles->obstacles.emplace_back(); - obstacles->obstacles.back().polygon = *it; - obstacles->obstacles.back().velocities.twist.linear.x = 0; - obstacles->obstacles.back().velocities.twist.linear.y = 0; - obstacles->obstacles.back().id = -1; - } - } - // Otherwise leave static obstacles point-shaped - else - { - for(int i = 0; i < height; i++) - { - for(int j = 0; j < width; j++) - { - uchar value = img_data[i * stride + j]; - if (value > 0) - { - // obstacle found - obstacles->obstacles.emplace_back(); - geometry_msgs::msg::Point32 pt; - pt.x = (double)j*costmap_->getResolution() + costmap_->getOriginX(); - pt.y = (double)i*costmap_->getResolution() + costmap_->getOriginY(); - obstacles->obstacles.back().polygon.points.push_back(pt); - obstacles->obstacles.back().velocities.twist.linear.x = 0; - obstacles->obstacles.back().velocities.twist.linear.y = 0; - obstacles->obstacles.back().id = -1; - } - } - } - } - } - - updateObstacleContainer(obstacles); -} - -void CostmapToDynamicObstacles::setCostmap2D(nav2_costmap_2d::Costmap2D* costmap) -{ - if (!costmap) - return; - - costmap_ = costmap; - - updateCostmap2D(); -} - -void CostmapToDynamicObstacles::updateCostmap2D() -{ - if (!costmap_->getMutex()) - { - RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null"); - return; - } - std::unique_lock lock(*costmap_->getMutex()); - - // Initialize costmapMat_ directly with the unsigned char array of costmap_ - //costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1, - // costmap_->getCharMap()).clone(); // Deep copy: TODO - costmap_mat_ = cv::Mat(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY(), CV_8UC1, - costmap_->getCharMap()); -} - -ObstacleArrayConstPtr CostmapToDynamicObstacles::getObstacles() -{ - std::lock_guard lock(mutex_); - return obstacles_; -} - -void CostmapToDynamicObstacles::updateObstacleContainer(ObstacleArrayPtr obstacles) -{ - std::lock_guard lock(mutex_); - obstacles_ = obstacles; -} - -Point_t CostmapToDynamicObstacles::getEstimatedVelocityOfObject(unsigned int idx) -{ - // vel [px/s] * costmapResolution [m/px] = vel [m/s] - Point_t vel = tracker_->tracks.at(idx)->getEstimatedVelocity() * costmap_->getResolution() + ego_vel_; - - //ROS_INFO("vel x: %f, vel y: %f, vel z: %f", vel.x, vel.y, vel.z); - // velocity in /map frame - return vel; -} - -void CostmapToDynamicObstacles::odomCallback(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - RCLCPP_INFO_ONCE(getLogger(), "CostmapToDynamicObstacles: odom received."); - - tf2::Quaternion pose; - tf2::fromMsg(msg->pose.pose.orientation, pose); - - tf2::Vector3 twistLinear; - // tf2::fromMsg(msg->twist.twist.linear, twistLinear); // not available in tf2 - twistLinear.setX(msg->twist.twist.linear.x); - twistLinear.setY(msg->twist.twist.linear.y); - twistLinear.setZ(msg->twist.twist.linear.z); - - - // velocity of the robot in x, y and z coordinates - tf2::Vector3 vel = tf2::quatRotate(pose, twistLinear); - ego_vel_.x = vel.x(); - ego_vel_.y = vel.y(); - ego_vel_.z = vel.z(); -} - -//void CostmapToDynamicObstacles::reconfigureCB(CostmapToDynamicObstaclesConfig& config, uint32_t level) -//{ -// publish_static_obstacles_ = config.publish_static_obstacles; - -// // BackgroundSubtraction Parameters -// BackgroundSubtractor::Params bg_sub_params; -// bg_sub_params.alpha_slow = config.alpha_slow; -// bg_sub_params.alpha_fast = config.alpha_fast; -// bg_sub_params.beta = config.beta; -// bg_sub_params.min_sep_between_fast_and_slow_filter = config.min_sep_between_slow_and_fast_filter; -// bg_sub_params.min_occupancy_probability = config.min_occupancy_probability; -// bg_sub_params.max_occupancy_neighbors = config.max_occupancy_neighbors; -// bg_sub_params.morph_size = config.morph_size; -// bg_sub_->updateParameters(bg_sub_params); - -// // BlobDetector Parameters -// BlobDetector::Params blob_det_params; -// // necessary, because blobDetParams are otherwise initialized with default values for dark blobs -// blob_det_params.filterByColor = true; // actually filterByIntensity, always true -// blob_det_params.blobColor = 255; // Extract light blobs -// blob_det_params.thresholdStep = 256; // Input for blobDetection is already a binary image -// blob_det_params.minThreshold = 127; -// blob_det_params.maxThreshold = 255; -// blob_det_params.minRepeatability = 1; -// blob_det_params.minDistBetweenBlobs = config.min_distance_between_blobs; // TODO: Currently not working as expected -// blob_det_params.filterByArea = config.filter_by_area; -// blob_det_params.minArea = config.min_area; -// blob_det_params.maxArea = config.max_area; -// blob_det_params.filterByCircularity = config.filter_by_circularity; -// blob_det_params.minCircularity = config.min_circularity; -// blob_det_params.maxCircularity = config.max_circularity; -// blob_det_params.filterByInertia = config.filter_by_inertia; -// blob_det_params.minInertiaRatio = config.min_inertia_ratio; -// blob_det_params.maxInertiaRatio = config.max_inertia_ratio; -// blob_det_params.filterByConvexity = config.filter_by_convexity; -// blob_det_params.minConvexity = config.min_convexity; -// blob_det_params.maxConvexity = config.max_convexity; -// blob_det_->updateParameters(blob_det_params); - -// // Tracking Parameters -// CTracker::Params tracking_params; -// tracking_params.dt = config.dt; -// tracking_params.dist_thresh = config.dist_thresh; -// tracking_params.max_allowed_skipped_frames = config.max_allowed_skipped_frames; -// tracking_params.max_trace_length = config.max_trace_length; -// tracker_->updateParameters(tracking_params); -//} - -void CostmapToDynamicObstacles::getContour(unsigned int idx, std::vector& contour) -{ - assert(!tracker_->tracks.empty() && idx < tracker_->tracks.size()); - - contour.clear(); - - // contour [px] * costmapResolution [m/px] = contour [m] - std::vector contour2i = tracker_->tracks.at(idx)->getLastContour(); - - contour.reserve(contour2i.size()); - - Point_t costmap_origin(costmap_->getOriginX(), costmap_->getOriginY(), 0); - - for (std::size_t i = 0; i < contour2i.size(); ++i) - { - contour.push_back((Point_t(contour2i.at(i).x, contour2i.at(i).y, 0.0)*costmap_->getResolution()) - + costmap_origin); // Shift to /map - } - -} - -void CostmapToDynamicObstacles::visualize(const std::string& name, const cv::Mat& image) -{ - if (!image.empty()) - { - cv::Mat im = image.clone(); - cv::flip(im, im, 0); - cv::imshow(name, im); - cv::waitKey(1); - } -} -} diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp deleted file mode 100644 index 80ddde4..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Ctracker.cpp +++ /dev/null @@ -1,130 +0,0 @@ -// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3 -// Refer to README.md in this directory. - -#include - -// --------------------------------------------------------------------------- -// Tracker. Manage tracks. Create, remove, update. -// --------------------------------------------------------------------------- -CTracker::CTracker(const Params ¶meters) - : params(parameters), - NextTrackID(0) -{ -} -// --------------------------------------------------------------------------- -// -// --------------------------------------------------------------------------- -void CTracker::Update(const std::vector& detectedCentroid, const std::vector< std::vector >& contours) -{ - // Each contour has a centroid - assert(detectedCentroid.size() == contours.size()); - - // ----------------------------------- - // If there is no tracks yet, then every cv::Point begins its own track. - // ----------------------------------- - if (tracks.size() == 0) - { - // If no tracks yet - for (size_t i = 0; i < detectedCentroid.size(); ++i) - { - tracks.push_back( - std::unique_ptr(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++))); - } - } - - size_t N = tracks.size(); - size_t M = detectedCentroid.size(); - - assignments_t assignment; - - if (!tracks.empty()) - { - // Distance matrix of N-th Track to the M-th detectedCentroid - distMatrix_t Cost(N * M); - - // calculate distance between the blobs centroids - for (size_t i = 0; i < tracks.size(); i++) - { - for (size_t j = 0; j < detectedCentroid.size(); j++) - { - Cost[i + j * N] = tracks[i]->CalcDist(detectedCentroid[j]); - } - } - - // ----------------------------------- - // Solving assignment problem (tracks and predictions of Kalman filter) - // ----------------------------------- - AssignmentProblemSolver APS; - APS.Solve(Cost, N, M, assignment, AssignmentProblemSolver::optimal); - - // ----------------------------------- - // clean assignment from pairs with large distance - // ----------------------------------- - for (size_t i = 0; i < assignment.size(); i++) - { - if (assignment[i] != -1) - { - if (Cost[i + assignment[i] * N] > params.dist_thresh) - { - assignment[i] = -1; - tracks[i]->skipped_frames = 1; - } - } - else - { - // If track have no assigned detect, then increment skipped frames counter. - tracks[i]->skipped_frames++; - } - } - - // ----------------------------------- - // If track didn't get detects long time, remove it. - // ----------------------------------- - for (int i = 0; i < static_cast(tracks.size()); i++) - { - if ((int)tracks[i]->skipped_frames > params.max_allowed_skipped_frames) - { - tracks.erase(tracks.begin() + i); - assignment.erase(assignment.begin() + i); - i--; - } - } - } - - // ----------------------------------- - // Search for unassigned detects and start new tracks for them. - // ----------------------------------- - for (size_t i = 0; i < detectedCentroid.size(); ++i) - { - if (find(assignment.begin(), assignment.end(), i) == assignment.end()) - { - tracks.push_back(std::unique_ptr(new CTrack(detectedCentroid[i], contours[i], params.dt, NextTrackID++))); - } - } - - // Update Kalman Filters state - - for (size_t i = 0; i < assignment.size(); i++) - { - // If track updated less than one time, than filter state is not correct. - - if (assignment[i] != -1) // If we have assigned detect, then update using its coordinates, - { - tracks[i]->skipped_frames = 0; - tracks[i]->Update(detectedCentroid[assignment[i]], contours[assignment[i]], true, params.max_trace_length); - } - else // if not continue using predictions - { - tracks[i]->Update(Point_t(), std::vector(), false, params.max_trace_length); - } - } -} - -void CTracker::updateParameters(const Params ¶meters) -{ - params = parameters; -} -// --------------------------------------------------------------------------- -// -// --------------------------------------------------------------------------- -CTracker::~CTracker(void) {} diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp deleted file mode 100644 index 33ef4b9..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/HungarianAlg.cpp +++ /dev/null @@ -1,732 +0,0 @@ -// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3 -// Refer to README.md in this directory. - -#include -#include - -AssignmentProblemSolver::AssignmentProblemSolver() {} - -AssignmentProblemSolver::~AssignmentProblemSolver() {} - -track_t AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns, - std::vector& assignment, TMethod Method) -{ - assignment.resize(nOfRows, -1); - - track_t cost = 0; - - switch (Method) - { - case optimal: - assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns); - break; - - case many_forbidden_assignments: - assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns); - break; - - case without_forbidden_assignments: - assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns); - break; - } - - return cost; -} -// -------------------------------------------------------------------------- -// Computes the optimal assignment (minimum overall costs) using Munkres algorithm. -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost, - const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns) -{ - // Generate distance cv::Matrix - // and check cv::Matrix elements positiveness :) - - // Total elements number - size_t nOfElements = nOfRows * nOfColumns; - // Memory allocation - track_t* distMatrix = (track_t*)malloc(nOfElements * sizeof(track_t)); - // Pointer to last element - track_t* distMatrixEnd = distMatrix + nOfElements; - - for (size_t row = 0; row < nOfElements; row++) - { - track_t value = distMatrixIn[row]; - assert(value >= 0); - distMatrix[row] = value; - } - - // Memory allocation - bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool)); - bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool)); - bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool)); - bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool)); - bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */ - - /* preliminary steps */ - if (nOfRows <= nOfColumns) - { - for (size_t row = 0; row < nOfRows; row++) - { - /* find the smallest element in the row */ - track_t* distMatrixTemp = distMatrix + row; - track_t minValue = *distMatrixTemp; - distMatrixTemp += nOfRows; - while (distMatrixTemp < distMatrixEnd) - { - track_t value = *distMatrixTemp; - if (value < minValue) - { - minValue = value; - } - distMatrixTemp += nOfRows; - } - /* subtract the smallest element from each element of the row */ - distMatrixTemp = distMatrix + row; - while (distMatrixTemp < distMatrixEnd) - { - *distMatrixTemp -= minValue; - distMatrixTemp += nOfRows; - } - } - /* Steps 1 and 2a */ - for (size_t row = 0; row < nOfRows; row++) - { - for (size_t col = 0; col < nOfColumns; col++) - { - if (distMatrix[row + nOfRows * col] == 0) - { - if (!coveredColumns[col]) - { - starMatrix[row + nOfRows * col] = true; - coveredColumns[col] = true; - break; - } - } - } - } - } - else /* if(nOfRows > nOfColumns) */ - { - for (size_t col = 0; col < nOfColumns; col++) - { - /* find the smallest element in the column */ - track_t* distMatrixTemp = distMatrix + nOfRows * col; - track_t* columnEnd = distMatrixTemp + nOfRows; - track_t minValue = *distMatrixTemp++; - while (distMatrixTemp < columnEnd) - { - track_t value = *distMatrixTemp++; - if (value < minValue) - { - minValue = value; - } - } - /* subtract the smallest element from each element of the column */ - distMatrixTemp = distMatrix + nOfRows * col; - while (distMatrixTemp < columnEnd) - { - *distMatrixTemp++ -= minValue; - } - } - /* Steps 1 and 2a */ - for (size_t col = 0; col < nOfColumns; col++) - { - for (size_t row = 0; row < nOfRows; row++) - { - if (distMatrix[row + nOfRows * col] == 0) - { - if (!coveredRows[row]) - { - starMatrix[row + nOfRows * col] = true; - coveredColumns[col] = true; - coveredRows[row] = true; - break; - } - } - } - } - - for (size_t row = 0; row < nOfRows; row++) - { - coveredRows[row] = false; - } - } - /* move to step 2b */ - step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, - nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns); - /* compute cost and remove invalid assignments */ - computeassignmentcost(assignment, cost, distMatrixIn, nOfRows); - /* free allocated memory */ - free(distMatrix); - free(coveredColumns); - free(coveredRows); - free(starMatrix); - free(primeMatrix); - free(newStarMatrix); - return; -} -// -------------------------------------------------------------------------- -// -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool* starMatrix, size_t nOfRows, - size_t nOfColumns) -{ - for (size_t row = 0; row < nOfRows; row++) - { - for (size_t col = 0; col < nOfColumns; col++) - { - if (starMatrix[row + nOfRows * col]) - { - assignment[row] = static_cast(col); - break; - } - } - } -} -// -------------------------------------------------------------------------- -// -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost, - const distMatrix_t& distMatrixIn, size_t nOfRows) -{ - for (size_t row = 0; row < nOfRows; row++) - { - const int col = assignment[row]; - if (col >= 0) - { - cost += distMatrixIn[row + nOfRows * col]; - } - } -} - -// -------------------------------------------------------------------------- -// -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, - bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, - size_t nOfRows, size_t nOfColumns, size_t minDim) -{ - bool* starMatrixTemp, *columnEnd; - /* cover every column containing a starred zero */ - for (size_t col = 0; col < nOfColumns; col++) - { - starMatrixTemp = starMatrix + nOfRows * col; - columnEnd = starMatrixTemp + nOfRows; - while (starMatrixTemp < columnEnd) - { - if (*starMatrixTemp++) - { - coveredColumns[col] = true; - break; - } - } - } - /* move to step 3 */ - step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, - nOfColumns, minDim); -} - -// -------------------------------------------------------------------------- -// -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, - bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, - size_t nOfRows, size_t nOfColumns, size_t minDim) -{ - /* count covered columns */ - size_t nOfCoveredColumns = 0; - for (size_t col = 0; col < nOfColumns; col++) - { - if (coveredColumns[col]) - { - nOfCoveredColumns++; - } - } - if (nOfCoveredColumns == minDim) - { - /* algorithm finished */ - buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns); - } - else - { - /* move to step 3 */ - step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, - nOfColumns, minDim); - } -} - -// -------------------------------------------------------------------------- -// -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::step3(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, - bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, - size_t nOfRows, size_t nOfColumns, size_t minDim) -{ - bool zerosFound = true; - while (zerosFound) - { - zerosFound = false; - for (size_t col = 0; col < nOfColumns; col++) - { - if (!coveredColumns[col]) - { - for (size_t row = 0; row < nOfRows; row++) - { - if ((!coveredRows[row]) && (distMatrix[row + nOfRows * col] == 0)) - { - /* prime zero */ - primeMatrix[row + nOfRows * col] = true; - /* find starred zero in current row */ - size_t starCol = 0; - for (; starCol < nOfColumns; starCol++) - { - if (starMatrix[row + nOfRows * starCol]) - { - break; - } - } - if (starCol == nOfColumns) /* no starred zero found */ - { - /* move to step 4 */ - step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, - nOfRows, nOfColumns, minDim, row, col); - return; - } - else - { - coveredRows[row] = true; - coveredColumns[starCol] = false; - zerosFound = true; - break; - } - } - } - } - } - } - /* move to step 5 */ - step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, - nOfColumns, minDim); -} - -// -------------------------------------------------------------------------- -// -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::step4(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, - bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, - size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col) -{ - const size_t nOfElements = nOfRows * nOfColumns; - /* generate temporary copy of starMatrix */ - for (size_t n = 0; n < nOfElements; n++) - { - newStarMatrix[n] = starMatrix[n]; - } - /* star current zero */ - newStarMatrix[row + nOfRows * col] = true; - /* find starred zero in current column */ - size_t starCol = col; - size_t starRow = 0; - for (; starRow < nOfRows; starRow++) - { - if (starMatrix[starRow + nOfRows * starCol]) - { - break; - } - } - while (starRow < nOfRows) - { - /* unstar the starred zero */ - newStarMatrix[starRow + nOfRows * starCol] = false; - /* find primed zero in current row */ - size_t primeRow = starRow; - size_t primeCol = 0; - for (; primeCol < nOfColumns; primeCol++) - { - if (primeMatrix[primeRow + nOfRows * primeCol]) - { - break; - } - } - /* star the primed zero */ - newStarMatrix[primeRow + nOfRows * primeCol] = true; - /* find starred zero in current column */ - starCol = primeCol; - for (starRow = 0; starRow < nOfRows; starRow++) - { - if (starMatrix[starRow + nOfRows * starCol]) - { - break; - } - } - } - /* use temporary copy as new starMatrix */ - /* delete all primes, uncover all rows */ - for (size_t n = 0; n < nOfElements; n++) - { - primeMatrix[n] = false; - starMatrix[n] = newStarMatrix[n]; - } - for (size_t n = 0; n < nOfRows; n++) - { - coveredRows[n] = false; - } - /* move to step 2a */ - step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, - nOfColumns, minDim); -} - -// -------------------------------------------------------------------------- -// -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::step5(assignments_t& assignment, track_t* distMatrix, bool* starMatrix, - bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, - size_t nOfRows, size_t nOfColumns, size_t minDim) -{ - /* find smallest uncovered element h */ - float h = std::numeric_limits::max(); - for (size_t row = 0; row < nOfRows; row++) - { - if (!coveredRows[row]) - { - for (size_t col = 0; col < nOfColumns; col++) - { - if (!coveredColumns[col]) - { - const float value = distMatrix[row + nOfRows * col]; - if (value < h) - { - h = value; - } - } - } - } - } - /* add h to each covered row */ - for (size_t row = 0; row < nOfRows; row++) - { - if (coveredRows[row]) - { - for (size_t col = 0; col < nOfColumns; col++) - { - distMatrix[row + nOfRows * col] += h; - } - } - } - /* subtract h from each uncovered column */ - for (size_t col = 0; col < nOfColumns; col++) - { - if (!coveredColumns[col]) - { - for (size_t row = 0; row < nOfRows; row++) - { - distMatrix[row + nOfRows * col] -= h; - } - } - } - /* move to step 3 */ - step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, - nOfColumns, minDim); -} - -// -------------------------------------------------------------------------- -// Computes a suboptimal solution. Good for cases without forbidden assignments. -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost, - const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns) -{ - /* make working copy of distance Matrix */ - const size_t nOfElements = nOfRows * nOfColumns; - float* distMatrix = (float*)malloc(nOfElements * sizeof(float)); - for (size_t n = 0; n < nOfElements; n++) - { - distMatrix[n] = distMatrixIn[n]; - } - - /* recursively search for the minimum element and do the assignment */ - for (;;) - { - /* find minimum distance observation-to-track pair */ - float minValue = std::numeric_limits::max(); - size_t tmpRow = 0; - size_t tmpCol = 0; - for (size_t row = 0; row < nOfRows; row++) - { - for (size_t col = 0; col < nOfColumns; col++) - { - const float value = distMatrix[row + nOfRows * col]; - if (value != std::numeric_limits::max() && (value < minValue)) - { - minValue = value; - tmpRow = row; - tmpCol = col; - } - } - } - - if (minValue != std::numeric_limits::max()) - { - assignment[tmpRow] = static_cast(tmpCol); - cost += minValue; - for (size_t n = 0; n < nOfRows; n++) - { - distMatrix[n + nOfRows * tmpCol] = std::numeric_limits::max(); - } - for (size_t n = 0; n < nOfColumns; n++) - { - distMatrix[tmpRow + nOfRows * n] = std::numeric_limits::max(); - } - } - else - { - break; - } - } - - free(distMatrix); -} -// -------------------------------------------------------------------------- -// Computes a suboptimal solution. Good for cases with many forbidden assignments. -// -------------------------------------------------------------------------- -void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost, - const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns) -{ - /* make working copy of distance Matrix */ - const size_t nOfElements = nOfRows * nOfColumns; - float* distMatrix = (float*)malloc(nOfElements * sizeof(float)); - for (size_t n = 0; n < nOfElements; n++) - { - distMatrix[n] = distMatrixIn[n]; - } - - /* allocate memory */ - int* nOfValidObservations = (int*)calloc(nOfRows, sizeof(int)); - int* nOfValidTracks = (int*)calloc(nOfColumns, sizeof(int)); - - /* compute number of validations */ - bool infiniteValueFound = false; - bool finiteValueFound = false; - for (size_t row = 0; row < nOfRows; row++) - { - for (size_t col = 0; col < nOfColumns; col++) - { - if (distMatrix[row + nOfRows * col] != std::numeric_limits::max()) - { - nOfValidTracks[col] += 1; - nOfValidObservations[row] += 1; - finiteValueFound = true; - } - else - { - infiniteValueFound = true; - } - } - } - - if (infiniteValueFound) - { - if (!finiteValueFound) - { - free(nOfValidTracks); - free(nOfValidObservations); - free(distMatrix); - return; - } - bool repeatSteps = true; - - while (repeatSteps) - { - repeatSteps = false; - - /* step 1: reject assignments of multiply validated tracks to singly validated observations */ - for (size_t col = 0; col < nOfColumns; col++) - { - bool singleValidationFound = false; - for (size_t row = 0; row < nOfRows; row++) - { - if (distMatrix[row + nOfRows * col] != std::numeric_limits::max() && - (nOfValidObservations[row] == 1)) - { - singleValidationFound = true; - break; - } - - if (singleValidationFound) - { - for (size_t row = 0; row < nOfRows; row++) - if ((nOfValidObservations[row] > 1) && - distMatrix[row + nOfRows * col] != std::numeric_limits::max()) - { - distMatrix[row + nOfRows * col] = std::numeric_limits::max(); - nOfValidObservations[row] -= 1; - nOfValidTracks[col] -= 1; - repeatSteps = true; - } - } - } - } - - /* step 2: reject assignments of multiply validated observations to singly validated tracks */ - if (nOfColumns > 1) - { - for (size_t row = 0; row < nOfRows; row++) - { - bool singleValidationFound = false; - for (size_t col = 0; col < nOfColumns; col++) - { - if (distMatrix[row + nOfRows * col] != std::numeric_limits::max() && (nOfValidTracks[col] == 1)) - { - singleValidationFound = true; - break; - } - } - - if (singleValidationFound) - { - for (size_t col = 0; col < nOfColumns; col++) - { - if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows * col] != std::numeric_limits::max()) - { - distMatrix[row + nOfRows * col] = std::numeric_limits::max(); - nOfValidObservations[row] -= 1; - nOfValidTracks[col] -= 1; - repeatSteps = true; - } - } - } - } - } - } /* while(repeatSteps) */ - - /* for each multiply validated track that validates only with singly validated */ - /* observations, choose the observation with minimum distance */ - for (size_t row = 0; row < nOfRows; row++) - { - if (nOfValidObservations[row] > 1) - { - bool allSinglyValidated = true; - float minValue = std::numeric_limits::max(); - size_t tmpCol = 0; - for (size_t col = 0; col < nOfColumns; col++) - { - const float value = distMatrix[row + nOfRows * col]; - if (value != std::numeric_limits::max()) - { - if (nOfValidTracks[col] > 1) - { - allSinglyValidated = false; - break; - } - else if ((nOfValidTracks[col] == 1) && (value < minValue)) - { - tmpCol = col; - minValue = value; - } - } - } - - if (allSinglyValidated) - { - assignment[row] = static_cast(tmpCol); - cost += minValue; - for (size_t n = 0; n < nOfRows; n++) - { - distMatrix[n + nOfRows * tmpCol] = std::numeric_limits::max(); - } - for (size_t n = 0; n < nOfColumns; n++) - { - distMatrix[row + nOfRows * n] = std::numeric_limits::max(); - } - } - } - } - - // for each multiply validated observation that validates only with singly validated track, choose the track with - // minimum distance - for (size_t col = 0; col < nOfColumns; col++) - { - if (nOfValidTracks[col] > 1) - { - bool allSinglyValidated = true; - float minValue = std::numeric_limits::max(); - size_t tmpRow = 0; - for (size_t row = 0; row < nOfRows; row++) - { - const float value = distMatrix[row + nOfRows * col]; - if (value != std::numeric_limits::max()) - { - if (nOfValidObservations[row] > 1) - { - allSinglyValidated = false; - break; - } - else if ((nOfValidObservations[row] == 1) && (value < minValue)) - { - tmpRow = row; - minValue = value; - } - } - } - - if (allSinglyValidated) - { - assignment[tmpRow] = static_cast(col); - cost += minValue; - for (size_t n = 0; n < nOfRows; n++) - { - distMatrix[n + nOfRows * col] = std::numeric_limits::max(); - } - for (size_t n = 0; n < nOfColumns; n++) - { - distMatrix[tmpRow + nOfRows * n] = std::numeric_limits::max(); - } - } - } - } - } /* if(infiniteValueFound) */ - - /* now, recursively search for the minimum element and do the assignment */ - for (;;) - { - /* find minimum distance observation-to-track pair */ - float minValue = std::numeric_limits::max(); - size_t tmpRow = 0; - size_t tmpCol = 0; - for (size_t row = 0; row < nOfRows; row++) - { - for (size_t col = 0; col < nOfColumns; col++) - { - const float value = distMatrix[row + nOfRows * col]; - if (value != std::numeric_limits::max() && (value < minValue)) - { - minValue = value; - tmpRow = row; - tmpCol = col; - } - } - } - - if (minValue != std::numeric_limits::max()) - { - assignment[tmpRow] = static_cast(tmpCol); - cost += minValue; - for (size_t n = 0; n < nOfRows; n++) - { - distMatrix[n + nOfRows * tmpCol] = std::numeric_limits::max(); - } - for (size_t n = 0; n < nOfColumns; n++) - { - distMatrix[tmpRow + nOfRows * n] = std::numeric_limits::max(); - } - } - else - { - break; - } - } - - /* free allocated memory */ - free(nOfValidObservations); - free(nOfValidTracks); - free(distMatrix); -} diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp deleted file mode 100644 index 7bf8e71..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/Kalman.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Based on https://github.com/Smorodov/Multitarget-tracker/tree/master/Tracker, GPLv3 -// Refer to README.md in this directory. - -#include -#include "opencv2/opencv.hpp" -#include -#include - -//--------------------------------------------------------------------------- -//--------------------------------------------------------------------------- -TKalmanFilter::TKalmanFilter(Point_t pt, track_t deltatime) -{ - // time increment (lower values makes target more "massive") - dt = deltatime; - - // 6 state variables [x y z xdot ydot zdot], 3 measurements [x y z] - kalman = new cv::KalmanFilter(6, 3, 0); - // Transition cv::Matrix - kalman->transitionMatrix = (cv::Mat_(6, 6) << - 1, 0, 0, dt, 0, 0, - 0, 1, 0, 0, dt, 0, - 0, 0, 1, 0, 0, dt, - 0, 0, 0, 1, 0, 0, - 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 0, 1); - // init... - LastPosition = pt; - kalman->statePre.at(0) = pt.x; - kalman->statePre.at(1) = pt.y; - kalman->statePre.at(2) = pt.z; - - kalman->statePre.at(3) = 0; - kalman->statePre.at(4) = 0; - kalman->statePre.at(5) = 0; - - kalman->statePost.at(0) = pt.x; - kalman->statePost.at(1) = pt.y; - kalman->statePost.at(2) = pt.z; - - // Nur x, y und z Koordinaten messbar! - kalman->measurementMatrix = (cv::Mat_(3, 6) << - 1, 0, 0, 0, 0, 0, - 0, 1, 0, 0, 0, 0, - 0, 0, 1, 0, 0, 0); - - float sigma = 0.5; // Assume standard deviation for acceleration, todo: dynamic reconfigure - float c1 = pow(dt,4.0)/4.0; - float c2 = pow(dt,2.0); - float c3 = pow(dt,3.0)/2.0; - kalman->processNoiseCov = sigma*sigma*(cv::Mat_(6, 6) << - c1, 0, 0, c3, 0, 0, - 0, c1, 0, 0, c3, 0, - 0, 0, c1, 0, 0, c3, - c3, 0, 0, c2, 0, 0, - 0, c3, 0, 0, c2, 0, - 0, 0, c3, 0, 0, c2); - - cv::setIdentity(kalman->measurementNoiseCov, cv::Scalar::all(5e-2)); - - cv::setIdentity(kalman->errorCovPost, cv::Scalar::all(1000000)); -} -//--------------------------------------------------------------------------- -TKalmanFilter::~TKalmanFilter() { delete kalman; } - -//--------------------------------------------------------------------------- -void TKalmanFilter::Prediction() -{ - cv::Mat prediction = kalman->predict(); - LastPosition = Point_t(prediction.at(0), prediction.at(1), prediction.at(2)); - LastVelocity = Point_t(prediction.at(3), prediction.at(4), prediction.at(5)); -} - -//--------------------------------------------------------------------------- -Point_t TKalmanFilter::Update(Point_t p, bool DataCorrect) -{ - cv::Mat measurement(3, 1, Mat_t(1)); - if (!DataCorrect) - { - measurement.at(0) = LastPosition.x; // update using prediction - measurement.at(1) = LastPosition.y; - measurement.at(2) = LastPosition.z; - } - else - { - measurement.at(0) = p.x; // update using measurements - measurement.at(1) = p.y; - measurement.at(2) = p.z; - } - // Correction - cv::Mat estimated = kalman->correct(measurement); - LastPosition.x = estimated.at(0); // update using measurements - LastPosition.y = estimated.at(1); - LastPosition.z = estimated.at(2); - LastVelocity.x = estimated.at(3); - LastVelocity.y = estimated.at(4); - LastVelocity.z = estimated.at(5); - return LastPosition; -} -//--------------------------------------------------------------------------- diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md deleted file mode 100644 index 3fb4614..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_dynamic_obstacles/multitarget_tracker/README.md +++ /dev/null @@ -1,13 +0,0 @@ -Multitarget Tracker -=================== - -This code is mostly copied from the (MultitargetTracker)[https://github.com/Smorodov/Multitarget-tracker]. -It is utilized for the *CostmapToDynamicObstacles* plugin. - -The *MultitargetTracker* is licensed under (GNU GPLv3)[https://www.gnu.org/licenses/gpl-3.0.txt]. -The package itself depends on other third party packages with different licenses (refer to the package repository). - -TODO: Include the whole package as external CMake project. - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_convex_hull.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_convex_hull.cpp deleted file mode 100644 index 4177cd2..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_convex_hull.cpp +++ /dev/null @@ -1,303 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann, Otniel Rinaldo - *********************************************************************/ - -#include -#include -#include - -PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::BaseCostmapToPolygons) - -namespace costmap_converter -{ - -CostmapToLinesDBSMCCH::CostmapToLinesDBSMCCH() : CostmapToPolygonsDBSMCCH() -{ -// dynamic_recfg_ = NULL; -} - -CostmapToLinesDBSMCCH::~CostmapToLinesDBSMCCH() -{ -// if (dynamic_recfg_ != NULL) -// delete dynamic_recfg_; -} - -void CostmapToLinesDBSMCCH::initialize(rclcpp::Node::SharedPtr nh) -{ - BaseCostmapToPolygons::initialize(nh); - - // DB SCAN - parameter_.max_distance_ = 0.4; - nh->get_parameter_or("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_); - - parameter_.min_pts_ = 2; - nh->get_parameter_or("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_); - - parameter_.max_pts_ = 30; - nh->get_parameter_or("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_); - - // convex hull - parameter_.min_keypoint_separation_ = 0.1; - nh->get_parameter_or("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_); - - parameter_buffered_ = parameter_; - - // Line extraction - support_pts_max_dist_ = 0.3; - nh->get_parameter_or("support_pts_max_dist", support_pts_max_dist_, support_pts_max_dist_); - - support_pts_max_dist_inbetween_ = 1.0; - nh->get_parameter_or("support_pts_max_dist_inbetween", support_pts_max_dist_inbetween_, support_pts_max_dist_inbetween_); - - min_support_pts_ = 2; - nh->get_parameter_or("min_support_pts", min_support_pts_, min_support_pts_); - - // setup dynamic reconfigure -// dynamic_recfg_ = new dynamic_reconfigure::Server(nh); -// dynamic_reconfigure::Server::CallbackType cb = boost::bind(&CostmapToLinesDBSMCCH::reconfigureCB, this, _1, _2); -// dynamic_recfg_->setCallback(cb); - - // deprecated - rclcpp::Parameter dummy; - if (nh->get_parameter("support_pts_min_dist_", dummy) || nh->get_parameter("support_pts_min_dist", dummy)) - RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'support_pts_min_dist' is deprecated and not included anymore."); - if (nh->get_parameter("min_support_pts_", dummy)) - RCLCPP_WARN(nh->get_logger(), "CostmapToLinesDBSMCCH: Parameter 'min_support_pts_' is not found. Remove the underscore."); -} - -void CostmapToLinesDBSMCCH::compute() -{ - std::vector< std::vector > clusters; - dbScan(clusters); - - // Create new polygon container - PolygonContainerPtr polygons(new std::vector()); - - - // add convex hulls to polygon container - for (size_t i = 1; i push_back( geometry_msgs::msg::Polygon() ); - convertPointToPolygon(clusters.front()[i], polygons->back()); - } - } - - // replace shared polygon container - updatePolygonContainer(polygons); -} - -typedef CostmapToLinesDBSMCCH CL; -bool sort_keypoint_x(const std::size_t& i, const std::size_t& j, const std::vector& cluster) -{ return (cluster[i].x& cluster) -{ return (cluster[i].y& cluster, const geometry_msgs::msg::Polygon& polygon, - std::back_insert_iterator< std::vector > lines) -{ - if (polygon.points.empty()) - return; - - if (polygon.points.size() < 2) - { - lines = polygon; // our polygon is just a point, push back onto the output sequence - return; - } - int n = (int)polygon.points.size(); - - for (int i=1; ix - vertex1->x; - double dy = vertex2->y - vertex1->y; -// double norm = std::sqrt(dx*dx + dy*dy); -// dx /= norm; -// dy /= norm; -// for (int j=i; j<(int)polygon.points.size() - 2; ++j) -// { -// const geometry_msgs::msg::Point32* vertex_jp2 = &polygon.points[j+2]; -// double dx2 = vertex_jp2->x - vertex2->x; -// double dy2 = vertex_jp2->y - vertex2->y; -// double norm2 = std::sqrt(dx2*dx2 + dy2*dy2); -// dx2 /= norm2; -// dy2 /= norm2; -// if (std::abs(dx*dx2 + dy*dy2) < 0.05) //~3 degree -// { -// vertex2 = &polygon.points[j+2]; -// i = j; // DO NOT USE "i" afterwards -// } -// else break; -// } - - //Search support points - std::vector support_points; // store indices of cluster - - for (std::size_t k = 0; k < cluster.size(); ++k) - { - bool is_inbetween = false; - double dist_line_to_point = computeDistanceToLineSegment( cluster[k], *vertex1, *vertex2, &is_inbetween ); - - if(is_inbetween && dist_line_to_point <= support_pts_max_dist_) - { - support_points.push_back(k); - continue; - } - } - - // now check if the inlier models a line by checking the minimum distance between all support points (avoid lines over gaps) - // and by checking if the minium number of points is reached // deactivate by setting support_pts_max_dist_inbetween_==0 - bool is_line=true; - if (support_pts_max_dist_inbetween_!=0) - { - if ((int)support_points.size() >= min_support_pts_ + 2) // +2 since start and goal are included - { - // sort points - if (std::abs(dx) >= std::abs(dy)) - std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_x, std::placeholders::_1, std::placeholders::_2, std::cref(cluster))); - else - std::sort(support_points.begin(), support_points.end(), std::bind(sort_keypoint_y, std::placeholders::_1, std::placeholders::_2, std::cref(cluster))); - - // now calculate distances - for (int k = 1; k < int(support_points.size()); ++k) - { - double dist_x = cluster[support_points[k]].x - cluster[support_points[k-1]].x; - double dist_y = cluster[support_points[k]].y - cluster[support_points[k-1]].y; - double dist = std::sqrt( dist_x*dist_x + dist_y*dist_y); - if (dist > support_pts_max_dist_inbetween_) - { - is_line = false; - break; - } - } - - } - else - is_line = false; - } - - if (is_line) - { - // line found: - geometry_msgs::msg::Polygon line; - line.points.push_back(*vertex1); - line.points.push_back(*vertex2); - lines = line; // back insertion - - // remove inlier from list - // iterate in reverse order, otherwise indices are not valid after erasing elements - std::vector::reverse_iterator support_it = support_points.rbegin(); - for (; support_it != support_points.rend(); ++support_it) - { - cluster.erase(cluster.begin() + *support_it); - } - } - else - { - // remove goal, since it will be added in the subsequent iteration - //support_points.pop_back(); - // old: -// // add vertex 1 as single point -// geometry_msgs::msg::Polygon point; -// point.points.push_back(*vertex1); -// lines = point; // back insertion - - // add complete inlier set as points -// for (int k = 0; k < int(support_points.size()); ++k) -// { -// geometry_msgs::msg::Polygon polygon; -// convertPointToPolygon(cluster[support_points[k]], polygon); -// lines = polygon; // back insertion -// } - - // remove inlier from list and add them as point obstacles - // iterate in reverse order, otherwise indices are not valid after erasing elements - std::vector::reverse_iterator support_it = support_points.rbegin(); - for (; support_it != support_points.rend(); ++support_it) - { - geometry_msgs::msg::Polygon polygon; - convertPointToPolygon(cluster[*support_it], polygon); - lines = polygon; // back insertion - - cluster.erase(cluster.begin() + *support_it); - } - } - } - - // add all remaining cluster points, that do not belong to a line - for (int i=0; i<(int)cluster.size();++i) - { - geometry_msgs::msg::Polygon polygon; - convertPointToPolygon(cluster[i], polygon); - lines = polygon; // back insertion - } - -} - -//void CostmapToLinesDBSMCCH::reconfigureCB(CostmapToLinesDBSMCCHConfig& config, uint32_t level) -//{ -// boost::mutex::scoped_lock lock(parameter_mutex_); -// parameter_buffered_.max_distance_ = config.cluster_max_distance; -// parameter_buffered_.min_pts_ = config.cluster_min_pts; -// parameter_buffered_.max_pts_ = config.cluster_max_pts; -// parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts; -// support_pts_max_dist_ = config.support_pts_max_dist; -// support_pts_max_dist_inbetween_ = config.support_pts_max_dist_inbetween; -// min_support_pts_ = config.min_support_pts; -//} - - - -}//end namespace costmap_converter - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_ransac.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_ransac.cpp deleted file mode 100644 index 38e3d06..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_lines_ransac.cpp +++ /dev/null @@ -1,361 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include -#include - -PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::BaseCostmapToPolygons) - -namespace costmap_converter -{ - -CostmapToLinesDBSRANSAC::CostmapToLinesDBSRANSAC() : CostmapToPolygonsDBSMCCH() -{ -// dynamic_recfg_ = NULL; -} - -CostmapToLinesDBSRANSAC::~CostmapToLinesDBSRANSAC() -{ -// if (dynamic_recfg_ != NULL) -// delete dynamic_recfg_; -} - -void CostmapToLinesDBSRANSAC::initialize(rclcpp::Node::SharedPtr nh) -{ - BaseCostmapToPolygons::initialize(nh); - - // DB SCAN - parameter_.max_distance_ = 0.4; - nh->get_parameter_or("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_); - - parameter_.min_pts_ = 2; - nh->get_parameter_or("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_); - - parameter_.max_pts_ = 30; - nh->get_parameter_or("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_); - - // convex hull (only necessary if outlier filtering is enabled) - parameter_.min_keypoint_separation_ = 0.1; - nh->get_parameter_or("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_); - - parameter_buffered_ = parameter_; - - // ransac - ransac_inlier_distance_ = 0.2; - nh->get_parameter_or("ransac_inlier_distance", ransac_inlier_distance_, ransac_inlier_distance_); - - ransac_min_inliers_ = 10; - nh->get_parameter_or("ransac_min_inliers", ransac_min_inliers_, ransac_min_inliers_); - - ransac_no_iterations_ = 2000; - nh->get_parameter_or("ransac_no_iterations", ransac_no_iterations_, ransac_no_iterations_); - - ransac_remainig_outliers_ = 3; - nh->get_parameter_or("ransac_remainig_outliers", ransac_remainig_outliers_, ransac_remainig_outliers_); - - ransac_convert_outlier_pts_ = true; - nh->get_parameter_or("ransac_convert_outlier_pts", ransac_convert_outlier_pts_, ransac_convert_outlier_pts_); - - ransac_filter_remaining_outlier_pts_ = false; - nh->get_parameter_or("ransac_filter_remaining_outlier_pts", ransac_filter_remaining_outlier_pts_, ransac_filter_remaining_outlier_pts_); - - // setup dynamic reconfigure -// dynamic_recfg_ = new dynamic_reconfigure::Server(nh); -// dynamic_reconfigure::Server::CallbackType cb = boost::bind(&CostmapToLinesDBSRANSAC::reconfigureCB, this, _1, _2); -// dynamic_recfg_->setCallback(cb); -} - -void CostmapToLinesDBSRANSAC::compute() -{ - std::vector< std::vector > clusters; - dbScan(clusters); - - // Create new polygon container - PolygonContainerPtr polygons(new std::vector()); - - - // fit lines using ransac for each cluster - for (size_t i = 1; i ransac_remainig_outliers_) - { -// std::vector inliers; - std::vector outliers; - std::pair model; - if (!lineRansac(clusters[i], ransac_inlier_distance_, ransac_no_iterations_, ransac_min_inliers_, model, NULL, &outliers ) ) - break; - - // add to polygon container - geometry_msgs::msg::Polygon line; - line.points.resize(2); - model.first.toPointMsg(line.points.front()); - model.second.toPointMsg(line.points.back()); - polygons->push_back(line); - - clusters[i] = outliers; - } - // create point polygons for remaining outliers - if (ransac_convert_outlier_pts_) - { - if (ransac_filter_remaining_outlier_pts_) - { - // take edge points of a convex polygon - // these points define a cluster and since all lines are extracted, - // we remove points from the interior... - geometry_msgs::msg::Polygon polygon; - convexHull2(clusters[i], polygon); - for (int j=0; j < (int)polygon.points.size(); ++j) - { - polygons->push_back(geometry_msgs::msg::Polygon()); - convertPointToPolygon(polygon.points[j], polygons->back()); - } - } - else - { - for (int j = 0; j < (int)clusters[i].size(); ++j) - { - polygons->push_back(geometry_msgs::msg::Polygon()); - convertPointToPolygon(clusters[i][j], polygons->back()); - } - } - - } - - - } - - // add our non-cluster points to the polygon container (as single points) - if (!clusters.empty()) - { - for (size_t i=0; i < clusters.front().size(); ++i) - { - polygons->push_back( geometry_msgs::msg::Polygon() ); - convertPointToPolygon(clusters.front()[i], polygons->back()); - } - } - - // replace shared polygon container - updatePolygonContainer(polygons); -} - - -bool CostmapToLinesDBSRANSAC::lineRansac(const std::vector& data, double inlier_distance, int no_iterations, - int min_inliers, std::pair& best_model, - std::vector* inliers, std::vector* outliers) -{ - if ((int)data.size() < 2 || (int)data.size() < min_inliers) - { - return false; - } - - std::uniform_int_distribution<> distribution(0, data.size()-1); - - std::pair best_model_idx; - int best_no_inliers = -1; - - - for (int i=0; i < no_iterations; ++i) - { - // choose random points to define a line candidate - int start_idx = distribution(rnd_generator_); - int end_idx = start_idx; - while (end_idx == start_idx) - end_idx = distribution(rnd_generator_); - - - // compute inliers - int no_inliers = 0; - for (int j=0; j<(int)data.size(); ++j) - { - if ( isInlier(data[j], data[start_idx], data[end_idx], inlier_distance) ) - ++no_inliers; - } - - if (no_inliers > best_no_inliers) - { - best_no_inliers = no_inliers; - best_model_idx.first = start_idx; - best_model_idx.second = end_idx; - } - } - - best_model.first = data[best_model_idx.first]; - best_model.second = data[best_model_idx.second]; - - if (best_no_inliers < min_inliers) - return false; - - // Now repeat the calculation for the best model in order to obtain the inliers and outliers set - // This might be faster if no_iterations is large, but TEST - if (inliers || outliers) - { - if (inliers) - inliers->clear(); - if (outliers) - outliers->clear(); - -// int no_inliers = 0; - for (int i=0; i<(int)data.size(); ++i) - { - if ( isInlier( data[i], best_model.first, best_model.second, inlier_distance ) ) - { - if (inliers) - inliers->push_back( data[i] ); - } - else - { - if (outliers) - outliers->push_back( data[i] ); - } - } - } - - return true; -} - -bool CostmapToLinesDBSRANSAC::linearRegression(const std::vector& data, double& slope, double& intercept, - double* mean_x_out, double* mean_y_out) -{ - if (data.size() < 2) - { - RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: at least 2 data points required for linear regression"); - return false; - } - - double mean_x = 0; - double mean_y = 0; - for (int i=0; i<(int)data.size(); ++i) - { - mean_x += data[i].x; - mean_y += data[i].y; - } - mean_x /= double(data.size()); - mean_y /= double(data.size()); - - if (mean_x_out) - *mean_x_out = mean_x; - - if (mean_y_out) - *mean_y_out = mean_y; - - double numerator = 0.0; - double denominator = 0.0; - - for(int i=0; i<(int)data.size(); ++i) - { - double dx = data[i].x - mean_x; - numerator += dx * (data[i].y - mean_y); - denominator += dx*dx; - } - - if (denominator == 0) - { - RCLCPP_ERROR(getLogger(), "CostmapToLinesDBSRANSAC: linear regression failed, denominator 0"); - return false; - } - else - slope = numerator / denominator; - - intercept = mean_y - slope * mean_x; - return true; -} - - -//void CostmapToLinesDBSRANSAC::reconfigureCB(CostmapToLinesDBSRANSACConfig& config, uint32_t level) -//{ -// boost::mutex::scoped_lock lock(parameter_mutex_); -// parameter_buffered_.max_distance_ = config.cluster_max_distance; -// parameter_buffered_.min_pts_ = config.cluster_min_pts; -// parameter_buffered_.max_pts_ = config.cluster_max_pts; -// parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts; -// ransac_inlier_distance_ = config.ransac_inlier_distance; -// ransac_min_inliers_ = config.ransac_min_inliers; -// ransac_no_iterations_ = config.ransac_no_iterations; -// ransac_remainig_outliers_ = config.ransac_remainig_outliers; -// ransac_convert_outlier_pts_ = config.ransac_convert_outlier_pts; -// ransac_filter_remaining_outlier_pts_ = config.ransac_filter_remaining_outlier_pts; -//} - - -/* -void CostmapToLinesDBSRANSAC::adjustLineLength(const std::vector& data, const KeyPoint& linept1, const KeyPoint& linept2, - KeyPoint& line_start, KeyPoint& line_end) -{ - line_start = linept1; - line_end = linept2; - - // infinite line is defined by linept1 and linept2 - double dir_x = line_end.x - line_start.x; - double dir_y = line_end.y - line_start.y; - double norm = std::sqrt(dir_x*dir_x + dir_y*dir_y); - dir_x /= norm; - dir_y /= norm; - - // project data onto the line and check if the distance is increased in both directions - for (int i=0; i < (int) data.size(); ++i) - { - double dx = data[i].x - line_start.x; - double dy = data[i].y - line_start.y; - // check scalar product at start - double extension = dx*dir_x + dy*dir_y; - if (extension<0) - { - line_start.x -= dir_x*extension; - line_start.y -= dir_y*extension; - } - else - { - dx = data[i].x - line_end.x; - dy = data[i].y - line_end.y; - // check scalar product at end - double extension = dx*dir_x + dy*dir_y; - if (extension>0) - { - line_end.x += dir_x*extension; - line_end.y += dir_y*extension; - } - } - } - -}*/ - - -}//end namespace costmap_converter - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons.cpp deleted file mode 100644 index 24aaafc..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons.cpp +++ /dev/null @@ -1,514 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann, Otniel Rinaldo - *********************************************************************/ - -#include -#include -#include - -PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::BaseCostmapToPolygons) - -namespace -{ - -/** - * @brief Douglas-Peucker Algorithm for fitting lines into ordered set of points - * - * Douglas-Peucker Algorithm, see https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm - * - * @param begin iterator pointing to the begin of the range of points - * @param end interator pointing to the end of the range of points - * @param epsilon distance criteria for removing points if it is closer to the line segment than this - * @param result the simplified polygon - */ -std::vector douglasPeucker(std::vector::iterator begin, - std::vector::iterator end, double epsilon) -{ - if (std::distance(begin, end) <= 2) - { - return std::vector(begin, end); - } - - // Find the point with the maximum distance from the line [begin, end) - double dmax = std::numeric_limits::lowest(); - std::vector::iterator max_dist_it; - std::vector::iterator last = std::prev(end); - for (auto it = std::next(begin); it != last; ++it) - { - double d = costmap_converter::computeSquaredDistanceToLineSegment(*it, *begin, *last); - if (d > dmax) - { - max_dist_it = it; - dmax = d; - } - } - - if (dmax < epsilon * epsilon) - { // termination criterion reached, line is good enough - std::vector result; - result.push_back(*begin); - result.push_back(*last); - return result; - } - - // Recursive calls for the two splitted parts - auto firstLineSimplified = douglasPeucker(begin, std::next(max_dist_it), epsilon); - auto secondLineSimplified = douglasPeucker(max_dist_it, end, epsilon); - - // Combine the two lines into one line and return the merged line. - // Note that we have to skip the first point of the second line, as it is duplicated above. - firstLineSimplified.insert(firstLineSimplified.end(), - std::make_move_iterator(std::next(secondLineSimplified.begin())), - std::make_move_iterator(secondLineSimplified.end())); - return firstLineSimplified; -} - -} // end namespace - -namespace costmap_converter -{ - -CostmapToPolygonsDBSMCCH::CostmapToPolygonsDBSMCCH() : BaseCostmapToPolygons() -{ - costmap_ = NULL; - // dynamic_recfg_ = NULL; - neighbor_size_x_ = neighbor_size_y_ = -1; - offset_x_ = offset_y_ = 0.; -} - -CostmapToPolygonsDBSMCCH::~CostmapToPolygonsDBSMCCH() -{ -// if (dynamic_recfg_ != NULL) -// delete dynamic_recfg_; -} - -void CostmapToPolygonsDBSMCCH::initialize(rclcpp::Node::SharedPtr nh) -{ - BaseCostmapToPolygons::initialize(nh); - - costmap_ = NULL; - - parameter_.max_distance_ = 0.4; - nh->get_parameter_or("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_); - - parameter_.min_pts_ = 2; - nh->get_parameter_or("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_); - - parameter_.max_pts_ = 30; - nh->get_parameter_or("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_); - - parameter_.min_keypoint_separation_ = 0.1; - nh->get_parameter_or("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_); - - parameter_buffered_ = parameter_; - - // setup dynamic reconfigure -// dynamic_recfg_ = new dynamic_reconfigure::Server(nh); -// dynamic_reconfigure::Server::CallbackType cb = boost::bind(&CostmapToPolygonsDBSMCCH::reconfigureCB, this, _1, _2); -// dynamic_recfg_->setCallback(cb); -} - - -void CostmapToPolygonsDBSMCCH::compute() -{ - std::vector< std::vector > clusters; - dbScan(clusters); - - // Create new polygon container - PolygonContainerPtr polygons(new std::vector()); - - - // add convex hulls to polygon container - for (std::size_t i = 1; i push_back( geometry_msgs::msg::Polygon() ); - convexHull2(clusters[i], polygons->back() ); - } - - // add our non-cluster points to the polygon container (as single points) - if (!clusters.empty()) - { - for (std::size_t i=0; i < clusters.front().size(); ++i) - { - polygons->push_back( geometry_msgs::msg::Polygon() ); - convertPointToPolygon(clusters.front()[i], polygons->back()); - } - } - - // replace shared polygon container - updatePolygonContainer(polygons); -} - -void CostmapToPolygonsDBSMCCH::setCostmap2D(nav2_costmap_2d::Costmap2D *costmap) -{ - if (!costmap) - return; - - costmap_ = costmap; - - updateCostmap2D(); -} - -void CostmapToPolygonsDBSMCCH::updateCostmap2D() -{ - occupied_cells_.clear(); - - if (!costmap_->getMutex()) - { - RCLCPP_ERROR(getLogger(), "Cannot update costmap since the mutex pointer is null"); - return; - } - - // TODO: currently dynamic reconigure is not supported in ros2 - { // get a copy of our parameters from dynamic reconfigure - std::lock_guard lock(parameter_mutex_); - parameter_ = parameter_buffered_; - } - - std::unique_lock lock(*costmap_->getMutex()); - - // allocate neighbor lookup - int cells_x = int(costmap_->getSizeInMetersX() / parameter_.max_distance_) + 1; - int cells_y = int(costmap_->getSizeInMetersY() / parameter_.max_distance_) + 1; - - if (cells_x != neighbor_size_x_ || cells_y != neighbor_size_y_) { - neighbor_size_x_ = cells_x; - neighbor_size_y_ = cells_y; - neighbor_lookup_.resize(neighbor_size_x_ * neighbor_size_y_); - } - offset_x_ = costmap_->getOriginX(); - offset_y_ = costmap_->getOriginY(); - for (auto& n : neighbor_lookup_) - n.clear(); - - auto size_x = costmap_->getSizeInCellsX(); - auto size_y = costmap_->getSizeInCellsY(); - - // get indices of obstacle cells - for(std::size_t i = 0; i < size_x; i++) - { - for(std::size_t j = 0; j < size_y; j++) - { - int value = costmap_->getCost(i,j); - if(value >= nav2_costmap_2d::LETHAL_OBSTACLE) - { - double x, y; - costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y); - addPoint(x, y); - } - } - } -} - - -void CostmapToPolygonsDBSMCCH::dbScan(std::vector< std::vector >& clusters) -{ - std::vector visited(occupied_cells_.size(), false); - - clusters.clear(); - - //DB Scan Algorithm - int cluster_id = 0; // current cluster_id - clusters.push_back(std::vector()); - for(int i = 0; i< (int)occupied_cells_.size(); i++) - { - if(!visited[i]) //keypoint has not been visited before - { - visited[i] = true; // mark as visited - std::vector neighbors; - regionQuery(i, neighbors); //Find neighbors around the keypoint - if((int)neighbors.size() < parameter_.min_pts_) //If not enough neighbors are found, mark as noise - { - clusters[0].push_back(occupied_cells_[i]); - } - else - { - ++cluster_id; // increment current cluster_id - clusters.push_back(std::vector()); - - // Expand the cluster - clusters[cluster_id].push_back(occupied_cells_[i]); - for(int j = 0; j<(int)neighbors.size(); j++) - { - if ((int)clusters[cluster_id].size() == parameter_.max_pts_) - break; - - if(!visited[neighbors[j]]) //keypoint has not been visited before - { - visited[neighbors[j]] = true; // mark as visited - std::vector further_neighbors; - regionQuery(neighbors[j], further_neighbors); //Find more neighbors around the new keypoint -// if(further_neighbors.size() < min_pts_) -// { -// clusters[0].push_back(occupied_cells[neighbors[j]]); -// } -// else - if ((int)further_neighbors.size() >= parameter_.min_pts_) - { - // neighbors found - neighbors.insert(neighbors.end(), further_neighbors.begin(), further_neighbors.end()); //Add these newfound P' neighbour to P neighbour vector "nb_indeces" - clusters[cluster_id].push_back(occupied_cells_[neighbors[j]]); - } - } - } - } - } - } -} - -void CostmapToPolygonsDBSMCCH::regionQuery(int curr_index, std::vector& neighbors) -{ - neighbors.clear(); - - double dist_sqr_threshold = parameter_.max_distance_ * parameter_.max_distance_; - const KeyPoint& kp = occupied_cells_[curr_index]; - int cx, cy; - pointToNeighborCells(kp, cx,cy); - - // loop over the neighboring cells for looking up the points - const int offsets[9][2] = {{-1, -1}, {0, -1}, {1, -1}, - {-1, 0}, {0, 0}, {1, 0}, - {-1, 1}, {0, 1}, {1, 1}}; - for (int i = 0; i < 9; ++i) - { - int idx = neighborCellsToIndex(cx + offsets[i][0], cy + offsets[i][1]); - if (idx < 0 || idx >= int(neighbor_lookup_.size())) - continue; - const std::vector& pointIndicesToCheck = neighbor_lookup_[idx]; - for (int point_idx : pointIndicesToCheck) { - if (point_idx == curr_index) // point is not a neighbor to itself - continue; - const KeyPoint& other = occupied_cells_[point_idx]; - double dx = other.x - kp.x; - double dy = other.y - kp.y; - double dist_sqr = dx*dx + dy*dy; - if (dist_sqr <= dist_sqr_threshold) - neighbors.push_back(point_idx); - } - } -} - -bool isXCoordinateSmaller(const CostmapToPolygonsDBSMCCH::KeyPoint& p1, const CostmapToPolygonsDBSMCCH::KeyPoint& p2) -{ - return p1.x < p2.x || (p1.x == p2.x && p1.y < p2.y); -} - -void CostmapToPolygonsDBSMCCH::convexHull(std::vector& cluster, geometry_msgs::msg::Polygon& polygon) -{ - //Monotone Chain ConvexHull Algorithm source from http://www.algorithmist.com/index.php/Monotone_Chain_Convex_Hull - - int k = 0; - int n = cluster.size(); - - // sort points according to x coordinate (TODO. is it already sorted due to the map representation?) - std::sort(cluster.begin(), cluster.end(), isXCoordinateSmaller); - - polygon.points.resize(2*n); - - // lower hull - for (int i = 0; i < n; ++i) - { - while (k >= 2 && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0) - { - --k; - } - cluster[i].toPointMsg(polygon.points[k]); - ++k; - } - - // upper hull - for (int i = n-2, t = k+1; i >= 0; --i) - { - while (k >= t && cross(polygon.points[k-2], polygon.points[k-1], cluster[i]) <= 0) - { - --k; - } - cluster[i].toPointMsg(polygon.points[k]); - ++k; - } - - - polygon.points.resize(k); // original - // TEST we skip the last point, since in our definition the polygon vertices do not contain the start/end vertex twice. -// polygon.points.resize(k-1); // TODO remove last point from the algorithm above to reduce computational cost - - simplifyPolygon(polygon); -} - - - -void CostmapToPolygonsDBSMCCH::convexHull2(std::vector& cluster, geometry_msgs::msg::Polygon& polygon) -{ - std::vector& P = cluster; - std::vector& points = polygon.points; - - // Sort P by x and y - std::sort(P.begin(), P.end(), isXCoordinateSmaller); - - // the output array H[] will be used as the stack - int i; // array scan index - - // Get the indices of points with min x-coord and min|max y-coord - int minmin = 0, minmax; - double xmin = P[0].x; - for (i = 1; i < (int)P.size(); i++) - if (P[i].x != xmin) break; - minmax = i - 1; - if (minmax == (int)P.size() - 1) - { // degenerate case: all x-coords == xmin - points.push_back(geometry_msgs::msg::Point32()); - P[minmin].toPointMsg(points.back()); - if (P[minmax].y != P[minmin].y) // a nontrivial segment - { - points.push_back(geometry_msgs::msg::Point32()); - P[minmax].toPointMsg(points.back()); - } - // add polygon endpoint - points.push_back(geometry_msgs::msg::Point32()); - P[minmin].toPointMsg(points.back()); - return; - } - - // Get the indices of points with max x-coord and min|max y-coord - int maxmin, maxmax = (int)P.size() - 1; - double xmax = P.back().x; - for (i = P.size() - 2; i >= 0; i--) - if (P[i].x != xmax) break; - maxmin = i+1; - - // Compute the lower hull on the stack H - // push minmin point onto stack - points.push_back(geometry_msgs::msg::Point32()); - P[minmin].toPointMsg(points.back()); - i = minmax; - while (++i <= maxmin) - { - // the lower line joins P[minmin] with P[maxmin] - if (cross(P[minmin], P[maxmin], P[i]) >= 0 && i < maxmin) - continue; // ignore P[i] above or on the lower line - - while (points.size() > 1) // there are at least 2 points on the stack - { - // test if P[i] is left of the line at the stack top - if (cross(points[points.size() - 2], points.back(), P[i]) > 0) - break; // P[i] is a new hull vertex - points.pop_back(); // pop top point off stack - } - // push P[i] onto stack - points.push_back(geometry_msgs::msg::Point32()); - P[i].toPointMsg(points.back()); - } - - // Next, compute the upper hull on the stack H above the bottom hull - if (maxmax != maxmin) // if distinct xmax points - { - // push maxmax point onto stack - points.push_back(geometry_msgs::msg::Point32()); - P[maxmax].toPointMsg(points.back()); - } - int bot = (int)points.size(); // the bottom point of the upper hull stack - i = maxmin; - while (--i >= minmax) - { - // the upper line joins P[maxmax] with P[minmax] - if (cross( P[maxmax], P[minmax], P[i]) >= 0 && i > minmax) - continue; // ignore P[i] below or on the upper line - - while ((int)points.size() > bot) // at least 2 points on the upper stack - { - // test if P[i] is left of the line at the stack top - if (cross(points[points.size() - 2], points.back(), P[i]) > 0) - break; // P[i] is a new hull vertex - points.pop_back(); // pop top point off stack - } - // push P[i] onto stack - points.push_back(geometry_msgs::msg::Point32()); - P[i].toPointMsg(points.back()); - } - if (minmax != minmin) - { - // push joining endpoint onto stack - points.push_back(geometry_msgs::msg::Point32()); - P[minmin].toPointMsg(points.back()); - } - - simplifyPolygon(polygon); -} - -void CostmapToPolygonsDBSMCCH::simplifyPolygon(geometry_msgs::msg::Polygon& polygon) -{ - size_t triangleThreshold = 3; - // check if first and last point are the same. If yes, a triangle has 4 points - if (polygon.points.size() > 1 - && std::abs(polygon.points.front().x - polygon.points.back().x) < 1e-5 - && std::abs(polygon.points.front().y - polygon.points.back().y) < 1e-5) - { - triangleThreshold = 4; - } - if (polygon.points.size() <= triangleThreshold) // nothing to do for triangles or lines - return; - // TODO Reason about better start conditions for splitting lines, e.g., by - // https://en.wikipedia.org/wiki/Rotating_calipers - polygon.points = douglasPeucker(polygon.points.begin(), polygon.points.end(), parameter_.min_keypoint_separation_);; -} - -void CostmapToPolygonsDBSMCCH::updatePolygonContainer(PolygonContainerPtr polygons) -{ - std::lock_guard lock(mutex_); - polygons_ = polygons; -} - - -PolygonContainerConstPtr CostmapToPolygonsDBSMCCH::getPolygons() -{ - std::lock_guard lock(mutex_); - PolygonContainerConstPtr polygons = polygons_; - return polygons; -} - -//void CostmapToPolygonsDBSMCCH::reconfigureCB(CostmapToPolygonsDBSMCCHConfig& config, uint32_t level) -//{ - //boost::mutex::scoped_lock lock(parameter_mutex_); - //parameter_buffered_.max_distance_ = config.cluster_max_distance; - //parameter_buffered_.min_pts_ = config.cluster_min_pts; - //parameter_buffered_.max_pts_ = config.cluster_max_pts; - //parameter_buffered_.min_keypoint_separation_ = config.convex_hull_min_pt_separation; -//} - -}//end namespace costmap_converter - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons_concave.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons_concave.cpp deleted file mode 100644 index f2ba987..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/src/costmap_to_polygons_concave.cpp +++ /dev/null @@ -1,231 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann, Otniel Rinaldo - *********************************************************************/ - -#include - -#include - -PLUGINLIB_EXPORT_CLASS(costmap_converter::CostmapToPolygonsDBSConcaveHull, costmap_converter::BaseCostmapToPolygons) - -namespace costmap_converter -{ - -CostmapToPolygonsDBSConcaveHull::CostmapToPolygonsDBSConcaveHull() : CostmapToPolygonsDBSMCCH() -{ -// dynamic_recfg_ = NULL; -} - -CostmapToPolygonsDBSConcaveHull::~CostmapToPolygonsDBSConcaveHull() -{ -// if (dynamic_recfg_ != NULL) -// delete dynamic_recfg_; -} - -void CostmapToPolygonsDBSConcaveHull::initialize(rclcpp::Node::SharedPtr nh) -{ - BaseCostmapToPolygons::initialize(nh); - - parameter_.max_distance_ = 0.4; - nh->get_parameter_or("cluster_max_distance", parameter_.max_distance_, parameter_.max_distance_); - - parameter_.min_pts_ = 2; - nh->get_parameter_or("cluster_min_pts", parameter_.min_pts_, parameter_.min_pts_); - - parameter_.max_pts_ = 30; - nh->get_parameter_or("cluster_max_pts", parameter_.max_pts_, parameter_.max_pts_); - - parameter_.min_keypoint_separation_ = 0.1; - nh->get_parameter_or("convex_hull_min_pt_separation", parameter_.min_keypoint_separation_, parameter_.min_keypoint_separation_); - - parameter_buffered_ = parameter_; - - concave_hull_depth_ = 2.0; - nh->get_parameter_or("concave_hull_depth", concave_hull_depth_, concave_hull_depth_); - - // setup dynamic reconfigure -// dynamic_recfg_ = new dynamic_reconfigure::Server(nh); -// dynamic_reconfigure::Server::CallbackType cb = boost::bind(&CostmapToPolygonsDBSConcaveHull::reconfigureCB, this, _1, _2); -// dynamic_recfg_->setCallback(cb); -} - - -void CostmapToPolygonsDBSConcaveHull::compute() -{ - std::vector< std::vector > clusters; - dbScan(clusters); - - // Create new polygon container - PolygonContainerPtr polygons(new std::vector()); - - - // add convex hulls to polygon container - for (size_t i = 1; i push_back( geometry_msgs::msg::Polygon() ); - concaveHull(clusters[i], concave_hull_depth_, polygons->back() ); - } - - // add our non-cluster points to the polygon container (as single points) - if (!clusters.empty()) - { - for (size_t i=0; i < clusters.front().size(); ++i) - { - polygons->push_back( geometry_msgs::msg::Polygon() ); - convertPointToPolygon(clusters.front()[i], polygons->back()); - } - } - - // replace shared polygon container - updatePolygonContainer(polygons); -} - - -void CostmapToPolygonsDBSConcaveHull::concaveHull(std::vector& cluster, double depth, geometry_msgs::msg::Polygon& polygon) -{ - // start with convex hull - convexHull2(cluster, polygon); - - std::vector& concave_list = polygon.points; - - for (int i = 0; i < (int)concave_list.size() - 1; ++i) - { - - // find nearest inner point pk from line (vertex1 -> vertex2) - const geometry_msgs::msg::Point32& vertex1 = concave_list[i]; - const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1]; - - bool found; - size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found); - if (!found) - continue; - - double line_length = norm2d(vertex1, vertex2); - - double dst1 = norm2d(cluster[nearest_idx], vertex1); - double dst2 = norm2d(cluster[nearest_idx], vertex2); - double dd = std::min(dst1, dst2); - if (dd<1e-8) - continue; - - if (line_length / dd > depth) - { - // Check that new candidate edge will not intersect existing edges. - bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]); - intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2); - if (!intersects) - { - geometry_msgs::msg::Point32 new_point; - cluster[nearest_idx].toPointMsg(new_point); - concave_list.insert(concave_list.begin() + i + 1, new_point); - i--; - } - } - } -} - - -void CostmapToPolygonsDBSConcaveHull::concaveHullClusterCut(std::vector& cluster, double depth, geometry_msgs::msg::Polygon& polygon) -{ - // start with convex hull - convexHull2(cluster, polygon); - - std::vector& concave_list = polygon.points; - - // get line length - double mean_length = 0; - for (int i = 0; i < (int)concave_list.size() - 1; ++i) - { - mean_length += norm2d(concave_list[i],concave_list[i+1]); - } - mean_length /= double(concave_list.size()); - - for (int i = 0; i < (int)concave_list.size() - 1; ++i) - { - - // find nearest inner point pk from line (vertex1 -> vertex2) - const geometry_msgs::msg::Point32& vertex1 = concave_list[i]; - const geometry_msgs::msg::Point32& vertex2 = concave_list[i+1]; - - double line_length = norm2d(vertex1, vertex2); - - bool found; - size_t nearest_idx = findNearestInnerPoint(vertex1, vertex2, cluster, concave_list, &found); - if (!found) - { - continue; - } - - - double dst1 = norm2d(cluster[nearest_idx], vertex1); - double dst2 = norm2d(cluster[nearest_idx], vertex2); - double dd = std::min(dst1, dst2); - if (dd<1e-8) - continue; - - if (line_length / dd > depth) - { - // Check that new candidate edge will not intersect existing edges. - bool intersects = checkLineIntersection(concave_list, vertex1, vertex2, vertex1, cluster[nearest_idx]); - intersects |= checkLineIntersection(concave_list, vertex1, vertex2, cluster[nearest_idx], vertex2); - if (!intersects) - { - geometry_msgs::msg::Point32 new_point; - cluster[nearest_idx].toPointMsg(new_point); - concave_list.insert(concave_list.begin() + i + 1, new_point); - i--; - } - } - } -} - - - - -//void CostmapToPolygonsDBSConcaveHull::reconfigureCB(CostmapToPolygonsDBSConcaveHullConfig& config, uint32_t level) -//{ -// boost::mutex::scoped_lock lock(parameter_mutex_); -// parameter_buffered_.max_distance_ = config.cluster_max_distance; -// parameter_buffered_.min_pts_ = config.cluster_min_pts; -// parameter_buffered_.max_pts_ = config.cluster_max_pts; -// parameter_buffered_.min_keypoint_separation_ = config.cluster_min_pts; -// concave_hull_depth_ = config.concave_hull_depth; -//} - -}//end namespace costmap_converter - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.cpp b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.cpp deleted file mode 100644 index e563475..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.cpp +++ /dev/null @@ -1,230 +0,0 @@ -#include -#include -#include - -#include - -namespace { -geometry_msgs::msg::Point32 create_point(double x, double y) -{ - geometry_msgs::msg::Point32 result; - result.x = x; - result.y = y; - result.z = 0.; - return result; -} -} // end namespace - -// make things accesible in the test -class CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBSMCCH -{ - public: - const std::vector& points() const {return occupied_cells_;} - costmap_converter::CostmapToPolygonsDBSMCCH::Parameters& parameters() {return parameter_;} - using costmap_converter::CostmapToPolygonsDBSMCCH::addPoint; - using costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery; - using costmap_converter::CostmapToPolygonsDBSMCCH::dbScan; - using costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2; - using costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon; -}; - -class CostmapToPolygonsDBSMCCHTest : public ::testing::Test -{ - protected: - void SetUp() override { - // parameters - costmap_to_polygons.parameters().max_distance_ = 0.5; - costmap_to_polygons.parameters().max_pts_ = 100; - costmap_to_polygons.parameters().min_pts_ = 2; - costmap_to_polygons.parameters().min_keypoint_separation_ = 0.1; - - costmap.reset(new nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.)); - costmap_to_polygons.setCostmap2D(costmap.get()); - - std::random_device rand_dev; - std::mt19937 generator(rand_dev()); - std::uniform_real_distribution<> random_angle(-M_PI, M_PI); - std::uniform_real_distribution<> random_dist(0., costmap_to_polygons.parameters().max_distance_); - - costmap_to_polygons.addPoint(0., 0.); - costmap_to_polygons.addPoint(1.3, 1.3); - - // adding random points - double center_x = costmap_to_polygons.points()[0].x; - double center_y = costmap_to_polygons.points()[0].y; - for (int i = 0; i < costmap_to_polygons.parameters().max_pts_ - 1; ++i) - { - double alpha = random_angle(generator); - double dist = random_dist(generator); - double wx = center_x + std::cos(alpha) * dist; - double wy = center_y + std::sin(alpha) * dist; - costmap_to_polygons.addPoint(wx, wy); - } - - // some noisy points not belonging to a cluster - costmap_to_polygons.addPoint(-1, -1); - costmap_to_polygons.addPoint(-2, -2); - - // adding random points - center_x = costmap_to_polygons.points()[1].x; - center_y = costmap_to_polygons.points()[1].y; - for (int i = 0; i < costmap_to_polygons.parameters().max_pts_/2; ++i) - { - double alpha = random_angle(generator); - double dist = random_dist(generator); - double wx = center_x + std::cos(alpha) * dist; - double wy = center_y + std::sin(alpha) * dist; - costmap_to_polygons.addPoint(wx, wy); - } - } - - void regionQueryTrivial(int curr_index, std::vector& neighbor_indices) - { - neighbor_indices.clear(); - const auto& query_point = costmap_to_polygons.points()[curr_index]; - for (int i = 0; i < int(costmap_to_polygons.points().size()); ++i) - { - if (i == curr_index) - continue; - const auto& kp = costmap_to_polygons.points()[i]; - double dx = query_point.x - kp.x; - double dy = query_point.y - kp.y; - double dist = sqrt(dx*dx + dy*dy); - if (dist < costmap_to_polygons.parameters().max_distance_) - neighbor_indices.push_back(i); - } - } - - CostmapToPolygons costmap_to_polygons; - std::shared_ptr costmap; -}; - -TEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery) -{ - std::vector neighbors, neighbors_trivial; - costmap_to_polygons.regionQuery(0, neighbors); - regionQueryTrivial(0, neighbors_trivial); - ASSERT_EQ(costmap_to_polygons.parameters().max_pts_ - 1, int(neighbors.size())); - ASSERT_EQ(neighbors_trivial.size(), neighbors.size()); - std::sort(neighbors.begin(), neighbors.end()); - ASSERT_EQ(neighbors_trivial, neighbors); - - costmap_to_polygons.regionQuery(1, neighbors); - regionQueryTrivial(1, neighbors_trivial); - ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2, int(neighbors.size())); - ASSERT_EQ(neighbors_trivial.size(), neighbors.size()); - std::sort(neighbors.begin(), neighbors.end()); - ASSERT_EQ(neighbors_trivial, neighbors); -} - -TEST_F(CostmapToPolygonsDBSMCCHTest, dbScan) -{ - std::vector< std::vector > clusters; - costmap_to_polygons.dbScan(clusters); - - ASSERT_EQ((unsigned)3, clusters.size()); - ASSERT_EQ((unsigned)2, clusters[0].size()); // noisy points not belonging to a cluster - ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_, clusters[1].size()); // first cluster at (0,0) - ASSERT_EQ((unsigned)costmap_to_polygons.parameters().max_pts_/2 + 1, clusters[2].size()); // second cluster at (1,1) -} - -TEST(CostmapToPolygonsDBSMCCH, EmptyMap) -{ - std::shared_ptr costmap = - std::make_shared(nav2_costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.)); - CostmapToPolygons costmap_to_polygons; - costmap_to_polygons.setCostmap2D(costmap.get()); - - std::vector< std::vector > clusters; - costmap_to_polygons.dbScan(clusters); - ASSERT_EQ((unsigned)1, clusters.size()); // noise cluster exists - ASSERT_EQ((unsigned)0, clusters[0].size()); // noise clsuter is empty -} - -TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon) -{ - const double simplification_threshold = 0.1; - CostmapToPolygons costmap_to_polygons; - costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold; - - geometry_msgs::msg::Polygon polygon; - polygon.points.push_back(create_point(0., 0.)); - polygon.points.push_back(create_point(1., 0.)); - - // degenerate case with just two points - geometry_msgs::msg::Polygon original_polygon = polygon; - costmap_to_polygons.simplifyPolygon(polygon); - ASSERT_EQ((unsigned)2, polygon.points.size()); - for (size_t i = 0; i < polygon.points.size(); ++i) - { - ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x); - ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y); - } - - // degenerate case with three points - polygon.points.push_back(create_point(1., simplification_threshold / 2.)); - original_polygon = polygon; - costmap_to_polygons.simplifyPolygon(polygon); - ASSERT_EQ((unsigned)3, polygon.points.size()); - for (size_t i = 0; i < polygon.points.size(); ++i) - { - ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x); - ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y); - } - - polygon.points.push_back(create_point(1., 1.)); - original_polygon = polygon; - // remove the point that has to be removed by the simplification - original_polygon.points.erase(original_polygon.points.begin()+2); - costmap_to_polygons.simplifyPolygon(polygon); - ASSERT_EQ((unsigned)3, polygon.points.size()); - for (size_t i = 0; i < polygon.points.size(); ++i) - { - ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x); - ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y); - } -} - -TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines) -{ - const double simplification_threshold = 0.1; - CostmapToPolygons costmap_to_polygons; - costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold; - - geometry_msgs::msg::Polygon polygon; - for (int i = 0; i <= 100; ++i) - polygon.points.push_back(create_point(i*1., 0.)); - geometry_msgs::msg::Point32 lastPoint = polygon.points.back(); - for (int i = 0; i <= 100; ++i) - polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.)); - lastPoint = polygon.points.back(); - for (int i = 0; i <= 100; ++i) - polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y)); - lastPoint = polygon.points.back(); - for (int i = 0; i <= 100; ++i) - polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.)); - lastPoint = polygon.points.back(); - for (int i = 0; i <= 100; ++i) - polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y)); - - costmap_to_polygons.simplifyPolygon(polygon); - ASSERT_EQ((unsigned)6, polygon.points.size()); - ASSERT_FLOAT_EQ( 0., polygon.points[0].x); - ASSERT_FLOAT_EQ( 0., polygon.points[0].y); - ASSERT_FLOAT_EQ(100., polygon.points[1].x); - ASSERT_FLOAT_EQ( 0., polygon.points[1].y); - ASSERT_FLOAT_EQ(100., polygon.points[2].x); - ASSERT_FLOAT_EQ(100., polygon.points[2].y); - ASSERT_FLOAT_EQ(200., polygon.points[3].x); - ASSERT_FLOAT_EQ(100., polygon.points[3].y); - ASSERT_FLOAT_EQ(200., polygon.points[4].x); - ASSERT_FLOAT_EQ(200., polygon.points[4].y); - ASSERT_FLOAT_EQ(300., polygon.points[5].x); - ASSERT_FLOAT_EQ(200., polygon.points[5].y); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.test b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.test deleted file mode 100644 index 8f42354..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter/test/costmap_polygons.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CHANGELOG.rst b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CHANGELOG.rst deleted file mode 100644 index f3fabe8..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CHANGELOG.rst +++ /dev/null @@ -1,41 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package costmap_converter_msgs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.1.2 (2020-06-22) ------------------- - -0.1.1 (2020-01-25) ------------------- - -0.1.0 (2020-01-23) ------------------- -* Messages moved to a separate package -* Contributors: Vinnam Kim - -0.0.9 (2018-05-28) ------------------- - -0.0.8 (2018-05-17) ------------------- - -0.0.7 (2017-09-20) ------------------- - -0.0.6 (2017-09-18) ------------------- - -0.0.5 (2016-02-01) ------------------- - -0.0.4 (2016-01-11) ------------------- - -0.0.3 (2015-12-23) ------------------- - -0.0.2 (2015-12-22) ------------------- - -0.0.1 (2015-12-21) ------------------- diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CMakeLists.txt b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CMakeLists.txt deleted file mode 100644 index 9ad3ee4..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.5) - -project(costmap_converter_msgs) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces(costmap_converter_msgs - "msg/ObstacleMsg.msg" - "msg/ObstacleArrayMsg.msg" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs -) - -ament_package() diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleArrayMsg.msg b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleArrayMsg.msg deleted file mode 100644 index e4eec4d..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleArrayMsg.msg +++ /dev/null @@ -1,10 +0,0 @@ -# Message that contains a list of polygon shaped obstacles. -# Special types: -# Polygon with 1 vertex: Point obstacle -# Polygon with 2 vertices: Line obstacle -# Polygon with more than 2 vertices: First and last points are assumed to be connected - -std_msgs/Header header - -costmap_converter_msgs/ObstacleMsg[] obstacles - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleMsg.msg b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleMsg.msg deleted file mode 100644 index 9a463a3..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/msg/ObstacleMsg.msg +++ /dev/null @@ -1,24 +0,0 @@ -# Special types: -# Polygon with 1 vertex: Point obstacle (you might also specify a non-zero value for radius) -# Polygon with 2 vertices: Line obstacle -# Polygon with more than 2 vertices: First and last points are assumed to be connected - -std_msgs/Header header - -# Obstacle footprint (polygon descriptions) -geometry_msgs/Polygon polygon - -# Specify the radius for circular/point obstacles -float64 radius - -# Obstacle ID -# Specify IDs in order to provide (temporal) relationships -# between obstacles among multiple messages. -int64 id - -# Individual orientation (centroid) -geometry_msgs/Quaternion orientation - -# Individual velocities (centroid) -geometry_msgs/TwistWithCovariance velocities - diff --git a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/package.xml b/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/package.xml deleted file mode 100644 index faa0863..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/costmap_converter/costmap_converter_msgs/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - costmap_converter_msgs - 0.1.2 - Package containing message types for costmap conversion - Christoph Rösmann - - BSD - - ament_cmake - - rosidl_default_generators - - builtin_interfaces - geometry_msgs - std_msgs - - builtin_interfaces - geometry_msgs - rosidl_default_runtime - std_msgs - - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/CMakeLists.txt b/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/CMakeLists.txt deleted file mode 100644 index cf42230..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -cmake_minimum_required(VERSION 3.10) -project(fake_vel_transform) - -## Use C++14 -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, -## enforcing cleaner code. -add_definitions(-Wall -Werror) - -## Export compile commands for clangd -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -####################### -## Find dependencies ## -####################### - -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -########### -## Build ## -########### - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/fake_vel_transform.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN fake_vel_transform::FakeVelTransform - EXECUTABLE ${PROJECT_NAME}_node -) - -############# -## Testing ## -############# - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_copyright - ament_cmake_uncrustify - ) - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# - -ament_auto_package( - INSTALL_TO_SHARE - launch -) diff --git a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/README.md b/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/README.md deleted file mode 100644 index a43c718..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/README.md +++ /dev/null @@ -1,29 +0,0 @@ -# fake_vel_transform - -该包用于实现雷达和云台共用的 yaw 轴时的兼容性，即使云台处于旋转扫描状态，速度变换后，仍然可以实现较稳定的轨迹跟踪效果。 - -## 实现方式 - -创建 XYZ 轴基于 `base_link`, RPY 轴基于局部路径规划朝向的 `base_link_fake` 坐标系，使得 nav2 局部规划器能够将机器人的方向视为与当前路径规划方向一致。 - -nav2 发布的速度也是基于 `base_link_fake` 坐标系的，通过 tf2 将速度转换到 `base_link` 坐标系，使机器人能够正常运动。在仿真中表现即底盘小陀螺时仍能跟随轨迹。 - -电控端执行底盘控制命令时，机器人运动正方向为云台枪管朝向。 - -## fake_vel_transform_node - -订阅： - -- nav2 发布的基于 base_link_fake 坐标系的速度指令 `/cmd_vel` -- nav2 controller 发布的局部路径朝向 `/local_path` -- `odom` 到 `base_link` 的 tf 变换 - -发布： - -- 转换到 base_link 坐标系的速度 `/cmd_vel_chassis` - -静态参数： - -- 底盘固定旋转速度 `spin_speed` - - 搭配电控固定小陀螺速度，将 spin_speed 设为负，可实现移动时小陀螺减慢。 diff --git a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp b/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp deleted file mode 100644 index 3ca3892..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/include/fake_vel_transform/fake_vel_transform.hpp +++ /dev/null @@ -1,56 +0,0 @@ -#ifndef FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_ -#define FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace fake_vel_transform -{ -class FakeVelTransform : public rclcpp::Node -{ -public: - explicit FakeVelTransform(const rclcpp::NodeOptions & options); - -private: - void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg); - - void localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg); - - void publishTransform(); - - // Subscriber with tf2 message_filter - std::string target_frame_; - std::shared_ptr tf2_buffer_; - std::shared_ptr tf2_listener_; - - rclcpp::Subscription::SharedPtr cmd_vel_sub_; - rclcpp::Subscription::SharedPtr local_pose_sub_; - - rclcpp::Publisher::SharedPtr cmd_vel_chassis_pub_; - - // Broadcast tf from base_link to base_link_fake - rclcpp::TimerBase::SharedPtr tf_timer_; - std::unique_ptr tf_broadcaster_; - - geometry_msgs::msg::PoseStamped planner_local_pose_; - double current_angle_; - double base_link_angle_; - float spin_speed_; -}; - -} // namespace fake_vel_transform - -#endif // FAKE_VEL_TRANSFORM__FAKE_VEL_TRANSFORM_HPP_ diff --git a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/launch/fake_vel_transform.launch.py b/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/launch/fake_vel_transform.launch.py deleted file mode 100644 index 18cf552..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/launch/fake_vel_transform.launch.py +++ /dev/null @@ -1,20 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - use_sim_time = LaunchConfiguration('use_sim_time', default='true') - - fake_vel_transform_node = Node( - package='fake_vel_transform', - executable='fake_vel_transform_node', - output='screen', - parameters=[ - {'use_sim_time': use_sim_time } - ] - ) - - return LaunchDescription([fake_vel_transform_node]) diff --git a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/package.xml b/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/package.xml deleted file mode 100644 index 9be9e08..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - fake_vel_transform - 0.1.0 - A template for ROS packages. - MIT - Lihan Chen - Lihan Chen - - - ament_cmake - - - rclcpp - rclcpp_components - geometry_msgs - nav_msgs - tf2_ros - tf2_geometry_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_clang_format - - - ament_cmake - - diff --git a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp b/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp deleted file mode 100644 index 53b0a86..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/fake_vel_transform/src/fake_vel_transform.cpp +++ /dev/null @@ -1,103 +0,0 @@ -#include "fake_vel_transform/fake_vel_transform.hpp" - -#include - -#include -#include -#include - -namespace fake_vel_transform -{ -const std::string CMD_VEL_TOPIC = "/cmd_vel"; -const std::string AFTER_TF_CMD_VEL = "/cmd_vel_chassis"; -const std::string TRAJECTORY_TOPIC = "/local_plan"; -const int TF_PUBLISH_FREQUENCY = 20; // base_link to base_link_fake. Frequency in Hz. - -FakeVelTransform::FakeVelTransform(const rclcpp::NodeOptions & options) -: Node("fake_vel_transform", options) -{ - RCLCPP_INFO(get_logger(), "Start FakeVelTransform!"); - - // Declare and get the spin speed parameter - this->declare_parameter("spin_speed", -6.0); - this->get_parameter("spin_speed", spin_speed_); - - // TF broadcaster - tf_broadcaster_ = std::make_unique(*this); - tf2_buffer_ = std::make_unique(this->get_clock()); - tf2_listener_ = std::make_shared(*tf2_buffer_); - - // Create Publisher and Subscriber - cmd_vel_sub_ = this->create_subscription( - CMD_VEL_TOPIC, 1, std::bind(&FakeVelTransform::cmdVelCallback, this, std::placeholders::_1)); - cmd_vel_chassis_pub_ = this->create_publisher( - AFTER_TF_CMD_VEL, rclcpp::QoS(rclcpp::KeepLast(1))); - local_pose_sub_ = this->create_subscription( - TRAJECTORY_TOPIC, 1, - std::bind(&FakeVelTransform::localPoseCallback, this, std::placeholders::_1)); - - // Create a timer to publish the transform regularly - tf_timer_ = this->create_wall_timer( - std::chrono::milliseconds(1000 / TF_PUBLISH_FREQUENCY), - std::bind(&FakeVelTransform::publishTransform, this)); -} - -// Get the local pose from planner -void FakeVelTransform::localPoseCallback(const nav_msgs::msg::Path::SharedPtr msg) -{ - if (!msg || msg->poses.empty()) { - RCLCPP_WARN(get_logger(), "Received empty or invalid PoseArray message"); - return; - } - - // Choose the pose based on the size of the poses array - size_t index = std::min(msg->poses.size() / 4, msg->poses.size() - 1); - const geometry_msgs::msg::Pose & selected_pose = msg->poses[index].pose; - - // Update current angle based on the difference between teb_angle and base_link_angle_ - double teb_angle = tf2::getYaw(selected_pose.orientation); - current_angle_ = teb_angle - base_link_angle_; -} - -// Transform the velocity from base_link to base_link_fake -void FakeVelTransform::cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) -{ - try { - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = tf2_buffer_->lookupTransform("odom", "base_link", tf2::TimePointZero); - base_link_angle_ = tf2::getYaw(transform_stamped.transform.rotation); - - double angle_diff = -current_angle_; - - geometry_msgs::msg::Twist aft_tf_vel; - aft_tf_vel.angular.z = (msg->angular.z != 0) ? spin_speed_ : 0; - aft_tf_vel.linear.x = msg->linear.x * cos(angle_diff) + msg->linear.y * sin(angle_diff); - aft_tf_vel.linear.y = -msg->linear.x * sin(angle_diff) + msg->linear.y * cos(angle_diff); - - cmd_vel_chassis_pub_->publish(aft_tf_vel); - } catch (tf2::TransformException & ex) { - RCLCPP_INFO(this->get_logger(), "Could not transform odom to base_link: %s", ex.what()); - } -} - -// Publish transform from base_link to base_link_fake -void FakeVelTransform::publishTransform() -{ - geometry_msgs::msg::TransformStamped t; - t.header.stamp = get_clock()->now(); - t.header.frame_id = "base_link"; - t.child_frame_id = "base_link_fake"; - tf2::Quaternion q; - q.setRPY(0, 0, current_angle_); - t.transform.rotation = tf2::toMsg(q); - tf_broadcaster_->sendTransform(t); -} - -} // namespace fake_vel_transform - -#include "rclcpp_components/register_node_macro.hpp" - -// Register the component with class_loader. -// This acts as a sort of entry point, allowing the component to be discoverable when its library -// is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(fake_vel_transform::FakeVelTransform) \ No newline at end of file diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/CMakeLists.txt b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/CMakeLists.txt deleted file mode 100644 index 319374c..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/CMakeLists.txt +++ /dev/null @@ -1,34 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(rm_navigation) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -if(CMAKE_EXPORT_COMPILE_COMMANDS) - set(CMAKE_EXPORT_COMPILE_COMMANDS ${CMAKE_EXPORT_COMPILE_COMMANDS}) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package(INSTALL_TO_SHARE - params - rviz - launch - ) diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/bringup_rm_navigation.py b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/bringup_rm_navigation.py deleted file mode 100644 index f634161..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/bringup_rm_navigation.py +++ /dev/null @@ -1,156 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, GroupAction, - IncludeLaunchDescription, SetEnvironmentVariable) -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch_ros.actions import PushRosNamespace -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('rm_navigation') - launch_dir = os.path.join(bringup_dir, 'launch') - - # Create the launch configuration variables - namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') - map_yaml_file = LaunchConfiguration('map') - use_sim_time = LaunchConfiguration('use_sim_time') - params_file = LaunchConfiguration('params_file') - autostart = LaunchConfiguration('autostart') - use_composition = LaunchConfiguration('use_composition') - use_respawn = LaunchConfiguration('use_respawn') - log_level = LaunchConfiguration('log_level') - use_nav_rviz = LaunchConfiguration('nav_rviz') - - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time, - 'yaml_filename': map_yaml_file} - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) - - stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') - - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') - - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack') - - declare_use_slam_cmd = DeclareLaunchArgument( - 'use_slam', - default_value='True', - description='Whether run a SLAM') - - declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - default_value= os.path.join(bringup_dir,'map', 'RMUL.yaml'), - description='Full path to map yaml file to load') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='True', - description='Use simulation (Gazebo) clock if true') - - declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') - - - declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='True', - description='Automatically startup the nav2 stack') - - declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', - description='Whether to use composed bringup') - - declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='True', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') - - declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') - - declare_nav_rviz_cmd = DeclareLaunchArgument( - 'nav_rviz', - default_value='True', - description='Visualize navigation2 if true') - - # Specify the actions - bringup_cmd_group = GroupAction([ - PushRosNamespace( - condition=IfCondition(use_namespace), - namespace=namespace), - - Node( - condition=IfCondition(use_composition), - name='nav2_container', - package='rclcpp_components', - executable='component_container_mt', - parameters=[configured_params, {'autostart': autostart}], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings, - output='screen'), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'autostart': autostart, - 'params_file': params_file, - 'use_composition': use_composition, - 'use_respawn': use_respawn, - 'container_name': 'nav2_container'}.items()), - - IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(launch_dir, 'rviz_launch.py')), - condition=IfCondition(use_nav_rviz) - ), - ]) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set environment variables - ld.add_action(stdout_linebuf_envvar) - - # Declare the launch options - ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_use_slam_cmd) - ld.add_action(declare_map_yaml_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_use_composition_cmd) - ld.add_action(declare_use_respawn_cmd) - ld.add_action(declare_log_level_cmd) - ld.add_action(declare_nav_rviz_cmd) - - # Add the actions to launch all of the navigation nodes - ld.add_action(bringup_cmd_group) - - return ld diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/localization_amcl_launch.py b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/localization_amcl_launch.py deleted file mode 100644 index 598084f..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/localization_amcl_launch.py +++ /dev/null @@ -1,149 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, SetEnvironmentVariable -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import LoadComposableNodes, Node -from launch_ros.descriptions import ComposableNode, ParameterFile -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - - namespace = LaunchConfiguration('namespace') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - params_file = LaunchConfiguration('params_file') - use_composition = LaunchConfiguration('use_composition') - container_name = LaunchConfiguration('container_name') - container_name_full = (namespace, '/', container_name) - use_respawn = LaunchConfiguration('use_respawn') - log_level = LaunchConfiguration('log_level') - - lifecycle_nodes = ['amcl'] - - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time} - - configured_params = ParameterFile( - RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True), - allow_substs=True) - - stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') - - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation (Gazebo) clock if true') - - declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') - - declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') - - declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') - - declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') - - declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') - - declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') - - load_nodes = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), - actions=[ - Node( - package='nav2_amcl', - executable='amcl', - name='amcl', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_localization', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ] - ) - - load_composable_nodes = LoadComposableNodes( - condition=IfCondition(use_composition), - target_container=container_name_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_amcl', - plugin='nav2_amcl::AmclNode', - name='amcl', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_localization', - parameters=[{'use_sim_time': use_sim_time, - 'autostart': autostart, - 'node_names': lifecycle_nodes}]), - ], - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set environment variables - ld.add_action(stdout_linebuf_envvar) - - # Declare the launch options - ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_use_composition_cmd) - ld.add_action(declare_container_name_cmd) - ld.add_action(declare_use_respawn_cmd) - ld.add_action(declare_log_level_cmd) - - # Add the actions to launch all of the localiztion nodes - ld.add_action(load_nodes) - ld.add_action(load_composable_nodes) - - return ld \ No newline at end of file diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/map_server_launch.py b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/map_server_launch.py deleted file mode 100644 index ecc31b5..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/map_server_launch.py +++ /dev/null @@ -1,158 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode, ParameterFile -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - - namespace = LaunchConfiguration('namespace') - map_yaml_file = LaunchConfiguration('map') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - params_file = LaunchConfiguration('params_file') - use_composition = LaunchConfiguration('use_composition') - container_name = LaunchConfiguration('container_name') - container_name_full = (namespace, '/', container_name) - use_respawn = LaunchConfiguration('use_respawn') - log_level = LaunchConfiguration('log_level') - - lifecycle_nodes = ['map_server'] - - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time, - 'yaml_filename': map_yaml_file} - - configured_params = ParameterFile( - RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True), - allow_substs=True) - - stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') - - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') - - declare_map_yaml_cmd = DeclareLaunchArgument( - 'map', - description='Full path to map yaml file to load') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation (Gazebo) clock if true') - - declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') - - declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') - - declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') - - declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') - - declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') - - declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') - - load_nodes = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), - actions=[ - Node( - package='nav2_map_server', - executable='map_server', - name='map_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_localization', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]) - ] - ) - - load_composable_nodes = LoadComposableNodes( - condition=IfCondition(use_composition), - target_container=container_name_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_map_server', - plugin='nav2_map_server::MapServer', - name='map_server', - parameters=[configured_params], - remappings=remappings), - - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_localization', - parameters=[{'use_sim_time': use_sim_time, - 'autostart': autostart, - 'node_names': lifecycle_nodes}]), - ], - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set environment variables - ld.add_action(stdout_linebuf_envvar) - - # Declare the launch options - ld.add_action(declare_namespace_cmd) - ld.add_action(declare_map_yaml_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_use_composition_cmd) - ld.add_action(declare_container_name_cmd) - ld.add_action(declare_use_respawn_cmd) - ld.add_action(declare_log_level_cmd) - - # Add the actions to launch all of the localiztion nodes - ld.add_action(load_nodes) - ld.add_action(load_composable_nodes) - - return ld \ No newline at end of file diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/navigation_launch.py b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/navigation_launch.py deleted file mode 100644 index 1dadff1..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/navigation_launch.py +++ /dev/null @@ -1,270 +0,0 @@ -# Copyright (c) 2018 Intel Corporation -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes -from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode -from nav2_common.launch import RewrittenYaml - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('rm_navigation') - - namespace = LaunchConfiguration('namespace') - use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') - params_file = LaunchConfiguration('params_file') - use_composition = LaunchConfiguration('use_composition') - container_name = LaunchConfiguration('container_name') - container_name_full = (namespace, '/', container_name) - use_respawn = LaunchConfiguration('use_respawn') - log_level = LaunchConfiguration('log_level') - - lifecycle_nodes = ['controller_server', - 'smoother_server', - 'planner_server', - 'behavior_server', - 'bt_navigator', - 'waypoint_follower', - 'velocity_smoother'] - - # Map fully qualified names to relative ones so the node's namespace can be prepended. - # In case of the transforms (tf), currently, there doesn't seem to be a better alternative - # https://github.com/ros/geometry2/issues/32 - # https://github.com/ros/robot_state_publisher/pull/30 - # TODO(orduno) Substitute with `PushNodeRemapping` - # https://github.com/ros2/launch_ros/issues/56 - remappings = [('/tf', 'tf'), - ('/tf_static', 'tf_static')] - - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'use_sim_time': use_sim_time, - 'autostart': autostart} - - configured_params = RewrittenYaml( - source_file=params_file, - root_key=namespace, - param_rewrites=param_substitutions, - convert_types=True) - - stdout_linebuf_envvar = SetEnvironmentVariable( - 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') - - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') - - declare_use_sim_time_cmd = DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - description='Use simulation (Gazebo) clock if true') - - declare_params_file_cmd = DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use for all launched nodes') - - declare_autostart_cmd = DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack') - - declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') - - declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') - - declare_use_respawn_cmd = DeclareLaunchArgument( - 'use_respawn', default_value='False', - description='Whether to respawn if a node crashes. Applied when composition is disabled.') - - declare_log_level_cmd = DeclareLaunchArgument( - 'log_level', default_value='info', - description='log level') - - load_nodes = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), - actions=[ - Node( - package='nav2_controller', - executable='controller_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), - Node( - package='nav2_smoother', - executable='smoother_server', - name='smoother_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_waypoint_follower', - executable='waypoint_follower', - name='waypoint_follower', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_velocity_smoother', - executable='velocity_smoother', - name='velocity_smoother', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - ] - ) - - load_composable_nodes = LoadComposableNodes( - condition=IfCondition(use_composition), - target_container=container_name_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_controller', - plugin='nav2_controller::ControllerServer', - name='controller_server', - parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), - ComposableNode( - package='nav2_smoother', - plugin='nav2_smoother::SmootherServer', - name='smoother_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_planner', - plugin='nav2_planner::PlannerServer', - name='planner_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_behaviors', - plugin='behavior_server::BehaviorServer', - name='behavior_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_bt_navigator', - plugin='nav2_bt_navigator::BtNavigator', - name='bt_navigator', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_waypoint_follower', - plugin='nav2_waypoint_follower::WaypointFollower', - name='waypoint_follower', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_velocity_smoother', - plugin='nav2_velocity_smoother::VelocitySmoother', - name='velocity_smoother', - parameters=[configured_params], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_navigation', - parameters=[{'use_sim_time': use_sim_time, - 'autostart': autostart, - 'node_names': lifecycle_nodes}]), - ], - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Set environment variables - ld.add_action(stdout_linebuf_envvar) - - # Declare the launch options - ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_sim_time_cmd) - ld.add_action(declare_params_file_cmd) - ld.add_action(declare_autostart_cmd) - ld.add_action(declare_use_composition_cmd) - ld.add_action(declare_container_name_cmd) - ld.add_action(declare_use_respawn_cmd) - ld.add_action(declare_log_level_cmd) - # Add the actions to launch all of the navigation nodes - ld.add_action(load_nodes) - ld.add_action(load_composable_nodes) - - return ld diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/rviz_launch.py b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/rviz_launch.py deleted file mode 100644 index d786039..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/launch/rviz_launch.py +++ /dev/null @@ -1,109 +0,0 @@ -# Copyright (c) 2018 Intel Corporation -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler -from launch.conditions import IfCondition, UnlessCondition -from launch.event_handlers import OnProcessExit -from launch.events import Shutdown -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from nav2_common.launch import ReplaceString - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('rm_navigation') - - # Create the launch configuration variables - namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') - rviz_config_file = LaunchConfiguration('rviz_config') - - # Declare the launch arguments - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='navigation', - description=('Top-level namespace. The value will be used to replace the ' - ' keyword on the rviz config file.')) - - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack') - - declare_rviz_config_file_cmd = DeclareLaunchArgument( - 'rviz_config', - default_value=os.path.join(bringup_dir, 'rviz', 'nav2.rviz'), - description='Full path to the RVIZ config file to use') - - # Launch rviz - start_rviz_cmd = Node( - condition=UnlessCondition(use_namespace), - package='rviz2', - executable='rviz2', - arguments=['-d', rviz_config_file], - output='screen') - - namespaced_rviz_config_file = ReplaceString( - source_file=rviz_config_file, - replacements={'': ('/', namespace)}) - - start_namespaced_rviz_cmd = Node( - condition=IfCondition(use_namespace), - package='rviz2', - executable='rviz2', - namespace=namespace, - arguments=['-d', namespaced_rviz_config_file], - output='screen', - remappings=[('/map', 'map'), - ('/tf', 'tf'), - ('/tf_static', 'tf_static'), - ('/goal_pose', 'goal_pose'), - ('/clicked_point', 'clicked_point'), - ('/initialpose', 'initialpose')]) - - exit_event_handler = RegisterEventHandler( - condition=UnlessCondition(use_namespace), - event_handler=OnProcessExit( - target_action=start_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) - - exit_event_handler_namespaced = RegisterEventHandler( - condition=IfCondition(use_namespace), - event_handler=OnProcessExit( - target_action=start_namespaced_rviz_cmd, - on_exit=EmitEvent(event=Shutdown(reason='rviz exited')))) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(declare_namespace_cmd) - ld.add_action(declare_use_namespace_cmd) - ld.add_action(declare_rviz_config_file_cmd) - - # Add any conditioned actions - ld.add_action(start_rviz_cmd) - ld.add_action(start_namespaced_rviz_cmd) - - # Add other nodes and processes we need - ld.add_action(exit_event_handler) - ld.add_action(exit_event_handler_namespaced) - - return ld diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/package.xml b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/package.xml deleted file mode 100644 index 8030112..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - rm_navigation - 0.0.0 - TODO: Package description - fyt - TODO: License declaration - - ament_cmake - - spatio_temporal_voxel_layer - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/mapper_params_online_async.yaml b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/mapper_params_online_async.yaml deleted file mode 100644 index f1677b6..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/mapper_params_online_async.yaml +++ /dev/null @@ -1,73 +0,0 @@ -slam_toolbox: - ros__parameters: - - # Plugin params - solver_plugin: solver_plugins::CeresSolver - ceres_linear_solver: SPARSE_NORMAL_CHOLESKY - ceres_preconditioner: SCHUR_JACOBI - ceres_trust_strategy: LEVENBERG_MARQUARDT - ceres_dogleg_type: TRADITIONAL_DOGLEG - ceres_loss_function: None - - # ROS Parameters - odom_frame: odom - map_frame: map - base_frame: livox_frame - scan_topic: /scan - mode: mapping #localization - - # if you'd like to immediately start continuing a map at a given pose - # or at the dock, but they are mutually exclusive, if pose is given - # will use pose - #map_file_name: test_steve - # map_start_pose: [0.0, 0.0, 0.0] - #map_start_at_dock: true - - debug_logging: false - throttle_scans: 1 - transform_publish_period: 0.02 #if 0 never publishes odometry - map_update_interval: 5.0 - resolution: 0.05 - max_laser_range: 20.0 #for rastering images - minimum_time_interval: 0.5 - transform_timeout: 0.2 - tf_buffer_duration: 30. - stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps - enable_interactive_mode: true - - # General Parameters - use_scan_matching: true - use_scan_barycenter: true - minimum_travel_distance: 0.5 - minimum_travel_heading: 0.5 - scan_buffer_size: 10 - scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 - link_scan_maximum_distance: 1.5 - loop_search_maximum_distance: 3.0 - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 - loop_match_minimum_response_fine: 0.45 - - # Correlation Parameters - Correlation Parameters - correlation_search_space_dimension: 0.5 - correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 - - # Correlation Parameters - Loop Closure Parameters - loop_search_space_dimension: 8.0 - loop_search_space_resolution: 0.05 - loop_search_space_smear_deviation: 0.03 - - # Scan Matcher Parameters - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 - minimum_angle_penalty: 0.9 - minimum_distance_penalty: 0.5 - use_response_expansion: true \ No newline at end of file diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/nav2_params.yaml b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/nav2_params.yaml deleted file mode 100644 index 0d13565..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/params/nav2_params.yaml +++ /dev/null @@ -1,371 +0,0 @@ -amcl: - ros__parameters: - use_sim_time: False - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - base_frame_id: "base_link" - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: "map" - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: "likelihood_field" - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: "odom" - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: "nav2_amcl::OmniMotionModel" - save_pose_rate: 0.5 - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - scan_topic: scan - -amcl_map_client: - ros__parameters: - use_sim_time: False - -amcl_rclcpp_node: - ros__parameters: - use_sim_time: False - -bt_navigator: - ros__parameters: - use_sim_time: False - global_frame: map - robot_base_frame: base_link - odom_topic: /Odometry - bt_loop_duration: 10 - default_server_timeout: 20 - enable_groot_monitoring: True - groot_zmq_publisher_port: 1666 - groot_zmq_server_port: 1667 - plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_back_up_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - -bt_navigator_rclcpp_node: - ros__parameters: - use_sim_time: False - -controller_server: - ros__parameters: - use_sim_time: False - controller_frequency: 5.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 - progress_checker_plugin: "progress_checker" - goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" - controller_plugins: ["FollowPath"] - - # Progress checker parameters - progress_checker: - plugin: "nav2_controller::SimpleProgressChecker" - required_movement_radius: 0.5 - movement_time_allowance: 10.0 - - # Goal checker parameters - general_goal_checker: - stateful: True - plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - # DWB parameters - FollowPath: - plugin: "dwb_core::DWBLocalPlanner" - debug_trajectory_details: False - min_vel_x: -2.5 - min_vel_y: -2.5 - max_vel_x: 2.5 - max_vel_y: 2.5 - max_vel_theta: 12.0 - min_speed_xy: -8.0 - max_speed_xy: 8.0 - min_speed_theta: -12.0 - acc_lim_x: 5.0 - acc_lim_y: 5.0 - acc_lim_theta: 15.0 - decel_lim_x: -5.0 - decel_lim_y: -5.0 - decel_lim_theta: -15.0 - vx_samples: 20 - vy_samples: 20 - vtheta_samples: 20 - sim_time: 0.51 - linear_granularity: 0.05 - angular_granularity: 0.025 - transform_tolerance: 0.2 - xy_goal_tolerance: 0.25 - trans_stopped_velocity: 0.25 - short_circuit_trajectory_evaluation: True - stateful: True - critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] - BaseObstacle.scale: 0.02 - PathAlign.scale: 32.0 - PathAlign.forward_point_distance: 0.1 - GoalAlign.scale: 24.0 - GoalAlign.forward_point_distance: 0.1 - PathDist.scale: 32.0 - GoalDist.scale: 24.0 - RotateToGoal.scale: 32.0 - RotateToGoal.slowing_factor: 5.0 - RotateToGoal.lookahead_time: -1.0 - -controller_server_rclcpp_node: - ros__parameters: - use_sim_time: False - -local_costmap: - local_costmap: - ros__parameters: - update_frequency: 20.0 - publish_frequency: 10.0 - # global_frame: odom - global_frame: map - robot_base_frame: base_link - use_sim_time: False - rolling_window: true - width: 5 - height: 5 - resolution: 0.05 - robot_radius: 0.2 - plugins: ["voxel2d_layer", "voxel3d_layer", "inflation_layer"] - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 8.0 - inflation_radius: 0.7 - voxel2d_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - max_obstacle_height: 2.0 - mark_threshold: 0 - observation_sources: scan - scan: - topic: /scan - raytrace_min_range: 0.0 - raytrace_max_range: 50.0 - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - voxel3d_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - mark_threshold: 1 - observation_sources: livox - min_obstacle_height: 0.00 - max_obstacle_height: 2.0 - livox: - topic: /livox/lidar/pointcloud - raytrace_min_range: 0.0 - raytrace_max_range: 50.0 - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" - static_layer: - map_subscribe_transient_local: True - always_send_full_costmap: True - local_costmap_client: - ros__parameters: - use_sim_time: False - local_costmap_rclcpp_node: - ros__parameters: - use_sim_time: False - -global_costmap: - global_costmap: - ros__parameters: - update_frequency: 20.0 - publish_frequency: 10.0 - global_frame: map - robot_base_frame: base_link - use_sim_time: False - robot_radius: 0.2 - resolution: 0.05 - track_unknown_space: true - plugins: ["static_layer", "voxel2d_layer", "voxel3d_layer", "inflation_layer"] - voxel2d_layer: - plugin: "nav2_costmap_2d::ObstacleLayer" - enabled: True - observation_sources: scan - scan: - topic: /scan - raytrace_min_range: 0.0 - raytrace_max_range: 50.0 - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "LaserScan" - voxel3d_layer: - plugin: "nav2_costmap_2d::VoxelLayer" - enabled: True - publish_voxel_map: False - origin_z: 0.0 - z_resolution: 0.05 - z_voxels: 16 - mark_threshold: 1 - observation_sources: livox - min_obstacle_height: 0.00 - max_obstacle_height: 2.0 - livox: - topic: /livox/lidar/pointcloud - raytrace_min_range: 0.0 - raytrace_max_range: 50.0 - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - clearing: True - marking: True - data_type: "PointCloud2" - static_layer: - plugin: "nav2_costmap_2d::StaticLayer" - map_subscribe_transient_local: True - inflation_layer: - plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 8.0 - inflation_radius: 0.7 - always_send_full_costmap: True - global_costmap_client: - ros__parameters: - use_sim_time: False - global_costmap_rclcpp_node: - ros__parameters: - use_sim_time: False - -map_server: - ros__parameters: - use_sim_time: False - yaml_filename: "" - -map_saver: - ros__parameters: - use_sim_time: False - save_map_timeout: 5.0 - free_thresh_default: 0.25 - occupied_thresh_default: 0.65 - map_subscribe_transient_local: True - -planner_server: - ros__parameters: - expected_planner_frequency: 20.0 - use_sim_time: False - planner_plugins: ["GridBased"] - GridBased: - plugin: "nav2_navfn_planner/NavfnPlanner" - tolerance: 0.5 - use_astar: false - allow_unknown: true - -# planner_server_rclcpp_node: -# ros__parameters: -# use_sim_time: False - -recoveries_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - footprint_topic: local_costmap/published_footprint - cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] - spin: - plugin: "nav2_recoveries/Spin" - backup: - plugin: "nav2_recoveries/BackUp" - wait: - plugin: "nav2_recoveries/Wait" - global_frame: odom - robot_base_frame: base_link - transform_timeout: 0.1 - use_sim_time: False - simulate_ahead_time: 2.0 - max_rotational_vel: 20.0 - min_rotational_vel: 0.4 - rotational_acc_lim: 10.0 - -robot_state_publisher: - ros__parameters: - use_sim_time: False - -waypoint_follower: - ros__parameters: - loop_rate: 20 - stop_on_failure: false - waypoint_task_executor_plugin: "wait_at_waypoint" - wait_at_waypoint: - plugin: "nav2_waypoint_follower::WaitAtWaypoint" - enabled: True - waypoint_pause_duration: 200 - -velocity_smoother: - ros__parameters: - use_sim_time: True - smoothing_frequency: 5.0 - scale_velocities: False - feedback: "CLOSED_LOOP" - max_velocity: [2.5, 2.5, 12.0] - min_velocity: [-2.5, -2.5, -12.0] - max_accel: [5.0, 5.0, 15.0] - max_decel: [-5.0, -5.0, -15.0] - odom_topic: "Odometry" - odom_duration: 0.1 - deadband_velocity: [0.0, 0.0, 0.0] - velocity_timeout: 1.0 \ No newline at end of file diff --git a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/rviz/nav2.rviz b/src/rm_nav_bringup/config/rm_navigation/rm_navigation/rviz/nav2.rviz deleted file mode 100644 index 1925af2..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/rm_navigation/rviz/nav2.rviz +++ /dev/null @@ -1,665 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /TF1/Tree1 - Splitter Ratio: 0.5833333134651184 - Tree Height: 462 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: nav2_rviz_plugins/Navigation 2 - Name: Navigation 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Alpha: 1 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - dummy: - Alpha: 1 - Show Axes: false - Show Trail: false - imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wheel_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - wheel_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: RobotModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - base_link_fake: - Value: true - dummy: - Value: true - imu_link: - Value: true - lidar_odom: - Value: true - livox_frame: - Value: true - map: - Value: true - odom: - Value: true - wheel_1: - Value: true - wheel_2: - Value: true - wheel_3: - Value: true - wheel_4: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - map: - odom: - lidar_odom: - base_link: - base_link_fake: - {} - imu_link: - {} - livox_frame: - {} - wheel_1: - {} - wheel_2: - {} - wheel_3: - {} - wheel_4: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /scan - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: voxel_grid - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/voxel_grid - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Bumper Hit - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.07999999821186066 - Style: Spheres - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /mobile_base/sensors/bumper_pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 0.6000000238418579 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: true - Enabled: true - Name: Map - Topic: - Depth: 1 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map_updates - Use Timestamp: false - Value: true - - Alpha: 1 - Class: nav2_rviz_plugins/ParticleCloud - Color: 0; 180; 0 - Enabled: true - Max Arrow Length: 0.30000001192092896 - Min Arrow Length: 0.019999999552965164 - Name: Amcl Particle Swarm - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /particle_cloud - Value: true - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Global Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/costmap - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/costmap_updates - Use Timestamp: false - Value: true - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Downsampled Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /downsampled_costmap - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /downsampled_costmap_updates - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 255; 0; 0 - Enabled: true - Head Diameter: 0.019999999552965164 - Head Length: 0.019999999552965164 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: Arrows - Radius: 0.029999999329447746 - Shaft Diameter: 0.004999999888241291 - Shaft Length: 0.019999999552965164 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /plan - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 125; 125; 125 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: VoxelGrid - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.05000000074505806 - Style: Boxes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/voxel_marked_cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /global_costmap/published_footprint - Value: false - Enabled: true - Name: Global Planner - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: costmap - Draw Behind: false - Enabled: true - Name: Local Costmap - Topic: - Depth: 1 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/costmap - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/costmap_updates - Use Timestamp: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz_default_plugins/Path - Color: 0; 12; 255 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Local Plan - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: None - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_plan - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Trajectories - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /marker - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: true - Name: Polygon - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/published_footprint - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: VoxelGrid - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /local_costmap/voxel_marked_cloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Controller - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /waypoints - Value: true - - Alpha: 1 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.07000000029802322 - Head Radius: 0.029999999329447746 - Name: PoseArray - Shaft Length: 0.23000000417232513 - Shaft Radius: 0.009999999776482582 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /teb_poses - Value: true - - Class: rviz_default_plugins/Marker - Enabled: true - Name: Marker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /teb_markers - Value: true - - Class: rviz_default_plugins/Axes - Enabled: true - Length: 0.5 - Name: base_link_fake - Radius: 0.07999999821186066 - Reference Frame: base_link_fake - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - - Class: nav2_rviz_plugins/GoalTool - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 14.485743522644043 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 3.056168556213379 - Y: 0.4070683419704437 - Z: 0.4124468266963959 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 - Target Frame: - Value: Orbit (rviz_default_plugins) - Yaw: 3.1423678398132324 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 932 - Hide Left Dock: false - Hide Right Dock: false - Navigation 2: - collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000001560000034afc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000438000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000020b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e00200032010000024e000001390000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004ad0000034a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1545 - X: 1585 - Y: 228 diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitignore b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitignore deleted file mode 100644 index 9ee021e..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -.svn -.kdev4 -*.kdev4 diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitlab-ci.yml b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitlab-ci.yml deleted file mode 100644 index 5938186..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.gitlab-ci.yml +++ /dev/null @@ -1,23 +0,0 @@ -build-kinetic: - variables: - ROSDISTRO: "kinetic" - CATKIN_WS: "/root/catkin_ws" - stage: build - image: osrf/ros:$ROSDISTRO-desktop-full - before_script: - - apt-get -qq update - - apt-get -qq install git-core python-catkin-tools curl - - mkdir -p $CATKIN_WS/src - - cp -a . $CATKIN_WS/src/ - - cd $CATKIN_WS - - rosdep update - - rosdep install -y --from-paths src --ignore-src --rosdistro $ROSDISTRO --as-root=apt:false - script: - - source /ros_entrypoint.sh - - cd $CATKIN_WS - - catkin build -i -s --no-status - only: - - kinetic-devel - tags: - - ubuntu - - docker \ No newline at end of file diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.travis.yml b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.travis.yml deleted file mode 100644 index 42b824c..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/.travis.yml +++ /dev/null @@ -1,108 +0,0 @@ -# Generic .travis.yml file for running continuous integration on Travis-CI with -# any ROS package. -# -# This installs ROS on a clean Travis-CI virtual machine, creates a ROS -# workspace, resolves all listed dependencies, and sets environment variables -# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are -# no compilation errors), and runs all the tests. If any of the compilation/test -# phases fail, the build is marked as a failure. -# -# We handle two types of package dependencies: -# - packages (ros and otherwise) available through apt-get. These are installed -# using rosdep, based on the information in the ROS package.xml. -# - dependencies that must be checked out from source. These are handled by -# 'wstool', and should be listed in a file named dependencies.rosinstall. -# -# There are two variables you may want to change: -# - ROS_DISTRO (default is indigo). Note that packages must be available for -# ubuntu 14.04 trusty. -# - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo -# root). This should list all necessary repositories in wstool format (see -# the ros wiki). If the file does not exists then nothing happens. -# -# See the README.md for more information. -# -# Author: Felix Duvallet - -# NOTE: The build lifecycle on Travis.ci is something like this: -# before_install -# install -# before_script -# script -# after_success or after_failure -# after_script -# OPTIONAL before_deploy -# OPTIONAL deploy -# OPTIONAL after_deploy - -################################################################################ - -# Use ubuntu trusty (14.04) with sudo privileges. -dist: trusty -sudo: required -language: - - generic -cache: - - apt - -# Configuration variables. All variables are global now, but this can be used to -# trigger a build matrix for different ROS distributions if desired. -env: - global: - - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] - - CI_SOURCE_PATH=$(pwd) - - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall - - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options - - ROS_PARALLEL_JOBS='-j8 -l6' - matrix: - - ROS_DISTRO=jade - - -################################################################################ - -# Install system dependencies, namely a very barebones ROS setup. -before_install: - - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" - - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - - sudo apt-get update -qq - - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin - - source /opt/ros/$ROS_DISTRO/setup.bash - # Prepare rosdep to install dependencies. - - sudo rosdep init - - rosdep update - -# Create a catkin workspace with the package under integration. -install: - - mkdir -p ~/catkin_ws/src - - cd ~/catkin_ws/src - - catkin_init_workspace - # Create the devel/setup.bash (run catkin_make with an empty workspace) and - # source it to set the path variables. - - cd ~/catkin_ws - - catkin_make - - source devel/setup.bash - # Add the package under integration to the workspace using a symlink. - - cd ~/catkin_ws/src - - ln -s $CI_SOURCE_PATH . - -# Install all dependencies, using wstool and rosdep. -# wstool looks for a ROSINSTALL_FILE defined in the environment variables. -before_script: - # source dependencies: install using wstool. - - cd ~/catkin_ws/src - - wstool init - - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi - - wstool up - # package depdencies: install using rosdep. - - cd ~/catkin_ws - - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO - -# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to -# catkin_make. -script: - - cd ~/catkin_ws - - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) - # Testing: Use both run_tests (to see the output) and test (to error out). - - catkin_make run_tests # This always returns 0, but looks pretty. - - catkin_make test # This will return non-zero if a test fails. - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/README.md b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/README.md deleted file mode 100644 index 5eb9dbd..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/README.md +++ /dev/null @@ -1,61 +0,0 @@ -teb_local_planner ROS Package -============================= - -The teb_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. -The underlying method called Timed Elastic Band locally optimizes the robot's trajectory with respect to trajectory execution time, -separation from obstacles and compliance with kinodynamic constraints at runtime. - -Refer to http://wiki.ros.org/teb_local_planner for more information and tutorials. - -Build status of the *melodic-devel* branch: -- ROS Buildfarm (Melodic): [![Melodic Status](http://build.ros.org/buildStatus/icon?job=Mdev__teb_local_planner__ubuntu_bionic_amd64)](http://build.ros.org/job/Mdev__teb_local_planner__ubuntu_bionic_amd64/) - -### Port to ROS2 -This branch is the teb_local_planner package ported to ROS2(Dashing Diademata). Currently, it is currently compatible with [Navigation2(master branch)](https://github.com/ros-planning/navigation2/tree/master)([226f06c](https://github.com/ros-planning/navigation2/commit/226f06ce282c727ca240ce8be0cb4b093e26343b)). You can test teb_local_planner with Navigation2 and TurtleBot3 simulation by launching the following command. -``` -ros2 launch teb_local_planner teb_tb3_simulation_launch.py -``` - -## Citing the Software - -*Since a lot of time and effort has gone into the development, please cite at least one of the following publications if you are using the planner for your own research:* - -- C. Rösmann, F. Hoffmann and T. Bertram: Integrated online trajectory planning and optimization in distinctive topologies, Robotics and Autonomous Systems, Vol. 88, 2017, pp. 142–153. -- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Trajectory modification considering dynamic constraints of autonomous robots. Proc. 7th German Conference on Robotics, Germany, Munich, May 2012, pp 74–79. -- C. Rösmann, W. Feiten, T. Wösch, F. Hoffmann and T. Bertram: Efficient trajectory optimization using a sparse model. Proc. IEEE European Conference on Mobile Robots, Spain, Barcelona, Sept. 2013, pp. 138–143. -- C. Rösmann, F. Hoffmann and T. Bertram: Planning of Multiple Robot Trajectories in Distinctive Topologies, Proc. IEEE European Conference on Mobile Robots, UK, Lincoln, Sept. 2015. -- C. Rösmann, F. Hoffmann and T. Bertram: Kinodynamic Trajectory Optimization and Control for Car-Like Robots, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, Sept. 2017. - -Buy Me A Coffee - -## Videos - -The left of the following videos presents features of the package and shows examples from simulation and real robot situations. -Some spoken explanations are included in the audio track of the video. -The right one demonstrates features introduced in version 0.2 (supporting car-like robots and costmap conversion). Please watch the left one first. - - - - -## License - -The *teb_local_planner* package is licensed under the BSD license. -It depends on other ROS packages, which are listed in the package.xml. They are also BSD licensed. - -Some third-party dependencies are included that are licensed under different terms: - - *Eigen*, MPL2 license, http://eigen.tuxfamily.org - - *libg2o* / *g2o* itself is licensed under BSD, but the enabled *csparse_extension* is licensed under LGPL3+, - https://github.com/RainerKuemmerle/g2o. [*CSparse*](http://www.cise.ufl.edu/research/sparse/CSparse/) is included as part of the *SuiteSparse* collection, http://www.suitesparse.com. - - *Boost*, Boost Software License, http://www.boost.org - -All packages included are distributed in the hope that they will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details. - -## Requirements - -Install dependencies (listed in the *package.xml* and *CMakeLists.txt* file) using *rosdep*: - - rosdep install teb_local_planner - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CHANGELOG.rst b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CHANGELOG.rst deleted file mode 100644 index 11b4f73..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CHANGELOG.rst +++ /dev/null @@ -1,407 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package teb_local_planner -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.9.1 (2020-05-29) ------------------- -* Fixed RobotFootprintModel visualization bug (thanks to Anson Wang) -* Reserve the size of the relevant obstacles vector to avoid excessive memory allocations (thanks to João Monteiro) -* CMake: Removed system include to avoid compiling issues on some platforms -* Contributors: Anson Wang, Christoph Rösmann, João Carlos Espiúca Monteiro - -0.9.0 (2020-05-26) ------------------- -* Added pill resp. stadium-shaped obstacle -* Changed minimum CMake version to 3.1 -* Improved efficiency of 3d h-signature computation -* Changed default value for parameter penalty_epsilon to 0.05 -* Improved efficiency of findClosedTrajectoryPose() -* Removed obsolete method isHorizonReductionAppropriate() -* Contributors: Christoph Rösmann, XinyuKhan - -0.8.4 (2019-12-02) ------------------- -* Fixed TEB autoResize if last TimeDiff is small -* Add a rotational threshold for identifying a warm start goal -* Contributors: Rainer Kümmerle - -0.8.3 (2019-10-25) ------------------- -* Limiting the control look-ahead pose to the first that execeeds the expected look-ahead time (thanks to Marco Bassa) -* test_optim_node fix circular obstacles (thanks to dtaranta) -* Fix shadow variable warning (thanks to Victor Lopez) -* Use SYSTEM when including external dependencies headers (thanks to Victor Lopez) -* Robustify initTrajectoryToGoal if a plan is given (thanks to Rainer Kuemmerle) -* Adding the option to shift ahead the target pose used to extract the velocity command (thanks to Marco Bassa) -* Fixed segfault in optimal_planner.cpp when clearing graph with unallocated optimizer. - Fixes `#158 `_. -* On footprintCost, fail only if footprint is in collision, not outside the map or on unknown space (thanks to corot) -* Native MoveBaseFlex support added: Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces (thanks to corot) -* added warning if parameter optimal_time is <= 0 -* Nonlinear obstacle cost from EdgeInflatedObstacle also added to EdgeObstacle. - See `#140 `_. -* Fixed proper initialization of parameter obstacle_cost_exponent in case it is not loaded from the parameter server -* Contributors: Christoph Rösmann, Marco Bassa, Rainer Kuemmerle, Victor Lopez, corot, dtaranta - -0.8.2 (2019-07-02) ------------------- -* Allow scripts to be executable and usable by rosrun after catkin_make install and through the catkin release process (thanks to Devon Ash) -* Add nonlinear part to obstacle cost to improve narrow gap behavior. - Parameter `obstacle_cost_exponent` defines the exponent of the nonlinear cost term. - The default linear behavior is achieved by setting this parameter to 1 (default). - A value of 4 performed well in some tests and experiments (thanks to Howard Cochran). -* Parameter `global_plan_prune_distance` added via ros parameter server. -* Fixed SIGSEGV in optimizeAllTEBs() if main thread is interrupted by boost (thanks to Howard Cochran) -* Fixed SIGSEGV crash in deleteTebDetours() (thanks to Howard Cochran) -* On footprint visualization, avoid overshadowing by obstacles (thanks to corot) -* Do not ignore robot model on the association stage. - Important mostly for polygon footprint model (thanks to corot). -* Adjustable color for footprint visualization -* Showing (detected) infeasible robot poses in a separate marker namespace and color -* Added edge for minimizing Euclidean path length (parameter: `weight_shortest_path`) -* Ackermann steering conversion (python script): fixed direction inversion in backwards mode when `cmd_angle_instead_rotvel` is true (thanks to Tobi Loew) -* Fixed wrong skipping condition in AddEdgesKinematicsCarlike() (thanks to ShiquLIU) -* Never discarding the previous best teb in renewAndAnalyzeOldTebs (thanks to Marco Bassa) -* Allowing for the fallback to a different trajectory when the costmap check fails. This prevents the switch to unfeasible trajectories (thanks to Marco Bassa). -* Skipping the generation of the homotopy exploration graph in case the maximum number of allowed classes is reached (thanks to Marco Bassa) -* Changed isTrajectoryFeasible function to allow for a more accurate linear and angular discretization (thanks to Marco Bassa) -* Function TebOptimalPlanner::computeError() considers now the actual optimizer weights. - As a result, the default value of `selection_obst_cost_scale` is reduced (thanks to Howard Cochran). -* update to use non deprecated pluginlib macro (thanks to Mikael Arguedas) -* Avoiding h signature interpolation between coincident poses (thanks to Marco Bassa) -* New strategy for the deletion of detours: Detours are now determined w.r.t. the least-cost alternative and not w.r.t. just the goal heading. - Deletion of additional alternatives applies if either an initial backward motion is detected, if the transition time is much bigger than the duration of the best teb - and if a teb cannot be optimized (thanks to Marco Bassa). - Optionally allowing the usage of the initial plan orientation when initializing new tebs. -* Contributors: Christoph Rösmann, Mikael Arguedas, Devon Ash, Howard Cochran, Marco Bassa, ShiquLIU, Tobi Loew, corot - -0.8.1 (2018-08-14) ------------------- -* bugfix in calculateHSignature. Fixes `#90 `_. -* fixed centroid computation in a special case of polygon-obstacles -* Contributors: Christoph Rösmann - -0.8.0 (2018-08-06) ------------------- -* First melodic release -* Updated to new g2o API -* Migration to tf2 -* Contributors: Christoph Rösmann - -0.7.3 (2018-07-05) ------------------- -* Parameter `switching_blocking_period` added to homotopy class planner parameter group. - Values greater than zero enforce the homotopy class planner to only switch to new equivalence classes as soon - as the given period is expired (this might reduce oscillations in some scenarios). The value is set to zero seconds - by default in order to not change the behavior of existing configurations. -* Fixed unconsistent naming of parameter `global_plan_viapoint_sep`. - The parameter retrieved at startup was `global_plan_via_point_sep` and via dynamic_reconfigure it was `global_plan_viapoint_sep`. - `global_plan_via_point_sep` has now been replaced by `global_plan_viapoint_sep` since this is more consistent with the variable name - in the code as well as `weight_viapoint` and the ros wiki description. - In order to not break things, the old parameter name can still be used. However, a deprecated warning is printed. -* transformGlobalPlan searches now for the closest point within the complete subset of the global plan in the local costmap: - In every sampling interval, the global plan is processed in order to find the closest pose to the robot (as reference start) - and the current end pose (either local at costmap boundary or max_global_plan_lookahead_dist). - Previously, the search algorithm stopped as soon as the distance to the robot increased once. - This caused troubles with more complex global plans, hence the new strategy checks the complete subset - of the global plan in the local costmap for the closest distance to the robot. -* via-points that are very close to the current robot pose or behind the robot are now skipped (in non-ordered mode) -* Edge creation: minor performance improvement for dynamic obstacle edges -* dynamic_reconfigure: parameter visualize_with_time_as_z_axis_scale moved to group trajectory -* Contributors: Christoph Rösmann - -0.7.2 (2018-06-08) ------------------- -* Adds the possibility to provide via-points via a topic. - Currently, the user needs to decide whether to receive via-points from topic or to obtain them from the global reference plan - (e.g., activate the latter by setting global_plan_viapoint_sep>0 as before). - A small test script publish_viapoints.py is provided to demonstrate the feature within test_optim_node. -* Contributors: Christoph Rösmann - -0.7.1 (2018-06-05) ------------------- -* Fixed a crucial bug (from 0.6.6): A cost function for prefering a clockwise resp. anti-clockwise turn was enabled by default. - This cost function was only intended to be active only for recovering from an oscillating robot. - This cost led to a penalty for one of the turning directions and hence the maximum turning rate for the penalized direction could not be reached. - Furthermore, which is more crucial: since the penalty applied only to a small (initial) subset of the trajectory, the overall control performance was poor - (huge gap between planned motion and closed-loop trajectories led to frequent corrections of the robot pose and hence many motion reversals). -* Adds support for circular obstacle types. This includes support for the radius field in costmap_converter::ObstacleMsg -* rqt reconfigure: parameters are now grouped in tabs (robot, trajectory, viapoints, ...) -* Update to use non deprecated pluginlib macro -* Python scripts updated to new obstacle message definition. -* Fixed issue when start and end are at the same location (PR #43) -* Normalize marker quaternions in *test_optim_node* -* Contributors: Christoph Rösmann, Alexander Reimann, Mikael Arguedas, wollip - -0.7.0 (2017-09-23) ------------------- -* This update introduces support for dynamic obstacles (thanks to Franz Albers, who implemented and tested the code). - Dynamic obstacle support requires parameter *include\_dynamic\_obstacles* to be activated. - Note, this feature is still experimental and subject to testing. - Motion prediction is performed using a constant velocity model. - Dynamic obstacles might be incorporated as follows: - * via a custom message provided on topic ~/obstacles (warning: we changed the message type from teb_local_planner/ObstacleMsg to costmap_converter/ObstacleArrayMsg). - * via the CostmapToDynamicObstacles plugin as part of the costmap\_converter package (still experimental). - A tutorial is going to be provided soon. -* FeedbackMsg includes a ObstacleMsg instead of a polygon -* ObstacleMsg removed from package since it is now part of the costmap\_converter package. -* Homotopy class planer code update: graph search methods and equivalence classes (h-signatures) are now - implemented as subclasses of more general interfaces. -* TEB trajectory initialization now uses a max\_vel\_x argument instead of the desired time difference in order to give the optimizer a better warm start. - Old methods are marked as deprecated. This change does not affect users settings. -* Inplace rotations removed from trajectory initialization to improve convergence speed of the optimizer -* teb\_local\_planner::ObstacleMsg removed in favor of costmap\_converter::ObstacleArrayMsg. This also requires custom obstacle publishers to update to the new format -* the "new" trajectory resizing method is only activated, if "include_dynamic_obstacles" is set to true. - We introduced the non-fast mode with the support of dynamic obstacles - (which leads to better results in terms of x-y-t homotopy planning). - However, we have not yet tested this mode intensively, so we keep - the previous mode as default until we finish our tests. -* added parameter and code to update costmap footprint if it is dynamic (#49) -* Contributors: Franz Albers, Christoph Rösmann, procopiostein - -0.6.6 (2016-12-23) ------------------- -* Strategy for recovering from oscillating local plans added (see new parameters) -* Horizon reduction for resolving infeasible trajectories is not activated anymore if the global goal is already selected - (to avoid oscillations due to changing final orientations) -* Global plan orientations are now taken for TEB initialization if lobal_plan_overwrite_orientation==true -* Parameter max_samples added -* Further fixes (thanks to Matthias Füller and Daniel Neumann for providing patches) - -0.6.5 (2016-11-15) ------------------- -* The trajectory is now initialized backwards for goals close to and behind the robot. - Parameter 'allow_init_with_backwards_motion' added. -* Updated the TEB selection in the HomotopyClassPlanner. - * A new parameter is introduced to prefer the equivalence class of the initial plan - * Fixed some bugs related to the deletion of candidates and for keeping the equivalence class of the initial plan. -* Weight adaptation added for obstacles edges. - Added parameter 'weight_adapt_factor'. - Obstacle weights are repeatedly scaled by this factor in each outer TEB iteration. - Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions. -* Added a warning if the optim footprint + min_obstacle_dist is smaller than the costmap footprint. - Validation is performed by only comparing the inscribed radii of the footprints. -* Revision/extension of the reduced-horizon backup mode which is triggered in case infeasible trajectories are detected. -* Changed HSignature to a generic equivalence class -* Minor changes - -0.6.4 (2016-10-23) ------------------- -* New default obstacle association strategy: - During optimization graph creation, for each pose of the trajectory a - relevance detection is performed before considering the obstacle - during optimization. New parameters are introduced. The - old strategy is kept as 'legacy' strategy (see parameters). -* Computation of velocities, acceleration and turning radii extended: - Added an option to compute the actual arc length - instead of using the Euclidean distance approximation (see parameter `exact_arc_length`. -* Added intermediate edge layer for unary, binary and multi edges in order to reduce code redundancy. -* Script for visualizing velocity profile updated to accept the feedback topic name via rosparam server -* Removed TebConfig dependency in TebVisualization -* PolygonObstacle can now be constructed using a vertices container -* HomotopyClassPlanner public interface extended -* Changed H-Signature computation to work 'again' with few obstacles such like 1 or 2 -* Removed inline flags in visualization.cpp -* Removed inline flags in timed_elastic_band.cpp. - Fixes `#15 `_. -* Increased bounds of many variables in dynamic_reconfigure. - Resolves `#14 `_. - The particular variables are maximum velocities, maximum accelerations, - minimum turning radius,... - Note: optimization weights and dt_ref as well as dt_hyst are not - tuned for velocities and accelerations beyond - the default values (e.g. >1 m/s). Just increasing the maximum velocity - bounds without adjusting the other parameters leads to an insufficient behavior. -* Default parameter value update: 'costmap_obstacles_behind_robot_dist' -* Additional minor fixes. - -0.6.3 (2016-08-17) ------------------- -* Changed the f0 function for calculating the H-Signature. - The new one seems to be more robust for a much larger number of obstacles - after some testing. -* HomotopyClassPlanner: vertex collision check removed since collisions will be determined in the edge collision check again -* Fixed distance calculation polygon-to-polygon-obstacle -* cmake config exports now *include directories* of external packages for dependent projects -* Enlarged upper bounds on goal position and orientation tolerances in *dynamic_reconfigure*. Fixes #13. - - -0.6.2 (2016-06-15) ------------------- -* Fixed bug causing the goal to disappear in case the robot arrives with non-zero orientation error. -* Inflation mode for obstacles added. -* The homotopy class of the global plan is now always forced to be initialized as trajectory. -* The initial velocity of the robot is now taken into account correctly for - all candidate trajectories. -* Removed a check in which the last remaining candidate trajectory was rejected if it was close to an obstacle. - This fix addresses issue `#7 `_ - -0.6.1 (2016-05-23) ------------------- -* Debian ARM64 library path added to SuiteSparse cmake find-script (resolves ARM compilation issue) - - -0.6.0 (2016-05-22) ------------------- -* Extended support to holonomic robots -* Wrong parameter namespace for *costmap_converter* plugins fixed -* Added the option to scale the length of the hcp sampling area -* Compiler warnings fixed. -* Workaround for compilation issues that are caused by a bug in boost 1.58 - concerning the graph library (missing move constructor/assignment operator - in boost source). -* Using *tf_listener* from *move_base* now. -* Via-point support improved. - Added the possibility to take the actual order of via-points into account. - Additionally, via-points beyond start and goal are now included. -* Obsolete include of the angles package header removed -* Update to package.xml version 2 -* Some other minor fixes. - - -0.4.0 (2016-04-19) ------------------- -* The teb_local_planner supports a path-following mode (w.r.t. the global plan) and via-points now. - This allows the user to adapt the tradeoff between time-optimality and path-following. - Check out the new tutorial: "Following the Global Plan (Via-Points)". -* All external configuration and launch files are removed, since they are part - of the new teb_local_planner_tutorials package. - - -0.3.1 (2016-04-14) ------------------- -* Fixed wrong coordinate transformation in 'line' and 'polygon' footprint models. -* Trajectory selection strategy in case of multiple topologies updated: - * The obstacle costs for selection can now be scaling separately. - * The cost regarding time optimality can now be replaced by the actual transition time. - * Added a hysteresis to cost comparison between a new and the previously selected trajectory. - * In the default parameter setting the strategy is similar to release 0.3.0. -* Warning message removed that occured if an odom message with only zeros was received. - - -0.3.0 (2016-04-08) ------------------- -* Different/custom robot footprints are now supported and subject to optimization (refer to the new tutorial!). -* The new robot footprint is also visualized using the common marker topic. -* The strategy of taking occupied costmap cells behind the robot into account has been improved. - These changes significantly improve navigation close to walls. -* Parameter 'max_global_plan_lookahead_dist' added. - Previously, the complete subset of the global plan contained in the local costmap - was taken into account for choosing the current intermediate goal point. With this parameter, the maximum - length of the reference global plan can be limited. The actual global plan subset - is now computed using the logical conjunction of both local costmap size and 'max_global_plan_lookahead_dist'. -* Bug fixes: - * Fixed a compilation issue on ARM architectures - * If custom obstacles are used, the container with old obstacles is now cleared properly. -* Parameter cleanup: - * "weight_X_obstacle" parameters combined to single parameter "weight_obstacle". - * "X_obstacle_poses_affected" parameters combined to single parameter "obstacle_poses_affected". - * Deprecated parameter 'costmap_emergency_stop_dist' removed. -* Code cleanup - - -0.2.3 (2016-02-01) ------------------- -* Marker lifetime changed -* In case the local planner detects an infeasible trajectory it does now try to - reduce the horizon to 50 percent of the length. The trajectory is only reduced - if some predefined cases are detected. - This mechanism constitutes a backup behavior. -* Improved carlike robot support. - Instead of commanding the robot using translational and rotational velocities, - the robot might also be commanded using the transl. velocity and steering angle. - Appropriate parameters are added to the config. -* Changed default parameter for 'h_signature_threshold' from 0.01 to 0.1 to better match the actual precision. -* Some python scripts for data conversion added -* Minor other changes - -0.2.2 (2016-01-11) ------------------- -* Carlike robots (ackermann steering) are supported from now on (at least experimentally) - by specifying a minimum bound on the turning radius. - Currently, the output of the planner in carlike mode is still (v,omega). - Since I don't have any real carlike robot, I would be really happy if someone could provide me with - some feedback to further improve/extend the support. -* Obstacle cost function modified to avoid undesired jerks in the trajectory. -* Added a feedback message that contains current trajectory information (poses, velocities and temporal information). - This is useful for analyzing and debugging the velocity profile e.g. at runtime. - The message will be published only if it's activated (rosparam). - A small python script is added to plot the velocity profile (while *test_optim_node* runs). -* Cost functions are now taking the direction/sign of the translational velocity into account: - Specifying a maximum backwards velocity other than forward velocity works now. - Additionally, the change in acceleration is now computed correctly if the robot switches directions. -* The global plan is now pruned such that already passed posses are cut off - (relevant for global planners with *planning_rate=0*). -* Fixed issue#1: If a global planner with *planning_rate=0* was used, - a TF timing/extrapolation issue appeared after some time. -* The planner resets now properly if the velocity command cannot be computed due to invalid optimization results. - - -0.2.1 (2015-12-30) ------------------- -* This is an important bugfix release. -* Fixed a major issue concerning the stability and performance of the optimization process. Each time the global planner was updating the global plan, the local planner was resetted completely even if - the updated global plan did not differ from the previous one. This led to stupid reinitializations and a slighly jerky behavior if the update rate of the global planner was high (each 0.5-2s). - From now on the local planner is able to utilize the global plan as a warm start and determine automatically whether to reinitialize or not. -* Support for polygon obstacles extended and improved (e.g. the homotopy class planner does now compute actual distances to the polygon rather than utilizing the distance to the centroid). - -0.2.0 (2015-12-23) ------------------- -* The teb_local_planner supports costmap_converter plugins (pluginlib) from now on. Those plugins convert occupied costmap2d cells into polygon shapes. - The costmap_converter is disabled by default, since the extension still needs to be tested (parameter choices, computation time advantages, etc.). - A tutorial will explain how to activate the converter using the ros-param server. - -0.1.11 (2015-12-12) -------------------- -* This is a bugfix release (it fixes a lot of issues which occured frequently when the robot was close to the goal) - -0.1.10 (2015-08-13) -------------------- -* The optimizer copies the global plan as initialization now instead of using a simple straight line approximation. -* Some bugfixes and improvements - -0.1.9 (2015-06-24) ------------------- -* Fixed a segmentation fault issue. This minor update is crucial for stability. - -0.1.8 (2015-06-08) ------------------- -* Custom obstacles can be included via publishing dedicated messages -* Goal-reached-condition also checks orientation error (desired yaw) now -* Numerical improvements of the h-signature calculation -* Minor bugfixes - -0.1.7 (2015-05-22) ------------------- -* Finally fixed saucy compilation issue by retaining compatiblity to newer distros - (my "new" 13.10 VM helps me to stop spamming new releases for testing). - -0.1.6 (2015-05-22) ------------------- -* Fixed compilation errors on ubuntu saucy caused by different FindEigen.cmake scripts. - I am not able to test releasing on saucy, forcing me to release again and again. Sorry. - -0.1.5 (2015-05-21) ------------------- -* Added possibility to dynamically change parameters of test_optim_node using dynamic reconfigure. -* Fixed a wrong default-min-max tuple in the dynamic reconfigure config. -* Useful config and launch files are now added to cmake install. -* Added install target for the test_optim_node executable. - -0.1.4 (2015-05-20) ------------------- -* Fixed compilation errors on ROS Jade - -0.1.3 (2015-05-20) ------------------- -* Fixed compilation errors on ubuntu saucy - -0.1.2 (2015-05-19) ------------------- -* Removed unused include that could break compilation. - -0.1.1 (2015-05-19) ------------------- -* All files added to the indigo-devel branch -* Initial commit -* Contributors: Christoph Rösmann diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CMakeLists.txt b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CMakeLists.txt deleted file mode 100644 index 43859bc..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/CMakeLists.txt +++ /dev/null @@ -1,162 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(teb_local_planner) - -# Set to Release in order to speed up the program significantly -set(CMAKE_BUILD_TYPE Release) #None, Debug, Release, RelWithDebInfo, MinSizeRel - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -#if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") -# add_compile_options(-Wall -Wextra -Wpedantic) -#endif() - -## Find catkin macros and libraries -find_package(ament_cmake REQUIRED) -find_package(costmap_converter REQUIRED) -find_package(dwb_critics REQUIRED) -find_package(nav_2d_utils REQUIRED) -find_package(nav2_core REQUIRED) -find_package(nav2_costmap_2d REQUIRED) -find_package(nav2_util REQUIRED) -#find_package(interactive_markers REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_action REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(std_msgs REQUIRED) -find_package(pluginlib REQUIRED) -find_package(teb_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_eigen REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(visualization_msgs REQUIRED) -find_package(builtin_interfaces REQUIRED) - -message(STATUS "System: ${CMAKE_SYSTEM}") -## System dependencies are found with CMake's conventions -SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake_modules) -message(STATUS "${CMAKE_MODULE_PATH}") -find_package(SUITESPARSE REQUIRED) -find_package(G2O REQUIRED) -find_package(Boost REQUIRED) - -# Eigen3 FindScript Backward compatibility (ubuntu saucy) -# Since FindEigen.cmake is deprecated starting from jade. -if (EXISTS "FindEigen3.cmake") - find_package(Eigen3 REQUIRED) - set(Eigen_INCLUDE_DIRS ${Eigen3_INCLUDE_DIRS}) -elseif (EXISTS "FindEigen.cmake") - find_package(Eigen REQUIRED) -elseif (EXISTS "FindEigen.cmake") - message(WARNING "No findEigen cmake script found. You must provde one of them, - e.g. by adding it to ${PROJECT_SOURCE_DIR}/cmake_modules.") -endif (EXISTS "FindEigen3.cmake") - -set(EXTERNAL_INCLUDE_DIRS ${Eigen_INCLUDE_DIRS} ${SUITESPARSE_INCLUDE_DIRS} ${G2O_INCLUDE_DIR}) -set(EXTERNAL_LIBS ${Boost_LIBRARIES} ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES}) - -########### -## Build ## -########### - -include_directories( - include - ${Boost_INCLUDE_DIRS} - ${EXTERNAL_INCLUDE_DIRS} -) - -## Build the teb_local_planner library - -add_library(teb_local_planner SHARED - src/timed_elastic_band.cpp - src/optimal_planner.cpp - src/obstacles.cpp - src/visualization.cpp - src/recovery_behaviors.cpp - src/teb_config.cpp - src/homotopy_class_planner.cpp - src/teb_local_planner_ros.cpp - src/graph_search.cpp -) - -set(ament_dependencies - costmap_converter - dwb_critics - nav_2d_utils - nav2_core - nav2_costmap_2d - nav2_util - geometry_msgs - nav_msgs - rclcpp - rclcpp_action - rclcpp_lifecycle - std_msgs - pluginlib - teb_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - visualization_msgs - builtin_interfaces -) -ament_target_dependencies(teb_local_planner - ${ament_dependencies} -) -target_link_libraries(teb_local_planner - ${EXTERNAL_LIBS} -) - -target_compile_definitions(teb_local_planner PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - -############# -## Install ## -############# - -## Mark executables and/or libraries for installation -install(TARGETS teb_local_planner - DESTINATION lib -) - -## Mark cpp header files for installation -install(DIRECTORY include/ - DESTINATION include/ -) - -## Mark other files for installation (e.g. launch and bag files, etc.) -install(FILES - teb_local_planner_plugin.xml - DESTINATION share - ) - -install(PROGRAMS scripts/cmd_vel_to_ackermann_drive.py DESTINATION lib/${PROJECT_NAME}) - -install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) - -ament_export_include_directories(include) -ament_export_libraries(teb_local_planner) -ament_export_dependencies(${ament_dependencies}) -pluginlib_export_plugin_description_file(nav2_core teb_local_planner_plugin.xml) - -############# -## Testing ## -############# - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - #ament_add_gtest(homotopy_class_planner_test - # test/homotopy_class_planner_test.cpp - #) - #target_link_libraries(homotopy_class_planner_test - # teb_local_planner - #) -endif() - -ament_package() diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/LICENSE b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/LICENSE deleted file mode 100644 index d4658cc..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/LICENSE +++ /dev/null @@ -1,28 +0,0 @@ -Copyright (c) 2016, TU Dortmund - Lehrstuhl RST -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of teb_local_planner nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg deleted file mode 100755 index f403d7e..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cfg/TebLocalPlannerReconfigure.cfg +++ /dev/null @@ -1,420 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import * -#from local_planner_limits import add_generic_localplanner_params - -gen = ParameterGenerator() - -# This unusual line allows to reuse existing parameter definitions -# that concern all localplanners -#add_generic_localplanner_params(gen) - -# For integers and doubles: -# Name Type Reconfiguration level -# Description -# Default Min Max - - -grp_trajectory = gen.add_group("Trajectory", type="tab") - -# Trajectory -grp_trajectory.add("teb_autosize", bool_t, 0, - "Enable the automatic resizing of the trajectory during optimization (based on the temporal resolution of the trajectory, recommended)", - True) - -grp_trajectory.add("dt_ref", double_t, 0, - "Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)", - 0.3, 0.01, 1) - -grp_trajectory.add("dt_hysteresis", double_t, 0, - "Hysteresis that is utilized for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref", - 0.1, 0.002, 0.5) - -grp_trajectory.add("global_plan_overwrite_orientation", bool_t, 0, - "Some global planners are not considering the orientation at local subgoals between start and global goal, therefore determine it automatically", - True) - -grp_trajectory.add("allow_init_with_backwards_motion", bool_t, 0, - "If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors)", - False) - -grp_trajectory.add("max_global_plan_lookahead_dist", double_t, 0, - "Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if 0 or negative: disabled; the length is also bounded by the local costmap size]", - 3.0, 0, 50.0) - -grp_trajectory.add("force_reinit_new_goal_dist", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting)", - 1.0, 0.0, 10.0) - -grp_trajectory.add("force_reinit_new_goal_angular", double_t, 0, - "Force the planner to reinitialize the trajectory if a previous goal is updated with a rotational difference of more than the specified value in radians (skip hot-starting)", - 0.78, 0.0, 4.0) - -grp_trajectory.add("feasibility_check_no_poses", int_t, 0, - "Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked.", - 5, -1, 50) - -grp_trajectory.add("feasibility_check_lookahead_distance", double_t, 0, - "Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked.", - -1, -1, 20) - -grp_trajectory.add("exact_arc_length", bool_t, 0, - "If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used.", - False) - -grp_trajectory.add("publish_feedback", bool_t, 0, - "Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes)", - False) - -grp_trajectory.add("visualize_with_time_as_z_axis_scale", double_t, 0, - "If this value is bigger than 0, the trajectory and obstacles are visualized in 3d using the time as the z-axis scaled by this value. Most useful for dynamic obstacles.", - 0, 0, 1) - -# ViaPoints -grp_viapoints = gen.add_group("ViaPoints", type="tab") - -grp_viapoints.add("global_plan_viapoint_sep", double_t, 0, - "Min. separation between each two consecutive via-points extracted from the global plan [if negative: disabled]", - -0.1, -0.1, 5.0) - -grp_viapoints.add("via_points_ordered", bool_t, 0, - "If true, the planner adheres to the order of via-points in the storage container", - False) - -# Robot -grp_robot = gen.add_group("Robot", type="tab") - -grp_robot.add("max_vel_x", double_t, 0, - "Maximum translational velocity of the robot", - 0.4, 0.01, 100) - -grp_robot.add("max_vel_x_backwards", double_t, 0, - "Maximum translational velocity of the robot for driving backwards", - 0.2, 0.01, 100) - -grp_robot.add("max_vel_theta", double_t, 0, - "Maximum angular velocity of the robot", - 0.3, 0.01, 100) - -grp_robot.add("acc_lim_x", double_t, 0, - "Maximum translational acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("acc_lim_theta", double_t, 0, - "Maximum angular acceleration of the robot", - 0.5, 0.01, 100) - -grp_robot.add("is_footprint_dynamic", bool_t, 0, - "If true, updated the footprint before checking trajectory feasibility", - False) - -grp_robot.add("use_proportional_saturation", bool_t, 0, - "If true, reduce all twists components (linear x and y, and angular z) proportionally if any exceed its corresponding bounds, instead of saturating each one individually", - False) -grp_robot.add("transform_tolerance", double_t, 0, - "Tolerance when querying the TF Tree for a transformation (seconds)", - 0.5, 0.001, 20) - -# Robot/Carlike - -grp_robot_carlike = grp_robot.add_group("Carlike", type="hide") - -grp_robot_carlike.add("min_turning_radius", double_t, 0, - "Minimum turning radius of a carlike robot (diff-drive robot: zero)", - 0.0, 0.0, 50.0) - -grp_robot_carlike.add("wheelbase", double_t, 0, - "The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots!", - 1.0, -10.0, 10.0) - -grp_robot_carlike.add("cmd_angle_instead_rotvel", bool_t, 0, - "Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance')", - False) - -# Robot/Omni - -grp_robot_omni = grp_robot.add_group("Omnidirectional", type="hide") - -grp_robot_omni.add("max_vel_y", double_t, 0, - "Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)", - 0.0, 0.0, 100) - -grp_robot_omni.add("acc_lim_y", double_t, 0, - "Maximum strafing acceleration of the robot", - 0.5, 0.01, 100) - -# GoalTolerance -grp_goal = gen.add_group("GoalTolerance", type="tab") - -grp_goal.add("free_goal_vel", bool_t, 0, - "Allow the robot's velocity to be nonzero for planning purposes (the robot can arrive at the goal with max speed)", - False) - -# Obstacles -grp_obstacles = gen.add_group("Obstacles", type="tab") - -grp_obstacles.add("min_obstacle_dist", double_t, 0, - "Minimum desired separation from obstacles", - 0.5, 0, 10) - -grp_obstacles.add("inflation_dist", double_t, 0, - "Buffer zone around obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", - 0.6, 0, 15) - -grp_obstacles.add("dynamic_obstacle_inflation_dist", double_t, 0, - "Buffer zone around predicted locations of dynamic obstacles with non-zero penalty costs (should be larger than min_obstacle_dist in order to take effect)", - 0.6, 0, 15) - -grp_obstacles.add("include_dynamic_obstacles", bool_t, 0, - "Specify whether the movement of dynamic obstacles should be predicted by a constant velocity model (this also changes the homotopy class search). If false, all obstacles are considered to be static.", - False) - -grp_obstacles.add("include_costmap_obstacles", bool_t, 0, - "Specify whether the obstacles in the costmap should be taken into account directly (this is necessary if no seperate clustering and detection is implemented)", - True) - -grp_obstacles.add("legacy_obstacle_association", bool_t, 0, - "If true, the old association strategy is used (for each obstacle, find the nearest TEB pose), otherwise the new one (for each teb pose, find only 'relevant' obstacles).", - False) - -grp_obstacles.add("obstacle_association_force_inclusion_factor", double_t, 0, - "The non-legacy obstacle association technique tries to connect only relevant obstacles with the discretized trajectory during optimization, all obstacles within a specifed distance are forced to be included (as a multiple of min_obstacle_dist), e.g. choose 2.0 in order to consider obstacles within a radius of 2.0*min_obstacle_dist.", - 1.5, 0.0, 100.0) - -grp_obstacles.add("obstacle_association_cutoff_factor", double_t, 0, - "See obstacle_association_force_inclusion_factor, but beyond a multiple of [value]*min_obstacle_dist all obstacles are ignored during optimization. obstacle_association_force_inclusion_factor is processed first.", - 5.0, 1.0, 100.0) - -grp_obstacles.add("costmap_obstacles_behind_robot_dist", double_t, 0, - "Limit the occupied local costmap obstacles taken into account for planning behind the robot (specify distance in meters)", - 1.5, 0.0, 20.0) - -grp_obstacles.add("obstacle_poses_affected", int_t, 0, - "The obstacle position is attached to the closest pose on the trajectory to reduce computational effort, but take a number of neighbors into account as well", - 30, 0, 200) - -# Obstacle - Velocity ratio parameters -grp_obstacles_velocity_limit = grp_obstacles.add_group("Reduce velocity near obstacles") - -grp_obstacles_velocity_limit.add("obstacle_proximity_ratio_max_vel", double_t, 0, - "Ratio of the maximum velocities used as an upper bound when reducing the speed due to the proximity to static obstacles", - 1, 0, 1) - -grp_obstacles_velocity_limit.add("obstacle_proximity_lower_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be lower", - 0, 0, 10) - -grp_obstacles_velocity_limit.add("obstacle_proximity_upper_bound", double_t, 0, - "Distance to a static obstacle for which the velocity should be higher", - 0.5, 0, 10) - -# Optimization -grp_optimization = gen.add_group("Optimization", type="tab") - -grp_optimization.add("no_inner_iterations", int_t, 0, - "Number of solver iterations called in each outerloop iteration", - 5, 1, 100) - -grp_optimization.add("no_outer_iterations", int_t, 0, - "Each outerloop iteration automatically resizes the trajectory and invokes the internal optimizer with no_inner_iterations", - 4, 1, 100) - -grp_optimization.add("optimization_activate", bool_t, 0, - "Activate the optimization", - True) - -grp_optimization.add("optimization_verbose", bool_t, 0, - "Print verbose information", - False) - -grp_optimization.add("penalty_epsilon", double_t, 0, - "Add a small safty margin to penalty functions for hard-constraint approximations", - 0.05, 0, 1.0) - -grp_optimization.add("weight_max_vel_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational velocity", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing velocity (in use only for holonomic robots)", - 2, 0, 1000) - -grp_optimization.add("weight_max_vel_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular velocity", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_x", double_t, 0, - "Optimization weight for satisfying the maximum allowed translational acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_y", double_t, 0, - "Optimization weight for satisfying the maximum allowed strafing acceleration (in use only for holonomic robots)", - 1, 0, 1000) - -grp_optimization.add("weight_acc_lim_theta", double_t, 0, - "Optimization weight for satisfying the maximum allowed angular acceleration", - 1, 0, 1000) - -grp_optimization.add("weight_kinematics_nh", double_t, 0, - "Optimization weight for satisfying the non-holonomic kinematics", - 1000 , 0, 10000) - -grp_optimization.add("weight_kinematics_forward_drive", double_t, 0, - "Optimization weight for forcing the robot to choose only forward directions (positive transl. velocities, only diffdrive robot)", - 1, 0, 10000) - -grp_optimization.add("weight_kinematics_turning_radius", double_t, 0, - "Optimization weight for enforcing a minimum turning radius (carlike robots)", - 1, 0, 1000) - -grp_optimization.add("weight_optimaltime", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. transition time", - 1, 0, 1000) - -grp_optimization.add("weight_shortest_path", double_t, 0, - "Optimization weight for contracting the trajectory w.r.t. path length", - 0, 0, 100) - -grp_optimization.add("weight_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_inflation", double_t, 0, - "Optimization weight for the inflation penalty (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_dynamic_obstacle", double_t, 0, - "Optimization weight for satisfying a minimum seperation from dynamic obstacles", - 50, 0, 1000) - -grp_optimization.add("weight_dynamic_obstacle_inflation", double_t, 0, - "Optimization weight for the inflation penalty of dynamic obstacles (should be small)", - 0.1, 0, 10) - -grp_optimization.add("weight_velocity_obstacle_ratio", double_t, 0, - "Optimization weight for satisfying a maximum allowed velocity with respect to the distance to a static obstacle", - 0, 0, 1000) - -grp_optimization.add("weight_viapoint", double_t, 0, - "Optimization weight for minimizing the distance to via-points", - 1, 0, 1000) - -grp_optimization.add("weight_adapt_factor", double_t, 0, - "Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new: weight_old * factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem.", - 2, 1, 100) - -grp_optimization.add("obstacle_cost_exponent", double_t, 0, - "Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default)", - 1, 0.01, 100) - - - -# Homotopy Class Planner -grp_hcp = gen.add_group("HCPlanning", type="tab") - -grp_hcp.add("enable_multithreading", bool_t, 0, - "Activate multiple threading for planning multiple trajectories in parallel", - True) - -grp_hcp.add("max_number_classes", int_t, 0, - "Specify the maximum number of allowed alternative homotopy classes (limits computational effort)", - 5, 1, 100) - -grp_hcp.add("max_number_plans_in_current_class", int_t, 0, - "Max number of trajectories to try that are in the same homotopy class as the current best trajectory (setting this to 2 or more helps avoid local minima). Must be <= max_number_classes", - 1, 1, 10) - -grp_hcp.add("selection_cost_hysteresis", double_t, 0, - "Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor)", - 1.0, 0, 2) - - -grp_hcp.add("selection_prefer_initial_plan", double_t, 0, - "Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan.)", - 0.95, 0, 1) - -grp_hcp.add("selection_obst_cost_scale", double_t, 0, - "Extra scaling of obstacle cost terms just for selecting the 'best' candidate (new_obst_cost: obst_cost*factor)", - 2.0, 0, 1000) - -grp_hcp.add("selection_viapoint_cost_scale", double_t, 0, - "Extra scaling of via-point cost terms just for selecting the 'best' candidate. (new_viapt_cost: viapt_cost*factor)", - 1.0, 0, 100) - -grp_hcp.add("selection_alternative_time_cost", bool_t, 0, - "If true, time cost is replaced by the total transition time.", - False) - -grp_hcp.add("selection_dropping_probability", double_t, 0, - "At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies.", - 0.0, 0.0, 1.0) - -grp_hcp.add("switching_blocking_period", double_t, 0, - "Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed", - 0.0, 0.0, 60) - -grp_hcp.add("roadmap_graph_no_samples", int_t, 0, - "Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off", - 15, 1, 100) - -grp_hcp.add("roadmap_graph_area_width", double_t, 0, - "Specify the width of the area in which sampled will be generated between start and goal [m] (the height equals the start-goal distance)", - 5, 0.1, 20) - -grp_hcp.add("roadmap_graph_area_length_scale", double_t, 0, - "The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal!)", - 1.0, 0.5, 2) - -grp_hcp.add("h_signature_prescaler", double_t, 0, - "Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2 - Value: true - - Alpha: 0.100000001 - Cell Size: 0.100000001 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.0299999993 - Value: Lines - Name: Fine Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.300000012 - Head Length: 0.200000003 - Length: 0.300000012 - Line Style: Lines - Line Width: 0.0299999993 - Name: Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Style: None - Radius: 0.0299999993 - Shaft Diameter: 0.100000001 - Shaft Length: 0.100000001 - Topic: /test_optim_node/local_plan - Unreliable: false - Value: true - - Alpha: 1 - Arrow Length: 0.300000012 - Axes Length: 0.300000012 - Axes Radius: 0.00999999978 - Class: rviz/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.0700000003 - Head Radius: 0.0299999993 - Name: PoseArray - Shaft Length: 0.230000004 - Shaft Radius: 0.00999999978 - Shape: Arrow (Flat) - Topic: /test_optim_node/teb_poses - Unreliable: false - Value: true - - Class: rviz/Marker - Enabled: true - Marker Topic: /test_optim_node/teb_markers - Name: Marker - Namespaces: - PointObstacles: true - Queue Size: 100 - Value: true - - Class: rviz/InteractiveMarkers - Enable Transparency: true - Enabled: true - Name: InteractiveMarkers - Show Axes: false - Show Descriptions: true - Show Visual Aids: false - Update Topic: /marker_obstacles/update - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: odom - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 7.77247 - Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 - Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 1.56979632 - Target Frame: - Value: Orbit (rviz) - Yaw: 4.71043873 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 846 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003da000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1354 - X: 137 - Y: 50 diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindG2O.cmake b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindG2O.cmake deleted file mode 100644 index b2670d3..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindG2O.cmake +++ /dev/null @@ -1,97 +0,0 @@ -# Locate the g2o libraries -# A general framework for graph optimization. -# -# This module defines -# G2O_FOUND, if false, do not try to link against g2o -# G2O_LIBRARIES, path to the libg2o -# G2O_INCLUDE_DIR, where to find the g2o header files -# -# Niko Suenderhauf -# Adapted by Felix Endres - -IF(UNIX) - - #IF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) - # in cache already - # SET(G2O_FIND_QUIETLY TRUE) - #ENDIF(G2O_INCLUDE_DIR AND G2O_LIBRARIES) - - MESSAGE(STATUS "Searching for g2o ...") - FIND_PATH(G2O_INCLUDE_DIR - NAMES core math_groups types - PATHS /usr/local /usr - PATH_SUFFIXES include/g2o include) - - IF (G2O_INCLUDE_DIR) - MESSAGE(STATUS "Found g2o headers in: ${G2O_INCLUDE_DIR}") - ENDIF (G2O_INCLUDE_DIR) - - FIND_LIBRARY(G2O_CORE_LIB - NAMES g2o_core g2o_core_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_STUFF_LIB - NAMES g2o_stuff g2o_stuff_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_TYPES_SLAM2D_LIB - NAMES g2o_types_slam2d g2o_types_slam2d_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_TYPES_SLAM3D_LIB - NAMES g2o_types_slam3d g2o_types_slam3d_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_SOLVER_CHOLMOD_LIB - NAMES g2o_solver_cholmod g2o_solver_cholmod_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_SOLVER_PCG_LIB - NAMES g2o_solver_pcg g2o_solver_pcg_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_SOLVER_CSPARSE_LIB - NAMES g2o_solver_csparse g2o_solver_csparse_rd - PATHS /usr/local /usr - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_INCREMENTAL_LIB - NAMES g2o_incremental g2o_incremental_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - FIND_LIBRARY(G2O_CSPARSE_EXTENSION_LIB - NAMES g2o_csparse_extension g2o_csparse_extension_rd - PATHS /usr/local /usr ${CMAKE_PREFIX_PATH} - PATH_SUFFIXES lib) - - SET(G2O_LIBRARIES ${G2O_CSPARSE_EXTENSION_LIB} - ${G2O_CORE_LIB} - ${G2O_STUFF_LIB} - ${G2O_TYPES_SLAM2D_LIB} - ${G2O_TYPES_SLAM3D_LIB} - ${G2O_SOLVER_CHOLMOD_LIB} - ${G2O_SOLVER_PCG_LIB} - ${G2O_SOLVER_CSPARSE_LIB} - ${G2O_INCREMENTAL_LIB} - ) - - IF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) - SET(G2O_FOUND "YES") - IF(NOT G2O_FIND_QUIETLY) - MESSAGE(STATUS "Found libg2o: ${G2O_LIBRARIES}") - ENDIF(NOT G2O_FIND_QUIETLY) - ELSE(G2O_LIBRARIES AND G2O_INCLUDE_DIR) - IF(NOT G2O_LIBRARIES) - IF(G2O_FIND_REQUIRED) - message(FATAL_ERROR "Could not find libg2o!") - ENDIF(G2O_FIND_REQUIRED) - ENDIF(NOT G2O_LIBRARIES) - - IF(NOT G2O_INCLUDE_DIR) - IF(G2O_FIND_REQUIRED) - message(FATAL_ERROR "Could not find g2o include directory!") - ENDIF(G2O_FIND_REQUIRED) - ENDIF(NOT G2O_INCLUDE_DIR) - ENDIF(G2O_LIBRARIES AND G2O_INCLUDE_DIR) - -ENDIF(UNIX) - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake deleted file mode 100644 index 101b79b..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/cmake_modules/FindSUITESPARSE.cmake +++ /dev/null @@ -1,133 +0,0 @@ -# - Try to find SUITESPARSE -# Once done this will define -# -# SUITESPARSE_FOUND - system has SUITESPARSE -# SUITESPARSE_INCLUDE_DIRS - the SUITESPARSE include directory -# SUITESPARSE_LIBRARIES - Link these to use SUITESPARSE -# SUITESPARSE_SPQR_LIBRARY - name of spqr library (necessary due to error in debian package) -# SUITESPARSE_SPQR_LIBRARY_DIR - name of spqr library (necessary due to error in debian package) -# SUITESPARSE_LIBRARY_DIR - Library main directory containing suitesparse libs -# SUITESPARSE_LIBRARY_DIRS - all Library directories containing suitesparse libs -# SUITESPARSE_SPQR_VALID - automatic identification whether or not spqr package is installed correctly - -IF (SUITESPARSE_INCLUDE_DIRS) - # Already in cache, be silent - SET(SUITESPARSE_FIND_QUIETLY TRUE) -ENDIF (SUITESPARSE_INCLUDE_DIRS) - -if( WIN32 ) - # Find cholmod part of the suitesparse library collection - - FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h - PATHS "C:\\libs\\win32\\SuiteSparse\\Include" ) - - # Add cholmod include directory to collection include directories - IF ( CHOLMOD_INCLUDE_DIR ) - list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) - ENDIF( CHOLMOD_INCLUDE_DIR ) - - - # find path suitesparse library - FIND_PATH( SUITESPARSE_LIBRARY_DIRS - amd.lib - PATHS "C:\\libs\\win32\\SuiteSparse\\libs" ) - - # if we found the library, add it to the defined libraries - IF ( SUITESPARSE_LIBRARY_DIRS ) - list ( APPEND SUITESPARSE_LIBRARIES optimized;amd;optimized;camd;optimized;ccolamd;optimized;cholmod;optimized;colamd;optimized;metis;optimized;spqr;optimized;umfpack;debug;amdd;debug;camdd;debug;ccolamdd;debug;cholmodd;debug;spqrd;debug;umfpackd;debug;colamdd;debug;metisd;optimized;blas;optimized;libf2c;optimized;lapack;debug;blasd;debug;libf2cd;debug;lapackd ) - ENDIF( SUITESPARSE_LIBRARY_DIRS ) - -else( WIN32 ) - IF(APPLE) - FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h - PATHS /opt/local/include/ufsparse - /usr/local/include ) - - FIND_PATH( SUITESPARSE_LIBRARY_DIR - NAMES libcholmod.a - PATHS /opt/local/lib - /usr/local/lib ) - ELSE(APPLE) - FIND_PATH( CHOLMOD_INCLUDE_DIR cholmod.h - PATHS /usr/local/include - /usr/include - /usr/include/suitesparse/ - ${CMAKE_SOURCE_DIR}/MacOS/Libs/cholmod - PATH_SUFFIXES cholmod/ CHOLMOD/ ) - - FIND_PATH( SUITESPARSE_LIBRARY_DIR - NAMES libcholmod.so libcholmod.a - PATHS /usr/lib - /usr/lib64 - /usr/lib/x86_64-linux-gnu - /usr/lib/i386-linux-gnu - /usr/local/lib - /usr/lib/arm-linux-gnueabihf/ - /usr/lib/aarch64-linux-gnu/ - /usr/lib/arm-linux-gnueabi/ - /usr/lib/arm-linux-gnu) - ENDIF(APPLE) - - # Add cholmod include directory to collection include directories - IF ( CHOLMOD_INCLUDE_DIR ) - list ( APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR} ) - ENDIF( CHOLMOD_INCLUDE_DIR ) - - # if we found the library, add it to the defined libraries - IF ( SUITESPARSE_LIBRARY_DIR ) - - list ( APPEND SUITESPARSE_LIBRARIES amd) - list ( APPEND SUITESPARSE_LIBRARIES btf) - list ( APPEND SUITESPARSE_LIBRARIES camd) - list ( APPEND SUITESPARSE_LIBRARIES ccolamd) - list ( APPEND SUITESPARSE_LIBRARIES cholmod) - list ( APPEND SUITESPARSE_LIBRARIES colamd) - # list ( APPEND SUITESPARSE_LIBRARIES csparse) - list ( APPEND SUITESPARSE_LIBRARIES cxsparse) - list ( APPEND SUITESPARSE_LIBRARIES klu) - # list ( APPEND SUITESPARSE_LIBRARIES spqr) - list ( APPEND SUITESPARSE_LIBRARIES umfpack) - - IF (APPLE) - list ( APPEND SUITESPARSE_LIBRARIES suitesparseconfig) - ENDIF (APPLE) - - # Metis and spqr are optional - FIND_LIBRARY( SUITESPARSE_METIS_LIBRARY - NAMES metis - PATHS ${SUITESPARSE_LIBRARY_DIR} ) - IF (SUITESPARSE_METIS_LIBRARY) - list ( APPEND SUITESPARSE_LIBRARIES metis) - ENDIF(SUITESPARSE_METIS_LIBRARY) - - if(EXISTS "${CHOLMOD_INCLUDE_DIR}/SuiteSparseQR.hpp") - SET(SUITESPARSE_SPQR_VALID TRUE CACHE BOOL "SuiteSparseSPQR valid") - else() - SET(SUITESPARSE_SPQR_VALID false CACHE BOOL "SuiteSparseSPQR valid") - endif() - - if(SUITESPARSE_SPQR_VALID) - FIND_LIBRARY( SUITESPARSE_SPQR_LIBRARY - NAMES spqr - PATHS ${SUITESPARSE_LIBRARY_DIR} ) - IF (SUITESPARSE_SPQR_LIBRARY) - list ( APPEND SUITESPARSE_LIBRARIES spqr) - ENDIF (SUITESPARSE_SPQR_LIBRARY) - endif() - - ENDIF( SUITESPARSE_LIBRARY_DIR ) - -endif( WIN32 ) - - -IF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) - IF(WIN32) - list (APPEND SUITESPARSE_INCLUDE_DIRS ${CHOLMOD_INCLUDE_DIR}/../../UFconfig ) - ENDIF(WIN32) - SET(SUITESPARSE_FOUND TRUE) - MESSAGE(STATUS "Found SuiteSparse") -ELSE (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) - SET( SUITESPARSE_FOUND FALSE ) - MESSAGE(FATAL_ERROR "Unable to find SuiteSparse") -ENDIF (SUITESPARSE_INCLUDE_DIRS AND SUITESPARSE_LIBRARIES) - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/distance_calculations.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/distance_calculations.h deleted file mode 100644 index 62f6b88..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/distance_calculations.h +++ /dev/null @@ -1,464 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef DISTANCE_CALCULATIONS_H -#define DISTANCE_CALCULATIONS_H - -#include -#include "teb_local_planner/misc.h" - - -namespace teb_local_planner -{ - -//! Abbrev. for a container storing 2d points -typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator > Point2dContainer; - - -/** - * @brief Helper function to obtain the closest point on a line segment w.r.t. a reference point - * @param point 2D point - * @param line_start 2D point representing the start of the line segment - * @param line_end 2D point representing the end of the line segment - * @return Closest point on the line segment - */ -inline Eigen::Vector2d closest_point_on_line_segment_2d(const Eigen::Ref& point, const Eigen::Ref& line_start, const Eigen::Ref& line_end) -{ - Eigen::Vector2d diff = line_end - line_start; - double sq_norm = diff.squaredNorm(); - - if (sq_norm == 0) - return line_start; - - double u = ((point.x() - line_start.x()) * diff.x() + (point.y() - line_start.y())*diff.y()) / sq_norm; - - if (u <= 0) return line_start; - else if (u >= 1) return line_end; - - return line_start + u*diff; -} - -/** - * @brief Helper function to calculate the distance between a line segment and a point - * @param point 2D point - * @param line_start 2D point representing the start of the line segment - * @param line_end 2D point representing the end of the line segment - * @return minimum distance to a given line segment - */ -inline double distance_point_to_segment_2d(const Eigen::Ref& point, const Eigen::Ref& line_start, const Eigen::Ref& line_end) -{ - return (point - closest_point_on_line_segment_2d(point, line_start, line_end)).norm(); -} - -/** - * @brief Helper function to check whether two line segments intersects - * @param line1_start 2D point representing the start of the first line segment - * @param line1_end 2D point representing the end of the first line segment - * @param line2_start 2D point representing the start of the second line segment - * @param line2_end 2D point representing the end of the second line segment - * @param[out] intersection [optional] Write intersection point to destination (the value is only written, if both lines intersect, e.g. if the function returns \c true) - * @return \c true if both line segments intersect - */ -inline bool check_line_segments_intersection_2d(const Eigen::Ref& line1_start, const Eigen::Ref& line1_end, - const Eigen::Ref& line2_start, const Eigen::Ref& line2_end, Eigen::Vector2d* intersection = NULL) -{ - // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect - double s_numer, t_numer, denom, t; - Eigen::Vector2d line1 = line1_end - line1_start; - Eigen::Vector2d line2 = line2_end - line2_start; - - denom = line1.x() * line2.y() - line2.x() * line1.y(); - if (denom == 0) return false; // Collinear - bool denomPositive = denom > 0; - - Eigen::Vector2d aux = line1_start - line2_start; - - s_numer = line1.x() * aux.y() - line1.y() * aux.x(); - if ((s_numer < 0) == denomPositive) return false; // No collision - - t_numer = line2.x() * aux.y() - line2.y() * aux.x(); - if ((t_numer < 0) == denomPositive) return false; // No collision - - if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive)) return false; // No collision - - // Otherwise collision detected - t = t_numer / denom; - if (intersection) - { - *intersection = line1_start + t * line1; - } - - return true; -} - - -/** - * @brief Helper function to calculate the smallest distance between two line segments - * @param line1_start 2D point representing the start of the first line segment - * @param line1_end 2D point representing the end of the first line segment - * @param line2_start 2D point representing the start of the second line segment - * @param line2_end 2D point representing the end of the second line segment - * @return smallest distance between both segments -*/ -inline double distance_segment_to_segment_2d(const Eigen::Ref& line1_start, const Eigen::Ref& line1_end, - const Eigen::Ref& line2_start, const Eigen::Ref& line2_end) -{ - // TODO more efficient implementation - - // check if segments intersect - if (check_line_segments_intersection_2d(line1_start, line1_end, line2_start, line2_end)) - return 0; - - // check all 4 combinations - std::array distances; - - distances[0] = distance_point_to_segment_2d(line1_start, line2_start, line2_end); - distances[1] = distance_point_to_segment_2d(line1_end, line2_start, line2_end); - distances[2] = distance_point_to_segment_2d(line2_start, line1_start, line1_end); - distances[3] = distance_point_to_segment_2d(line2_end, line1_start, line1_end); - - return *std::min_element(distances.begin(), distances.end()); -} - - -/** - * @brief Helper function to calculate the smallest distance between a point and a closed polygon - * @param point 2D point - * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end) - * @return smallest distance between point and polygon -*/ -inline double distance_point_to_polygon_2d(const Eigen::Vector2d& point, const Point2dContainer& vertices) -{ - double dist = HUGE_VAL; - - // the polygon is a point - if (vertices.size() == 1) - { - return (point - vertices.front()).norm(); - } - - // check each polygon edge - for (int i=0; i<(int)vertices.size()-1; ++i) - { - double new_dist = distance_point_to_segment_2d(point, vertices.at(i), vertices.at(i+1)); -// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1)); - if (new_dist < dist) - dist = new_dist; - } - - if (vertices.size()>2) // if not a line close polygon - { - double new_dist = distance_point_to_segment_2d(point, vertices.back(), vertices.front()); // check last edge - if (new_dist < dist) - return new_dist; - } - - return dist; -} - -/** - * @brief Helper function to calculate the smallest distance between a line segment and a closed polygon - * @param line_start 2D point representing the start of the line segment - * @param line_end 2D point representing the end of the line segment - * @param vertices Vertices describing the closed polygon (the first vertex is not repeated at the end) - * @return smallest distance between point and polygon -*/ -inline double distance_segment_to_polygon_2d(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, const Point2dContainer& vertices) -{ - double dist = HUGE_VAL; - - // the polygon is a point - if (vertices.size() == 1) - { - return distance_point_to_segment_2d(vertices.front(), line_start, line_end); - } - - // check each polygon edge - for (int i=0; i<(int)vertices.size()-1; ++i) - { - double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.at(i), vertices.at(i+1)); -// double new_dist = calc_distance_point_to_segment( position, vertices.at(i), vertices.at(i+1)); - if (new_dist < dist) - dist = new_dist; - } - - if (vertices.size()>2) // if not a line close polygon - { - double new_dist = distance_segment_to_segment_2d(line_start, line_end, vertices.back(), vertices.front()); // check last edge - if (new_dist < dist) - return new_dist; - } - - return dist; -} - -/** - * @brief Helper function to calculate the smallest distance between two closed polygons - * @param vertices1 Vertices describing the first closed polygon (the first vertex is not repeated at the end) - * @param vertices2 Vertices describing the second closed polygon (the first vertex is not repeated at the end) - * @return smallest distance between point and polygon -*/ -inline double distance_polygon_to_polygon_2d(const Point2dContainer& vertices1, const Point2dContainer& vertices2) -{ - double dist = HUGE_VAL; - - // the polygon1 is a point - if (vertices1.size() == 1) - { - return distance_point_to_polygon_2d(vertices1.front(), vertices2); - } - - // check each edge of polygon1 - for (int i=0; i<(int)vertices1.size()-1; ++i) - { - double new_dist = distance_segment_to_polygon_2d(vertices1[i], vertices1[i+1], vertices2); - if (new_dist < dist) - dist = new_dist; - } - - if (vertices1.size()>2) // if not a line close polygon1 - { - double new_dist = distance_segment_to_polygon_2d(vertices1.back(), vertices1.front(), vertices2); // check last edge - if (new_dist < dist) - return new_dist; - } - - return dist; -} - - - - -// Further distance calculations: - - -// The Distance Calculations are mainly copied from http://geomalgorithms.com/a07-_distance.html -// Copyright 2001 softSurfer, 2012 Dan Sunday -// This code may be freely used and modified for any purpose -// providing that this copyright notice is included with it. -// SoftSurfer makes no warranty for this code, and cannot be held -// liable for any real or imagined damage resulting from its use. -// Users of this code must verify correctness for their application. - -inline double calc_distance_line_to_line_3d(const Eigen::Ref& x1, Eigen::Ref& u, - const Eigen::Ref& x2, Eigen::Ref& v) -{ - Eigen::Vector3d w = x2 - x1; - double a = u.squaredNorm(); // dot(u,u) always >= 0 - double b = u.dot(v); - double c = v.squaredNorm(); // dot(v,v) always >= 0 - double d = u.dot(w); - double e = v.dot(w); - double D = a*c - b*b; // always >= 0 - double sc, tc; - - // compute the line parameters of the two closest points - if (D < SMALL_NUM) { // the lines are almost parallel - sc = 0.0; - tc = (b>c ? d/b : e/c); // use the largest denominator - } - else { - sc = (b*e - c*d) / D; - tc = (a*e - b*d) / D; - } - - // get the difference of the two closest points - Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = L1(sc) - L2(tc) - - return dP.norm(); // return the closest distance -} - - - - - -inline double calc_distance_segment_to_segment3D(const Eigen::Ref& line1_start, Eigen::Ref& line1_end, - const Eigen::Ref& line2_start, Eigen::Ref& line2_end) -{ - Eigen::Vector3d u = line1_end - line1_start; - Eigen::Vector3d v = line2_end - line2_start; - Eigen::Vector3d w = line2_start - line1_start; - double a = u.squaredNorm(); // dot(u,u) always >= 0 - double b = u.dot(v); - double c = v.squaredNorm(); // dot(v,v) always >= 0 - double d = u.dot(w); - double e = v.dot(w); - double D = a*c - b*b; // always >= 0 - double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 - double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 - - // compute the line parameters of the two closest points - if (D < SMALL_NUM) - { // the lines are almost parallel - sN = 0.0; // force using point P0 on segment S1 - sD = 1.0; // to prevent possible division by 0.0 later - tN = e; - tD = c; - } - else - { // get the closest points on the infinite lines - sN = (b*e - c*d); - tN = (a*e - b*d); - if (sN < 0.0) - { // sc < 0 => the s=0 edge is visible - sN = 0.0; - tN = e; - tD = c; - } - else if (sN > sD) - { // sc > 1 => the s=1 edge is visible - sN = sD; - tN = e + b; - tD = c; - } - } - - if (tN < 0.0) - { // tc < 0 => the t=0 edge is visible - tN = 0.0; - // recompute sc for this edge - if (-d < 0.0) - sN = 0.0; - else if (-d > a) - sN = sD; - else - { - sN = -d; - sD = a; - } - } - else if (tN > tD) - { // tc > 1 => the t=1 edge is visible - tN = tD; - // recompute sc for this edge - if ((-d + b) < 0.0) - sN = 0; - else if ((-d + b) > a) - sN = sD; - else - { - sN = (-d + b); - sD = a; - } - } - // finally do the division to get sc and tc - sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD); - tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD); - - // get the difference of the two closest points - Eigen::Vector3d dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) - - return dP.norm(); // return the closest distance -} - - - - -template -double calc_closest_point_to_approach_time(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2) -{ - VectorType dv = vel1 - vel2; - - double dv2 = dv.squaredNorm(); // dot(v,v) - if (dv2 < SMALL_NUM) // the tracks are almost parallel - return 0.0; // any time is ok. Use time 0. - - VectorType w0 = x1 - x2; - double cpatime = -w0.dot(dv) / dv2; - - return cpatime; // time of CPA -} - -template -double calc_closest_point_to_approach_distance(const VectorType& x1, const VectorType& vel1, const VectorType& x2, const VectorType& vel2, double bound_cpa_time = 0) -{ - double ctime = calc_closest_point_to_approach_time(x1, vel1, x2, vel2); - if (bound_cpa_time!=0 && ctime > bound_cpa_time) ctime = bound_cpa_time; - VectorType P1 = x1 + (ctime * vel1); - VectorType P2 = x2 + (ctime * vel2); - - return (P2-P1).norm(); // distance at CPA -} - - - -// dist_Point_to_Line(): get the distance of a point to a line -// Input: a Point P and a Line L (in any dimension) -// Return: the shortest distance from P to L -template -double calc_distance_point_to_line( const VectorType& point, const VectorType& line_base, const VectorType& line_dir) -{ - VectorType w = point - line_base; - - double c1 = w.dot(line_dir); - double c2 = line_dir.dot(line_dir); - double b = c1 / c2; - - VectorType Pb = line_base + b * line_dir; - return (point-Pb).norm(); -} -//=================================================================== - - -// dist_Point_to_Segment(): get the distance of a point to a segment -// Input: a Point P and a Segment S (in any dimension) -// Return: the shortest distance from P to S -template -double calc_distance_point_to_segment( const VectorType& point, const VectorType& line_start, const VectorType& line_end) -{ - VectorType v = line_end - line_start; - VectorType w = point - line_start; - - double c1 = w.dot(v); - if ( c1 <= 0 ) - return w.norm(); - - double c2 = v.dot(v); - if ( c2 <= c1 ) - return (point-line_end).norm(); - - double b = c1 / c2; - VectorType Pb = line_start + b * v; - return (point-Pb).norm(); -} - - - -} // namespace teb_local_planner - -#endif /* DISTANCE_CALCULATIONS_H */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/equivalence_relations.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/equivalence_relations.h deleted file mode 100644 index 8551152..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/equivalence_relations.h +++ /dev/null @@ -1,103 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EQUIVALENCE_RELATIONS_H_ -#define EQUIVALENCE_RELATIONS_H_ - -#include - -namespace teb_local_planner -{ - -/** - * @class EquivalenceClass - * @brief Abstract class that defines an interface for computing and comparing equivalence classes - * - * Equivalence relations are utilized in order to test if two trajectories are belonging to the same - * equivalence class w.r.t. the current obstacle configurations. A common equivalence relation is - * the concept of homotopy classes. All trajectories belonging to the same homotopy class - * can CONTINUOUSLY be deformed into each other without intersecting any obstacle. Hence they likely - * share the same local minimum after invoking (local) trajectory optimization. A weaker equivalence relation - * is defined by the concept of homology classes (e.g. refer to HSignature). - * - * Each EquivalenceClass object (or subclass) stores a candidate value which might be compared to another EquivalenceClass object. - * - * @remarks Currently, the computeEquivalenceClass method is not available in the generic interface EquivalenceClass. - * Call the "compute"-methods directly on the subclass. - */ -class EquivalenceClass -{ -public: - - /** - * @brief Default constructor - */ - EquivalenceClass() {} - - /** - * @brief virtual destructor - */ - virtual ~EquivalenceClass() {} - - /** - * @brief Check if two candidate classes are equivalent - * @param other The other equivalence class to test with - */ - virtual bool isEqual(const EquivalenceClass& other) const = 0; - - /** - * @brief Check if the equivalence value is detected correctly - * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur. - */ - virtual bool isValid() const = 0; - - /** - * @brief Check if the trajectory is non-looping around an obstacle - * @return Returns false, if the trajectory loops around an obstacle - */ - virtual bool isReasonable() const = 0; - -}; - -using EquivalenceClassPtr = std::shared_ptr; - - -} // namespace teb_local_planner - - -#endif /* EQUIVALENCE_RELATIONS_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h deleted file mode 100644 index dd35147..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/base_teb_edges.h +++ /dev/null @@ -1,278 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef _BASE_TEB_EDGES_H_ -#define _BASE_TEB_EDGES_H_ - -#include "teb_local_planner/teb_config.h" - -#include -#include -#include - -namespace teb_local_planner -{ - - -/** - * @class BaseTebUnaryEdge - * @brief Base edge connecting a single vertex in the TEB optimization problem - * - * This edge defines a base edge type for the TEB optimization problem. - * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). - * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. - * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. - * @see BaseTebMultiEdge, BaseTebBinaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge - */ -template -class BaseTebUnaryEdge : public g2o::BaseUnaryEdge -{ -public: - - using typename g2o::BaseUnaryEdge::ErrorVector; - using g2o::BaseUnaryEdge::computeError; - - /** - * @brief Compute and return error / cost value. - * - * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. - * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T - */ - ErrorVector& getError() - { - computeError(); - return _error; - } - - /** - * @brief Read values from input stream - */ - virtual bool read(std::istream& is) - { - // TODO generic read - return true; - } - - /** - * @brief Write values to an output stream - */ - virtual bool write(std::ostream& os) const - { - // TODO generic write - return os.good(); - } - - /** - * @brief Assign the TebConfig class for parameters. - * @param cfg TebConfig class - */ - void setTebConfig(const TebConfig& cfg) - { - cfg_ = &cfg; - } - -protected: - - using g2o::BaseUnaryEdge::_error; - using g2o::BaseUnaryEdge::_vertices; - - const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * @class BaseTebBinaryEdge - * @brief Base edge connecting two vertices in the TEB optimization problem - * - * This edge defines a base edge type for the TEB optimization problem. - * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). - * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. - * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. - * @see BaseTebMultiEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge - */ -template -class BaseTebBinaryEdge : public g2o::BaseBinaryEdge -{ -public: - - using typename g2o::BaseBinaryEdge::ErrorVector; - using g2o::BaseBinaryEdge::computeError; - - /** - * @brief Compute and return error / cost value. - * - * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. - * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T - */ - ErrorVector& getError() - { - computeError(); - return _error; - } - - /** - * @brief Read values from input stream - */ - virtual bool read(std::istream& is) - { - // TODO generic read - return true; - } - - /** - * @brief Write values to an output stream - */ - virtual bool write(std::ostream& os) const - { - // TODO generic write - return os.good(); - } - - /** - * @brief Assign the TebConfig class for parameters. - * @param cfg TebConfig class - */ - void setTebConfig(const TebConfig& cfg) - { - cfg_ = &cfg; - } - -protected: - - using g2o::BaseBinaryEdge::_error; - using g2o::BaseBinaryEdge::_vertices; - - const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -/** - * @class BaseTebMultiEdge - * @brief Base edge connecting multiple vertices in the TEB optimization problem - * - * This edge defines a base edge type for the TEB optimization problem. - * It is derived from the corresponding g2o base classes augmented with additional information for the dedicated TEB problem (e.g. config). - * The destructor erases the edge in all attached vertices in order to allow keeping the vertices valid in subsequent g2o optimization calls. - * Memory of edges should be freed by calling the clearEdge method of the g2o optimizer class. - * @see BaseTebBinaryEdge, BaseTebUnaryEdge, g2o::BaseBinaryEdge, g2o::BaseUnaryEdge, g2o::BaseMultiEdge - */ -template -class BaseTebMultiEdge : public g2o::BaseMultiEdge -{ -public: - - using typename g2o::BaseMultiEdge::ErrorVector; - using g2o::BaseMultiEdge::computeError; - - // Overwrites resize() from the parent class - virtual void resize(size_t size) - { - g2o::BaseMultiEdge::resize(size); - - for(std::size_t i=0; i<_vertices.size(); ++i) - _vertices[i] = NULL; - } - - /** - * @brief Compute and return error / cost value. - * - * This method is called by TebOptimalPlanner::computeCurrentCost to obtain the current cost. - * @return 2D Cost / error vector [nh cost, backward drive dir cost]^T - */ - ErrorVector& getError() - { - computeError(); - return _error; - } - - /** - * @brief Read values from input stream - */ - virtual bool read(std::istream& is) - { - // TODO generic read - return true; - } - - /** - * @brief Write values to an output stream - */ - virtual bool write(std::ostream& os) const - { - // TODO generic write - return os.good(); - } - - /** - * @brief Assign the TebConfig class for parameters. - * @param cfg TebConfig class - */ - void setTebConfig(const TebConfig& cfg) - { - cfg_ = &cfg; - } - -protected: - - using g2o::BaseMultiEdge::_error; - using g2o::BaseMultiEdge::_vertices; - - const TebConfig* cfg_{nullptr}; //!< Store TebConfig class for parameters - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - - - -} // end namespace - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h deleted file mode 100644 index 5d3ff98..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_acceleration.h +++ /dev/null @@ -1,732 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_ACCELERATION_H_ -#define EDGE_ACCELERATION_H_ - -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/vertex_timediff.h" -#include "teb_local_planner/g2o_types/penalties.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/misc.h" - -#include - -namespace teb_local_planner -{ - -/** - * @class EdgeAcceleration - * @brief Edge defining the cost function for limiting the translational and rotational acceleration. - * - * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes: - * \f$ \min \textrm{penaltyInterval}( [a, omegadot } ]^T ) \cdot weight \f$. \n - * \e a is calculated using the difference quotient (twice) and the position parts of all three poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and - * the second one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationStart - * @see EdgeAccelerationGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationStart() and EdgeAccelerationGoal() for defining boundary values! - */ -class EdgeAcceleration : public BaseTebMultiEdge<2, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeAcceleration() - { - this->resize(5); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexPose* pose3 = static_cast(_vertices[2]); - const VertexTimeDiff* dt1 = static_cast(_vertices[3]); - const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - - // VELOCITY & ACCELERATION - const Eigen::Vector2d diff1 = pose2->position() - pose1->position(); - const Eigen::Vector2d diff2 = pose3->position() - pose2->position(); - - double dist1 = diff1.norm(); - double dist2 = diff2.norm(); - const double angle_diff1 = g2o::normalize_theta(pose2->theta() - pose1->theta()); - const double angle_diff2 = g2o::normalize_theta(pose3->theta() - pose2->theta()); - - if (cfg_->trajectory.exact_arc_length) // use exact arc length instead of Euclidean approximation - { - if (angle_diff1 != 0) - { - const double radius = dist1/(2*sin(angle_diff1/2)); - dist1 = fabs( angle_diff1 * radius ); // actual arg length! - } - if (angle_diff2 != 0) - { - const double radius = dist2/(2*sin(angle_diff2/2)); - dist2 = fabs( angle_diff2 * radius ); // actual arg length! - } - } - - double vel1 = dist1 / dt1->dt(); - double vel2 = dist2 / dt2->dt(); - - - // consider directions -// vel1 *= g2o::sign(diff1[0]*cos(pose1->theta()) + diff1[1]*sin(pose1->theta())); -// vel2 *= g2o::sign(diff2[0]*cos(pose2->theta()) + diff2[1]*sin(pose2->theta())); - vel1 *= fast_sigmoid( 100*(diff1.x()*cos(pose1->theta()) + diff1.y()*sin(pose1->theta())) ); - vel2 *= fast_sigmoid( 100*(diff2.x()*cos(pose2->theta()) + diff2.y()*sin(pose2->theta())) ); - - const double acc_lin = (vel2 - vel1)*2 / ( dt1->dt() + dt2->dt() ); - - - _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - const double omega1 = angle_diff1 / dt1->dt(); - const double omega2 = angle_diff2 / dt2->dt(); - const double acc_rot = (omega2 - omega1)*2 / ( dt1->dt() + dt2->dt() ); - - _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]); - TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() rotational: _error[1]=%f\n",_error[1]); - } - - - -#ifdef USE_ANALYTIC_JACOBI -#if 0 - /* - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); - const VertexPointXY* conf1 = static_cast(_vertices[0]); - const VertexPointXY* conf2 = static_cast(_vertices[1]); - const VertexPointXY* conf3 = static_cast(_vertices[2]); - const VertexTimeDiff* deltaT1 = static_cast(_vertices[3]); - const VertexTimeDiff* deltaT2 = static_cast(_vertices[4]); - const VertexOrientation* angle1 = static_cast(_vertices[5]); - const VertexOrientation* angle2 = static_cast(_vertices[6]); - const VertexOrientation* angle3 = static_cast(_vertices[7]); - - Eigen::Vector2d deltaS1 = conf2->estimate() - conf1->estimate(); - Eigen::Vector2d deltaS2 = conf3->estimate() - conf2->estimate(); - double dist1 = deltaS1.norm(); - double dist2 = deltaS2.norm(); - - double sum_time = deltaT1->estimate() + deltaT2->estimate(); - double sum_time_inv = 1 / sum_time; - double dt1_inv = 1/deltaT1->estimate(); - double dt2_inv = 1/deltaT2->estimate(); - double aux0 = 2/sum_time_inv; - double aux1 = dist1 * deltaT1->estimate(); - double aux2 = dist2 * deltaT2->estimate(); - - double vel1 = dist1 * dt1_inv; - double vel2 = dist2 * dt2_inv; - double omega1 = g2o::normalize_theta( angle2->estimate() - angle1->estimate() ) * dt1_inv; - double omega2 = g2o::normalize_theta( angle3->estimate() - angle2->estimate() ) * dt2_inv; - double acc = (vel2 - vel1) * aux0; - double omegadot = (omega2 - omega1) * aux0; - double aux3 = -acc/2; - double aux4 = -omegadot/2; - - double dev_border_acc = penaltyBoundToIntervalDerivative(acc, tebConfig.robot_acceleration_max_trans,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); - double dev_border_omegadot = penaltyBoundToIntervalDerivative(omegadot, tebConfig.robot_acceleration_max_rot,optimizationConfig.optimization_boundaries_epsilon,optimizationConfig.optimization_boundaries_scale,optimizationConfig.optimization_boundaries_order); - - _jacobianOplus[0].resize(2,2); // conf1 - _jacobianOplus[1].resize(2,2); // conf2 - _jacobianOplus[2].resize(2,2); // conf3 - _jacobianOplus[3].resize(2,1); // deltaT1 - _jacobianOplus[4].resize(2,1); // deltaT2 - _jacobianOplus[5].resize(2,1); // angle1 - _jacobianOplus[6].resize(2,1); // angle2 - _jacobianOplus[7].resize(2,1); // angle3 - - if (aux1==0) aux1=1e-20; - if (aux2==0) aux2=1e-20; - - if (dev_border_acc!=0) - { - // TODO: double aux = aux0 * dev_border_acc; - // double aux123 = aux / aux1; - _jacobianOplus[0](0,0) = aux0 * deltaS1[0] / aux1 * dev_border_acc; // acc x1 - _jacobianOplus[0](0,1) = aux0 * deltaS1[1] / aux1 * dev_border_acc; // acc y1 - _jacobianOplus[1](0,0) = -aux0 * ( deltaS1[0] / aux1 + deltaS2[0] / aux2 ) * dev_border_acc; // acc x2 - _jacobianOplus[1](0,1) = -aux0 * ( deltaS1[1] / aux1 + deltaS2[1] / aux2 ) * dev_border_acc; // acc y2 - _jacobianOplus[2](0,0) = aux0 * deltaS2[0] / aux2 * dev_border_acc; // acc x3 - _jacobianOplus[2](0,1) = aux0 * deltaS2[1] / aux2 * dev_border_acc; // acc y3 - _jacobianOplus[2](0,0) = 0; - _jacobianOplus[2](0,1) = 0; - _jacobianOplus[3](0,0) = aux0 * (aux3 + vel1 * dt1_inv) * dev_border_acc; // acc deltaT1 - _jacobianOplus[4](0,0) = aux0 * (aux3 - vel2 * dt2_inv) * dev_border_acc; // acc deltaT2 - } - else - { - _jacobianOplus[0](0,0) = 0; // acc x1 - _jacobianOplus[0](0,1) = 0; // acc y1 - _jacobianOplus[1](0,0) = 0; // acc x2 - _jacobianOplus[1](0,1) = 0; // acc y2 - _jacobianOplus[2](0,0) = 0; // acc x3 - _jacobianOplus[2](0,1) = 0; // acc y3 - _jacobianOplus[3](0,0) = 0; // acc deltaT1 - _jacobianOplus[4](0,0) = 0; // acc deltaT2 - } - - if (dev_border_omegadot!=0) - { - _jacobianOplus[3](1,0) = aux0 * ( aux4 + omega1 * dt1_inv ) * dev_border_omegadot; // omegadot deltaT1 - _jacobianOplus[4](1,0) = aux0 * ( aux4 - omega2 * dt2_inv ) * dev_border_omegadot; // omegadot deltaT2 - _jacobianOplus[5](1,0) = aux0 * dt1_inv * dev_border_omegadot; // omegadot angle1 - _jacobianOplus[6](1,0) = -aux0 * ( dt1_inv + dt2_inv ) * dev_border_omegadot; // omegadot angle2 - _jacobianOplus[7](1,0) = aux0 * dt2_inv * dev_border_omegadot; // omegadot angle3 - } - else - { - _jacobianOplus[3](1,0) = 0; // omegadot deltaT1 - _jacobianOplus[4](1,0) = 0; // omegadot deltaT2 - _jacobianOplus[5](1,0) = 0; // omegadot angle1 - _jacobianOplus[6](1,0) = 0; // omegadot angle2 - _jacobianOplus[7](1,0) = 0; // omegadot angle3 - } - - _jacobianOplus[0](1,0) = 0; // omegadot x1 - _jacobianOplus[0](1,1) = 0; // omegadot y1 - _jacobianOplus[1](1,0) = 0; // omegadot x2 - _jacobianOplus[1](1,1) = 0; // omegadot y2 - _jacobianOplus[2](1,0) = 0; // omegadot x3 - _jacobianOplus[2](1,1) = 0; // omegadot y3 - _jacobianOplus[5](0,0) = 0; // acc angle1 - _jacobianOplus[6](0,0) = 0; // acc angle2 - _jacobianOplus[7](0,0) = 0; // acc angle3 - } -#endif -#endif - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -/** - * @class EdgeAccelerationStart - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n - * \e a is calculated using the difference quotient (twice) and the position parts of the poses. \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and - * the second one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAcceleration - * @see EdgeAccelerationGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationGoal() for defining boundary values at the end of the trajectory! - */ -class EdgeAccelerationStart : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeAccelerationStart() - { - _measurement = NULL; - this->resize(3); - } - - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - // VELOCITY & ACCELERATION - const Eigen::Vector2d diff = pose2->position() - pose1->position(); - double dist = diff.norm(); - const double angle_diff = g2o::normalize_theta(pose2->theta() - pose1->theta()); - if (cfg_->trajectory.exact_arc_length && angle_diff != 0) - { - const double radius = dist/(2*sin(angle_diff/2)); - dist = fabs( angle_diff * radius ); // actual arg length! - } - - const double vel1 = _measurement->linear.x; - double vel2 = dist / dt->dt(); - - // consider directions - //vel2 *= g2o::sign(diff[0]*cos(pose1->theta()) + diff[1]*sin(pose1->theta())); - vel2 *= fast_sigmoid( 100*(diff.x()*cos(pose1->theta()) + diff.y()*sin(pose1->theta())) ); - - const double acc_lin = (vel2 - vel1) / dt->dt(); - - _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - const double omega1 = _measurement->angular.z; - const double omega2 = angle_diff / dt->dt(); - const double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]); - TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() rotational: _error[1]=%f\n",_error[1]); - } - - /** - * @brief Set the initial velocity that is taken into account for calculating the acceleration - * @param vel_start twist message containing the translational and rotational velocity - */ - void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start) - { - _measurement = &vel_start; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - -/** - * @class EdgeAccelerationGoal - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [a, omegadot ]^T ) \cdot weight \f$. \n - * \e a is calculated using the difference quotient (twice) and the position parts of the poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 2: the first component represents the translational acceleration and - * the second one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAcceleration - * @see EdgeAccelerationStart - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationStart() for defining boundary (initial) values at the end of the trajectory - */ -class EdgeAccelerationGoal : public BaseTebMultiEdge<2, const geometry_msgs::msg::Twist*> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeAccelerationGoal() - { - _measurement = NULL; - this->resize(3); - } - - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()"); - const VertexPose* pose_pre_goal = static_cast(_vertices[0]); - const VertexPose* pose_goal = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - // VELOCITY & ACCELERATION - - const Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position(); - double dist = diff.norm(); - const double angle_diff = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()); - if (cfg_->trajectory.exact_arc_length && angle_diff != 0) - { - double radius = dist/(2*sin(angle_diff/2)); - dist = fabs( angle_diff * radius ); // actual arg length! - } - - double vel1 = dist / dt->dt(); - const double vel2 = _measurement->linear.x; - - // consider directions - //vel1 *= g2o::sign(diff[0]*cos(pose_pre_goal->theta()) + diff[1]*sin(pose_pre_goal->theta())); - vel1 *= fast_sigmoid( 100*(diff.x()*cos(pose_pre_goal->theta()) + diff.y()*sin(pose_pre_goal->theta())) ); - - const double acc_lin = (vel2 - vel1) / dt->dt(); - - _error[0] = penaltyBoundToInterval(acc_lin,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - const double omega1 = angle_diff / dt->dt(); - const double omega2 = _measurement->angular.z; - const double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[1] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]); - TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() rotational: _error[1]=%f\n",_error[1]); - } - - /** - * @brief Set the goal / final velocity that is taken into account for calculating the acceleration - * @param vel_goal twist message containing the translational and rotational velocity - */ - void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal) - { - _measurement = &vel_goal; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - -/** - * @class EdgeAccelerationHolonomic - * @brief Edge defining the cost function for limiting the translational and rotational acceleration. - * - * The edge depends on five vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \mathbf{s}_{ip2}, \Delta T_i, \Delta T_{ip1} \f$ and minimizes: - * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot } ]^T ) \cdot weight \f$. \n - * \e ax is calculated using the difference quotient (twice) and the x position parts of all three poses \n - * \e ay is calculated using the difference quotient (twice) and the y position parts of all three poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 3: the first component represents the translational acceleration (x-dir), - * the second one the strafing acceleration and the third one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationHolonomicStart - * @see EdgeAccelerationHolonomicGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationHolonomicStart() and EdgeAccelerationHolonomicGoal() for defining boundary values! - */ -class EdgeAccelerationHolonomic : public BaseTebMultiEdge<3, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeAccelerationHolonomic() - { - this->resize(5); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeAcceleration()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexPose* pose3 = static_cast(_vertices[2]); - const VertexTimeDiff* dt1 = static_cast(_vertices[3]); - const VertexTimeDiff* dt2 = static_cast(_vertices[4]); - - // VELOCITY & ACCELERATION - Eigen::Vector2d diff1 = pose2->position() - pose1->position(); - Eigen::Vector2d diff2 = pose3->position() - pose2->position(); - - double cos_theta1 = std::cos(pose1->theta()); - double sin_theta1 = std::sin(pose1->theta()); - double cos_theta2 = std::cos(pose2->theta()); - double sin_theta2 = std::sin(pose2->theta()); - - // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) - double p1_dx = cos_theta1*diff1.x() + sin_theta1*diff1.y(); - double p1_dy = -sin_theta1*diff1.x() + cos_theta1*diff1.y(); - // transform pose3 into robot frame pose2 (inverse 2d rotation matrix) - double p2_dx = cos_theta2*diff2.x() + sin_theta2*diff2.y(); - double p2_dy = -sin_theta2*diff2.x() + cos_theta2*diff2.y(); - - double vel1_x = p1_dx / dt1->dt(); - double vel1_y = p1_dy / dt1->dt(); - double vel2_x = p2_dx / dt2->dt(); - double vel2_y = p2_dy / dt2->dt(); - - double dt12 = dt1->dt() + dt2->dt(); - - double acc_x = (vel2_x - vel1_x)*2 / dt12; - double acc_y = (vel2_y - vel1_y)*2 / dt12; - - _error[0] = penaltyBoundToInterval(acc_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(acc_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - double omega1 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt1->dt(); - double omega2 = g2o::normalize_theta(pose3->theta() - pose2->theta()) / dt2->dt(); - double acc_rot = (omega2 - omega1)*2 / dt12; - - _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAcceleration::computeError() translational: _error[0]=%f\n",_error[0]); - TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAcceleration::computeError() strafing: _error[1]=%f\n",_error[1]); - TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAcceleration::computeError() rotational: _error[2]=%f\n",_error[2]); - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -/** - * @class EdgeAccelerationHolonomicStart - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the beginning of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setInitialVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n - * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses. \n - * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses. \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 3: the first component represents the translational acceleration, - * the second one the strafing acceleration and the third one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationHolonomic - * @see EdgeAccelerationHolonomicGoal - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationHolonomicGoal() for defining boundary values at the end of the trajectory! - */ -class EdgeAccelerationHolonomicStart : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeAccelerationHolonomicStart() - { - this->resize(3); - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setStartVelocity() on EdgeAccelerationStart()"); - const VertexPose* pose1 = static_cast(_vertices[0]); - const VertexPose* pose2 = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - // VELOCITY & ACCELERATION - Eigen::Vector2d diff = pose2->position() - pose1->position(); - - double cos_theta1 = std::cos(pose1->theta()); - double sin_theta1 = std::sin(pose1->theta()); - - // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) - double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y(); - double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y(); - - double vel1_x = _measurement->linear.x; - double vel1_y = _measurement->linear.y; - double vel2_x = p1_dx / dt->dt(); - double vel2_y = p1_dy / dt->dt(); - - double acc_lin_x = (vel2_x - vel1_x) / dt->dt(); - double acc_lin_y = (vel2_y - vel1_y) / dt->dt(); - - _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - double omega1 = _measurement->angular.z; - double omega2 = g2o::normalize_theta(pose2->theta() - pose1->theta()) / dt->dt(); - double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationStart::computeError() translational: _error[0]=%f\n",_error[0]); - TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationStart::computeError() strafing: _error[1]=%f\n",_error[1]); - TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationStart::computeError() rotational: _error[2]=%f\n",_error[2]); - } - - /** - * @brief Set the initial velocity that is taken into account for calculating the acceleration - * @param vel_start twist message containing the translational and rotational velocity - */ - void setInitialVelocity(const geometry_msgs::msg::Twist& vel_start) - { - _measurement = &vel_start; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - -/** - * @class EdgeAccelerationHolonomicGoal - * @brief Edge defining the cost function for limiting the translational and rotational acceleration at the end of the trajectory. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$, an initial velocity defined by setGoalVelocity() - * and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [ax, ay, omegadot ]^T ) \cdot weight \f$. \n - * \e ax is calculated using the difference quotient (twice) and the x-position parts of the poses \n - * \e ay is calculated using the difference quotient (twice) and the y-position parts of the poses \n - * \e omegadot is calculated using the difference quotient of the yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation() \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval() \n - * The dimension of the error / cost vector is 3: the first component represents the translational acceleration, - * the second one is the strafing velocity and the third one the rotational acceleration. - * @see TebOptimalPlanner::AddEdgesAcceleration - * @see EdgeAccelerationHolonomic - * @see EdgeAccelerationHolonomicStart - * @remarks Do not forget to call setTebConfig() - * @remarks Refer to EdgeAccelerationHolonomicStart() for defining boundary (initial) values at the end of the trajectory - */ -class EdgeAccelerationHolonomicGoal : public BaseTebMultiEdge<3, const geometry_msgs::msg::Twist*> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeAccelerationHolonomicGoal() - { - _measurement = NULL; - this->resize(3); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig() and setGoalVelocity() on EdgeAccelerationGoal()"); - const VertexPose* pose_pre_goal = static_cast(_vertices[0]); - const VertexPose* pose_goal = static_cast(_vertices[1]); - const VertexTimeDiff* dt = static_cast(_vertices[2]); - - // VELOCITY & ACCELERATION - - Eigen::Vector2d diff = pose_goal->position() - pose_pre_goal->position(); - - double cos_theta1 = std::cos(pose_pre_goal->theta()); - double sin_theta1 = std::sin(pose_pre_goal->theta()); - - // transform pose2 into robot frame pose1 (inverse 2d rotation matrix) - double p1_dx = cos_theta1*diff.x() + sin_theta1*diff.y(); - double p1_dy = -sin_theta1*diff.x() + cos_theta1*diff.y(); - - double vel1_x = p1_dx / dt->dt(); - double vel1_y = p1_dy / dt->dt(); - double vel2_x = _measurement->linear.x; - double vel2_y = _measurement->linear.y; - - double acc_lin_x = (vel2_x - vel1_x) / dt->dt(); - double acc_lin_y = (vel2_y - vel1_y) / dt->dt(); - - _error[0] = penaltyBoundToInterval(acc_lin_x,cfg_->robot.acc_lim_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(acc_lin_y,cfg_->robot.acc_lim_y,cfg_->optim.penalty_epsilon); - - // ANGULAR ACCELERATION - double omega1 = g2o::normalize_theta(pose_goal->theta() - pose_pre_goal->theta()) / dt->dt(); - double omega2 = _measurement->angular.z; - double acc_rot = (omega2 - omega1) / dt->dt(); - - _error[2] = penaltyBoundToInterval(acc_rot,cfg_->robot.acc_lim_theta,cfg_->optim.penalty_epsilon); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeAccelerationGoal::computeError() translational: _error[0]=%f\n",_error[0]); - TEB_ASSERT_MSG(std::isfinite(_error[1]), "EdgeAccelerationGoal::computeError() strafing: _error[1]=%f\n",_error[1]); - TEB_ASSERT_MSG(std::isfinite(_error[2]), "EdgeAccelerationGoal::computeError() rotational: _error[2]=%f\n",_error[2]); - } - - - /** - * @brief Set the goal / final velocity that is taken into account for calculating the acceleration - * @param vel_goal twist message containing the translational and rotational velocity - */ - void setGoalVelocity(const geometry_msgs::msg::Twist& vel_goal) - { - _measurement = &vel_goal; - } - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - -}; // end namespace - -#endif /* EDGE_ACCELERATION_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h deleted file mode 100755 index 8224042..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_dynamic_obstacle.h +++ /dev/null @@ -1,154 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann, Franz Albers - *********************************************************************/ - -#ifndef EDGE_DYNAMICOBSTACLE_H -#define EDGE_DYNAMICOBSTACLE_H - -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/vertex_timediff.h" -#include "teb_local_planner/g2o_types/penalties.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/obstacles.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/robot_footprint_model.h" -#include "teb_local_planner/misc.h" - -namespace teb_local_planner -{ - -/** - * @class EdgeDynamicObstacle - * @brief Edge defining the cost function for keeping a distance from dynamic (moving) obstacles. - * - * The edge depends on two vertices \f$ \mathbf{s}_i, \Delta T_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyBelow}( dist2obstacle) \cdot weight \f$. \n - * \e dist2obstacle denotes the minimum distance to the obstacle trajectory (spatial and temporal). \n - * \e weight can be set using setInformation(). \n - * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow(). \n - * @see TebOptimalPlanner::AddEdgesDynamicObstacles - * @remarks Do not forget to call setTebConfig(), setVertexIdx() and - * @warning Experimental - */ -class EdgeDynamicObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeDynamicObstacle() : t_(0) - { - } - - /** - * @brief Construct edge and specify the time for its associated pose (neccessary for computeError). - * @param t_ Estimated time until current pose is reached - */ - EdgeDynamicObstacle(double t) : t_(t) - { - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeDynamicObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - double dist = robot_model_->estimateSpatioTemporalDistance(bandpt->pose(), _measurement, t_); - - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.dynamic_obstacle_inflation_dist, 0.0); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeDynamicObstacle::computeError() _error[0]=%f\n",_error[0]); - } - - - /** - * @brief Set Obstacle for the underlying cost function - * @param obstacle Const pointer to an Obstacle or derived Obstacle - */ - void setObstacle(const Obstacle* obstacle) - { - _measurement = obstacle; - } - - /** - * @brief Set pointer to the robot model - * @param robot_model Robot model required for distance calculation - */ - void setRobotModel(const BaseRobotFootprintModel* robot_model) - { - robot_model_ = robot_model; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param robot_model Robot model required for distance calculation - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) - { - cfg_ = &cfg; - robot_model_ = robot_model; - _measurement = obstacle; - } - -protected: - - const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model - double t_; //!< Estimated time until current pose is reached - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - - -} // end namespace - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h deleted file mode 100755 index 012a865..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_kinematics.h +++ /dev/null @@ -1,231 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef _EDGE_KINEMATICS_H -#define _EDGE_KINEMATICS_H - -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/penalties.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/misc.h" - -#include - -namespace teb_local_planner -{ - -/** - * @class EdgeKinematicsDiffDrive - * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a differential drive mobile robot. - * - * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation - * of the non-holonomic constraint: - * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. - * - * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n - * A second equation is implemented to penalize backward motions (second element of the error /cost vector). \n - * The \e weight can be set using setInformation(): Matrix element 2,2: (A value ~1 allows backward driving, but penalizes it slighly). \n - * The dimension of the error / cost vector is 2: the first component represents the nonholonomic constraint cost, - * the second one backward-drive cost. - * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsCarlike - * @remarks Do not forget to call setTebConfig() - */ -class EdgeKinematicsDiffDrive : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeKinematicsDiffDrive() - { - this->setMeasurement(0.); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - // non holonomic constraint - _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); - - // positive-drive-direction constraint - Eigen::Vector2d angle_vec ( cos(conf1->theta()), sin(conf1->theta()) ); - _error[1] = penaltyBoundFromBelow(deltaS.dot(angle_vec), 0,0); - // epsilon=0, otherwise it pushes the first bandpoints away from start - - TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsDiffDrive::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); - } - -#ifdef USE_ANALYTIC_JACOBI -#if 1 - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsDiffDrive()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - double cos1 = cos(conf1->theta()); - double cos2 = cos(conf2->theta()); - double sin1 = sin(conf1->theta()); - double sin2 = sin(conf2->theta()); - double aux1 = sin1 + sin2; - double aux2 = cos1 + cos2; - - double dd_error_1 = deltaS[0]*cos1; - double dd_error_2 = deltaS[1]*sin1; - double dd_dev = penaltyBoundFromBelowDerivative(dd_error_1+dd_error_2, 0,0); - - double dev_nh_abs = g2o::sign( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); - - // conf1 - _jacobianOplusXi(0,0) = aux1 * dev_nh_abs; // nh x1 - _jacobianOplusXi(0,1) = -aux2 * dev_nh_abs; // nh y1 - _jacobianOplusXi(1,0) = -cos1 * dd_dev; // drive-dir x1 - _jacobianOplusXi(1,1) = -sin1 * dd_dev; // drive-dir y1 - _jacobianOplusXi(0,2) = (-dd_error_2 - dd_error_1) * dev_nh_abs; // nh angle - _jacobianOplusXi(1,2) = ( -sin1*deltaS[0] + cos1*deltaS[1] ) * dd_dev; // drive-dir angle1 - - // conf2 - _jacobianOplusXj(0,0) = -aux1 * dev_nh_abs; // nh x2 - _jacobianOplusXj(0,1) = aux2 * dev_nh_abs; // nh y2 - _jacobianOplusXj(1,0) = cos1 * dd_dev; // drive-dir x2 - _jacobianOplusXj(1,1) = sin1 * dd_dev; // drive-dir y2 - _jacobianOplusXj(0,2) = (-sin2*deltaS[1] - cos2*deltaS[0]) * dev_nh_abs; // nh angle - _jacobianOplusXj(1,2) = 0; // drive-dir angle1 - } -#endif -#endif - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - -/** - * @class EdgeKinematicsCarlike - * @brief Edge defining the cost function for satisfying the non-holonomic kinematics of a carlike mobile robot. - * - * The edge depends on two vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1} \f$ and minimizes a geometric interpretation - * of the non-holonomic constraint: - * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. - * - * The definition is identically to the one of the differential drive robot. - * Additionally, this edge incorporates a minimum turning radius that is required by carlike robots. - * The turning radius is defined by \f$ r=v/omega \f$. - * - * The \e weight can be set using setInformation(): Matrix element 1,1: (Choose a very high value: ~1000). \n - * The second equation enforces a minimum turning radius. - * The \e weight can be set using setInformation(): Matrix element 2,2. \n - * The dimension of the error / cost vector is 3: the first component represents the nonholonomic constraint cost, - * the second one backward-drive cost and the third one the minimum turning radius - * @see TebOptimalPlanner::AddEdgesKinematics, EdgeKinematicsDiffDrive - * @remarks Bounding the turning radius from below is not affected by the penalty_epsilon parameter, - * the user might add an extra margin to the min_turning_radius param. - * @remarks Do not forget to call setTebConfig() - */ -class EdgeKinematicsCarlike : public BaseTebBinaryEdge<2, double, VertexPose, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeKinematicsCarlike() - { - this->setMeasurement(0.); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeKinematicsCarlike()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - // non holonomic constraint - _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); - - // limit minimum turning radius - double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); - if (angle_diff == 0) - _error[1] = 0; // straight line motion - else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius - _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0); - else - _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0); - // This edge is not affected by the epsilon parameter, the user might add an exra margin to the min_turning_radius parameter. - - TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeKinematicsCarlike::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - - -} // end namespace - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h deleted file mode 100755 index 23c8bc4..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_obstacle.h +++ /dev/null @@ -1,295 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ -#ifndef EDGE_OBSTACLE_H_ -#define EDGE_OBSTACLE_H_ - -#include "teb_local_planner/obstacles.h" -#include "teb_local_planner/robot_footprint_model.h" -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/g2o_types/penalties.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/misc.h" - - -namespace teb_local_planner -{ - -/** - * @class EdgeObstacle - * @brief Edge defining the cost function for keeping a minimum distance from obstacles. - * - * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n - * \e dist2point denotes the minimum distance to the point obstacle. \n - * \e weight can be set using setInformation(). \n - * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n - * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle - * @remarks Do not forget to call setTebConfig() and setObstacle() - */ -class EdgeObstacle : public BaseTebUnaryEdge<1, const Obstacle*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeObstacle() - { - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); - - // Original obstacle cost. - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - - if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) - { - // Optional non-linear cost. Note the max cost (before weighting) is - // the same as the straight line version and that all other costs are - // below the straight line (for positive exponent), so it may be - // necessary to increase weight_obstacle and/or the inflation_weight - // when using larger exponents. - _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); - } - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeObstacle::computeError() _error[0]=%f\n",_error[0]); - } - -#ifdef USE_ANALYTIC_JACOBI -#if 0 - - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgePointObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - Eigen::Vector2d deltaS = *_measurement - bandpt->position(); - double angdiff = atan2(deltaS[1],deltaS[0])-bandpt->theta(); - - double dist_squared = deltaS.squaredNorm(); - double dist = sqrt(dist_squared); - - double aux0 = sin(angdiff); - double dev_left_border = penaltyBoundFromBelowDerivative(dist*fabs(aux0),cfg_->obstacles.min_obstacle_dist,cfg_->optim.penalty_epsilon); - - if (dev_left_border==0) - { - _jacobianOplusXi( 0 , 0 ) = 0; - _jacobianOplusXi( 0 , 1 ) = 0; - _jacobianOplusXi( 0 , 2 ) = 0; - return; - } - - double aux1 = -fabs(aux0) / dist; - double dev_norm_x = deltaS[0]*aux1; - double dev_norm_y = deltaS[1]*aux1; - - double aux2 = cos(angdiff) * g2o::sign(aux0); - double aux3 = aux2 / dist_squared; - double dev_proj_x = aux3 * deltaS[1] * dist; - double dev_proj_y = -aux3 * deltaS[0] * dist; - double dev_proj_angle = -aux2; - - _jacobianOplusXi( 0 , 0 ) = dev_left_border * ( dev_norm_x + dev_proj_x ); - _jacobianOplusXi( 0 , 1 ) = dev_left_border * ( dev_norm_y + dev_proj_y ); - _jacobianOplusXi( 0 , 2 ) = dev_left_border * dev_proj_angle; - } -#endif -#endif - - /** - * @brief Set pointer to associated obstacle for the underlying cost function - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setObstacle(const Obstacle* obstacle) - { - _measurement = obstacle; - } - - /** - * @brief Set pointer to the robot model - * @param robot_model Robot model required for distance calculation - */ - void setRobotModel(const BaseRobotFootprintModel* robot_model) - { - robot_model_ = robot_model; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param robot_model Robot model required for distance calculation - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) - { - cfg_ = &cfg; - robot_model_ = robot_model; - _measurement = obstacle; - } - -protected: - - const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -/** - * @class EdgeInflatedObstacle - * @brief Edge defining the cost function for keeping a minimum distance from inflated obstacles. - * - * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyBelow}( dist2point, min_obstacle_dist ) \cdot weight_inflation \f$. \n - * Additional, a second penalty is provided with \n - * \f$ \min \textrm{penaltyBelow}( dist2point, inflation_dist ) \cdot weight_inflation \f$. - * It is assumed that inflation_dist > min_obstacle_dist and weight_inflation << weight_inflation. - * \e dist2point denotes the minimum distance to the point obstacle. \n - * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n - * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeObstacle - * @remarks Do not forget to call setTebConfig() and setObstacle() - */ -class EdgeInflatedObstacle : public BaseTebUnaryEdge<2, const Obstacle*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeInflatedObstacle() - { - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeInflatedObstacle()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); - - // Original "straight line" obstacle cost. The max possible value - // before weighting is min_obstacle_dist - _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); - - if (cfg_->optim.obstacle_cost_exponent != 1.0 && cfg_->obstacles.min_obstacle_dist > 0.0) - { - // Optional non-linear cost. Note the max cost (before weighting) is - // the same as the straight line version and that all other costs are - // below the straight line (for positive exponent), so it may be - // necessary to increase weight_obstacle and/or the inflation_weight - // when using larger exponents. - _error[0] = cfg_->obstacles.min_obstacle_dist * std::pow(_error[0] / cfg_->obstacles.min_obstacle_dist, cfg_->optim.obstacle_cost_exponent); - } - - // Additional linear inflation cost - _error[1] = penaltyBoundFromBelow(dist, cfg_->obstacles.inflation_dist, 0.0); - - - TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]), "EdgeInflatedObstacle::computeError() _error[0]=%f, _error[1]=%f\n",_error[0], _error[1]); - } - - /** - * @brief Set pointer to associated obstacle for the underlying cost function - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setObstacle(const Obstacle* obstacle) - { - _measurement = obstacle; - } - - /** - * @brief Set pointer to the robot model - * @param robot_model Robot model required for distance calculation - */ - void setRobotModel(const BaseRobotFootprintModel* robot_model) - { - robot_model_ = robot_model; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param robot_model Robot model required for distance calculation - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) - { - cfg_ = &cfg; - robot_model_ = robot_model; - _measurement = obstacle; - } - -protected: - - const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -} // end namespace - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h deleted file mode 100755 index 6e5ec9e..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_prefer_rotdir.h +++ /dev/null @@ -1,115 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ -#ifndef EDGE_PREFER_ROTDIR_H_ -#define EDGE_PREFER_ROTDIR_H_ - -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/g2o_types/penalties.h" -#include "g2o/core/base_unary_edge.h" -#include "teb_local_planner/misc.h" - -namespace teb_local_planner -{ - -/** - * @class EdgePreferRotDir - * @brief Edge defining the cost function for penalzing a specified turning direction, in particular left resp. right turns - * - * The edge depends on two consecutive vertices \f$ \mathbf{s}_i, \mathbf{s}_{i+1} \f$ and penalizes a given rotation direction - * based on the \e weight and \e dir (\f$ dir \in \{-1,1\} \f$) - * \e dir should be +1 to prefer left rotations and -1 to prefer right rotations \n - * \e weight can be set using setInformation(). \n - * @see TebOptimalPlanner::AddEdgePreferRotDir - */ -class EdgePreferRotDir : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgePreferRotDir() - { - _measurement = 1; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - - _error[0] = penaltyBoundFromBelow( _measurement*g2o::normalize_theta(conf2->theta()-conf1->theta()) , 0, 0); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgePreferRotDir::computeError() _error[0]=%f\n",_error[0]); - } - - /** - * @brief Specify the prefered direction of rotation - * @param dir +1 to prefer the left side, -1 to prefer the right side - */ - void setRotDir(double dir) - { - _measurement = dir; - } - - /** Prefer rotations to the right */ - void preferRight() {_measurement = -1;} - - /** Prefer rotations to the right */ - void preferLeft() {_measurement = 1;} - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -} // end namespace - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h deleted file mode 100644 index 8d5bb87..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_shortest_path.h +++ /dev/null @@ -1,88 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_SHORTEST_PATH_H_ -#define EDGE_SHORTEST_PATH_H_ - -#include - -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/misc.h" - -#include - -namespace teb_local_planner { - -/** - * @class EdgeShortestPath - * @brief Edge defining the cost function for minimizing the Euclidean distance between two consectuive poses. - * - * @see TebOptimalPlanner::AddEdgesShortestPath - */ -class EdgeShortestPath : public BaseTebBinaryEdge<1, double, VertexPose, VertexPose> { -public: - /** - * @brief Construct edge. - */ - EdgeShortestPath() { this->setMeasurement(0.); } - - /** - * @brief Actual cost function - */ - void computeError() { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeShortestPath()"); - const VertexPose *pose1 = static_cast(_vertices[0]); - const VertexPose *pose2 = static_cast(_vertices[1]); - _error[0] = (pose2->position() - pose1->position()).norm(); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeShortestPath::computeError() _error[0]=%f\n", _error[0]); - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // end namespace - -#endif /* EDGE_SHORTEST_PATH_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h deleted file mode 100644 index 0f7671b..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_time_optimal.h +++ /dev/null @@ -1,117 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_TIMEOPTIMAL_H_ -#define EDGE_TIMEOPTIMAL_H_ - -#include - -//#include - -#include "teb_local_planner/g2o_types/vertex_timediff.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/g2o_types/penalties.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/misc.h" - -#include - -namespace teb_local_planner -{ - - -/** - * @class EdgeTimeOptimal - * @brief Edge defining the cost function for minimizing transition time of the trajectory. - * - * The edge depends on a single vertex \f$ \Delta T_i \f$ and minimizes: \n - * \f$ \min \Delta T_i^2 \cdot scale \cdot weight \f$. \n - * \e scale is determined using the penaltyEquality() function, since we experiences good convergence speeds with it. \n - * \e weight can be set using setInformation() (something around 1.0 seems to be fine). \n - * @see TebOptimalPlanner::AddEdgesTimeOptimal - * @remarks Do not forget to call setTebConfig() - */ -class EdgeTimeOptimal : public BaseTebUnaryEdge<1, double, VertexTimeDiff> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeTimeOptimal() - { - this->setMeasurement(0.); - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); - const VertexTimeDiff* timediff = static_cast(_vertices[0]); - - _error[0] = timediff->dt(); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeTimeOptimal::computeError() _error[0]=%f\n",_error[0]); - } - -#ifdef USE_ANALYTIC_JACOBI - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeTimeOptimal()"); - _jacobianOplusXi( 0 , 0 ) = 1; - } -#endif - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -}; // end namespace - -#endif /* EDGE_TIMEOPTIMAL_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h deleted file mode 100755 index 29956c0..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity.h +++ /dev/null @@ -1,275 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef EDGE_VELOCITY_H -#define EDGE_VELOCITY_H - -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/vertex_timediff.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/g2o_types/penalties.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/misc.h" - -#include - -#include - -namespace teb_local_planner -{ - - -/** - * @class EdgeVelocity - * @brief Edge defining the cost function for limiting the translational and rotational velocity. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [v,omega]^T ) \cdot weight \f$. \n - * \e v is calculated using the difference quotient and the position parts of both poses. \n - * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 2: the first component represents the translational velocity and - * the second one the rotational velocity. - * @see TebOptimalPlanner::AddEdgesVelocity - * @remarks Do not forget to call setTebConfig() - */ -class EdgeVelocity : public BaseTebMultiEdge<2, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeVelocity() - { - this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* deltaT = static_cast(_vertices[2]); - - const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position(); - - double dist = deltaS.norm(); - const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta()); - if (cfg_->trajectory.exact_arc_length && angle_diff != 0) - { - double radius = dist/(2*sin(angle_diff/2)); - dist = fabs( angle_diff * radius ); // actual arg length! - } - double vel = dist / deltaT->estimate(); - -// vel *= g2o::sign(deltaS[0]*cos(conf1->theta()) + deltaS[1]*sin(conf1->theta())); // consider direction - vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction - - const double omega = angle_diff / deltaT->estimate(); - - _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeVelocity::computeError() _error[0]=%f _error[1]=%f\n",_error[0],_error[1]); - } - -#ifdef USE_ANALYTIC_JACOBI -#if 0 //TODO the hardcoded jacobian does not include the changing direction (just the absolute value) - // Change accordingly... - - /** - * @brief Jacobi matrix of the cost function specified in computeError(). - */ - void linearizeOplus() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocity()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* deltaT = static_cast(_vertices[2]); - - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - double dist = deltaS.norm(); - double aux1 = dist*deltaT->estimate(); - double aux2 = 1/deltaT->estimate(); - - double vel = dist * aux2; - double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) * aux2; - - double dev_border_vel = penaltyBoundToIntervalDerivative(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); - double dev_border_omega = penaltyBoundToIntervalDerivative(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - _jacobianOplus[0].resize(2,3); // conf1 - _jacobianOplus[1].resize(2,3); // conf2 - _jacobianOplus[2].resize(2,1); // deltaT - -// if (aux1==0) aux1=1e-6; -// if (aux2==0) aux2=1e-6; - - if (dev_border_vel!=0) - { - double aux3 = dev_border_vel / aux1; - _jacobianOplus[0](0,0) = -deltaS[0] * aux3; // vel x1 - _jacobianOplus[0](0,1) = -deltaS[1] * aux3; // vel y1 - _jacobianOplus[1](0,0) = deltaS[0] * aux3; // vel x2 - _jacobianOplus[1](0,1) = deltaS[1] * aux3; // vel y2 - _jacobianOplus[2](0,0) = -vel * aux2 * dev_border_vel; // vel deltaT - } - else - { - _jacobianOplus[0](0,0) = 0; // vel x1 - _jacobianOplus[0](0,1) = 0; // vel y1 - _jacobianOplus[1](0,0) = 0; // vel x2 - _jacobianOplus[1](0,1) = 0; // vel y2 - _jacobianOplus[2](0,0) = 0; // vel deltaT - } - - if (dev_border_omega!=0) - { - double aux4 = aux2 * dev_border_omega; - _jacobianOplus[2](1,0) = -omega * aux4; // omega deltaT - _jacobianOplus[0](1,2) = -aux4; // omega angle1 - _jacobianOplus[1](1,2) = aux4; // omega angle2 - } - else - { - _jacobianOplus[2](1,0) = 0; // omega deltaT - _jacobianOplus[0](1,2) = 0; // omega angle1 - _jacobianOplus[1](1,2) = 0; // omega angle2 - } - - _jacobianOplus[0](1,0) = 0; // omega x1 - _jacobianOplus[0](1,1) = 0; // omega y1 - _jacobianOplus[1](1,0) = 0; // omega x2 - _jacobianOplus[1](1,1) = 0; // omega y2 - _jacobianOplus[0](0,2) = 0; // vel angle1 - _jacobianOplus[1](0,2) = 0; // vel angle2 - } -#endif -#endif - - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - - - - -/** - * @class EdgeVelocityHolonomic - * @brief Edge defining the cost function for limiting the translational and rotational velocity according to x,y and theta. - * - * The edge depends on three vertices \f$ \mathbf{s}_i, \mathbf{s}_{ip1}, \Delta T_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyInterval}( [vx,vy,omega]^T ) \cdot weight \f$. \n - * \e vx denotes the translational velocity w.r.t. x-axis (computed using finite differneces). \n - * \e vy denotes the translational velocity w.r.t. y-axis (computed using finite differneces). \n - * \e omega is calculated using the difference quotient of both yaw angles followed by a normalization to [-pi, pi]. \n - * \e weight can be set using setInformation(). \n - * \e penaltyInterval denotes the penalty function, see penaltyBoundToInterval(). \n - * The dimension of the error / cost vector is 3: the first component represents the translational velocity w.r.t. x-axis, - * the second one w.r.t. the y-axis and the third one the rotational velocity. - * @see TebOptimalPlanner::AddEdgesVelocity - * @remarks Do not forget to call setTebConfig() - */ -class EdgeVelocityHolonomic : public BaseTebMultiEdge<3, double> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeVelocityHolonomic() - { - this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_, "You must call setTebConfig on EdgeVelocityHolonomic()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* deltaT = static_cast(_vertices[2]); - Eigen::Vector2d deltaS = conf2->position() - conf1->position(); - - double cos_theta1 = std::cos(conf1->theta()); - double sin_theta1 = std::sin(conf1->theta()); - - // transform conf2 into current robot frame conf1 (inverse 2d rotation matrix) - double r_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y(); - double r_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y(); - - double vx = r_dx / deltaT->estimate(); - double vy = r_dy / deltaT->estimate(); - double omega = g2o::normalize_theta(conf2->theta() - conf1->theta()) / deltaT->estimate(); - - _error[0] = penaltyBoundToInterval(vx, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x, cfg_->optim.penalty_epsilon); - _error[1] = penaltyBoundToInterval(vy, cfg_->robot.max_vel_y, 0.0); // we do not apply the penalty epsilon here, since the velocity could be close to zero - _error[2] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); - - TEB_ASSERT_MSG(std::isfinite(_error[0]) && std::isfinite(_error[1]) && std::isfinite(_error[2]), - "EdgeVelocityHolonomic::computeError() _error[0]=%f _error[1]=%f _error[2]=%f\n",_error[0],_error[1],_error[2]); - } - - -public: - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -} // end namespace - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h deleted file mode 100644 index 1dff242..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_velocity_obstacle_ratio.h +++ /dev/null @@ -1,127 +0,0 @@ -#pragma once - - -#include -#include -#include -#include -#include - -namespace teb_local_planner -{ - - -/** - * @class EdgeVelocityObstacleRatio - * @brief Edge defining the cost function for keeping a minimum distance from obstacles. - * - * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n - * \f$ \min \textrm{penaltyBelow}( dist2point ) \cdot weight \f$. \n - * \e dist2point denotes the minimum distance to the point obstacle. \n - * \e weight can be set using setInformation(). \n - * \e penaltyBelow denotes the penalty function, see penaltyBoundFromBelow() \n - * @see TebOptimalPlanner::AddEdgesObstacles, TebOptimalPlanner::EdgeInflatedObstacle - * @remarks Do not forget to call setTebConfig() and setObstacle() - */ -class EdgeVelocityObstacleRatio : public BaseTebMultiEdge<2, const Obstacle*> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeVelocityObstacleRatio() : - robot_model_(nullptr) - { - // The three vertices are two poses and one time difference - this->resize(3); // Since we derive from a g2o::BaseMultiEdge, set the desired number of vertices - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement && robot_model_, "You must call setTebConfig(), setObstacle() and setRobotModel() on EdgeVelocityObstacleRatio()"); - const VertexPose* conf1 = static_cast(_vertices[0]); - const VertexPose* conf2 = static_cast(_vertices[1]); - const VertexTimeDiff* deltaT = static_cast(_vertices[2]); - - const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position(); - - double dist = deltaS.norm(); - const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta()); - if (cfg_->trajectory.exact_arc_length && angle_diff != 0) - { - double radius = dist/(2*sin(angle_diff/2)); - dist = fabs( angle_diff * radius ); // actual arg length! - } - double vel = dist / deltaT->estimate(); - - vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction - - const double omega = angle_diff / deltaT->estimate(); - - double dist_to_obstacle = robot_model_->calculateDistance(conf1->pose(), _measurement); - - double ratio; - if (dist_to_obstacle < cfg_->obstacles.obstacle_proximity_lower_bound) - ratio = 0; - else if (dist_to_obstacle > cfg_->obstacles.obstacle_proximity_upper_bound) - ratio = 1; - else - ratio = (dist_to_obstacle - cfg_->obstacles.obstacle_proximity_lower_bound) / - (cfg_->obstacles.obstacle_proximity_upper_bound - cfg_->obstacles.obstacle_proximity_lower_bound); - ratio *= cfg_->obstacles.obstacle_proximity_ratio_max_vel; - - const double max_vel_fwd = ratio * cfg_->robot.max_vel_x; - const double max_omega = ratio * cfg_->robot.max_vel_theta; - _error[0] = penaltyBoundToInterval(vel, max_vel_fwd, 0); - _error[1] = penaltyBoundToInterval(omega, max_omega, 0); - - TEB_ASSERT_MSG(std::isfinite(_error[0]) || std::isfinite(_error[1]), "EdgeVelocityObstacleRatio::computeError() _error[0]=%f , _error[1]=%f\n",_error[0],_error[1]); - } - - /** - * @brief Set pointer to associated obstacle for the underlying cost function - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setObstacle(const Obstacle* obstacle) - { - _measurement = obstacle; - } - - /** - * @brief Set pointer to the robot model - * @param robot_model Robot model required for distance calculation - */ - void setRobotModel(const BaseRobotFootprintModel* robot_model) - { - robot_model_ = robot_model; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param robot_model Robot model required for distance calculation - * @param obstacle 2D position vector containing the position of the obstacle - */ - void setParameters(const TebConfig& cfg, const BaseRobotFootprintModel* robot_model, const Obstacle* obstacle) - { - cfg_ = &cfg; - robot_model_ = robot_model; - _measurement = obstacle; - } - -protected: - - const BaseRobotFootprintModel* robot_model_; //!< Store pointer to robot_model - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - -} // end namespace - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h deleted file mode 100755 index 5575d0f..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/edge_via_point.h +++ /dev/null @@ -1,121 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ -#ifndef EDGE_VIA_POINT_H_ -#define EDGE_VIA_POINT_H_ - -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/base_teb_edges.h" -#include "teb_local_planner/misc.h" - -#include "g2o/core/base_unary_edge.h" - - -namespace teb_local_planner -{ - -/** - * @class EdgeViaPoint - * @brief Edge defining the cost function for pushing a configuration towards a via point - * - * The edge depends on a single vertex \f$ \mathbf{s}_i \f$ and minimizes: \n - * \f$ \min dist2point \cdot weight \f$. \n - * \e dist2point denotes the distance to the via point. \n - * \e weight can be set using setInformation(). \n - * @see TebOptimalPlanner::AddEdgesViaPoints - * @remarks Do not forget to call setTebConfig() and setViaPoint() - */ -class EdgeViaPoint : public BaseTebUnaryEdge<1, const Eigen::Vector2d*, VertexPose> -{ -public: - - /** - * @brief Construct edge. - */ - EdgeViaPoint() - { - _measurement = NULL; - } - - /** - * @brief Actual cost function - */ - void computeError() - { - TEB_ASSERT_MSG(cfg_ && _measurement, "You must call setTebConfig(), setViaPoint() on EdgeViaPoint()"); - const VertexPose* bandpt = static_cast(_vertices[0]); - - _error[0] = (bandpt->position() - *_measurement).norm(); - - TEB_ASSERT_MSG(std::isfinite(_error[0]), "EdgeViaPoint::computeError() _error[0]=%f\n",_error[0]); - } - - /** - * @brief Set pointer to associated via point for the underlying cost function - * @param via_point 2D position vector containing the position of the via point - */ - void setViaPoint(const Eigen::Vector2d* via_point) - { - _measurement = via_point; - } - - /** - * @brief Set all parameters at once - * @param cfg TebConfig class - * @param via_point 2D position vector containing the position of the via point - */ - void setParameters(const TebConfig& cfg, const Eigen::Vector2d* via_point) - { - cfg_ = &cfg; - _measurement = via_point; - } - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -} // end namespace - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h deleted file mode 100755 index 6870558..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/penalties.h +++ /dev/null @@ -1,193 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef PENALTIES_H -#define PENALTIES_H - -#include -#include -#include - -namespace teb_local_planner -{ - -/** - * @brief Linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ - * @param var The scalar that should be bounded - * @param a lower and upper absolute bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToIntervalDerivative - * @return Penalty / cost value that is nonzero if the constraint is not satisfied - */ -inline double penaltyBoundToInterval(const double& var,const double& a,const double& epsilon) -{ - if (var < -a+epsilon) - { - return (-var - (a - epsilon)); - } - if (var <= a-epsilon) - { - return 0.; - } - else - { - return (var - (a - epsilon)); - } -} - -/** - * @brief Linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param b upper bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToIntervalDerivative - * @return Penalty / cost value that is nonzero if the constraint is not satisfied - */ -inline double penaltyBoundToInterval(const double& var,const double& a, const double& b, const double& epsilon) -{ - if (var < a+epsilon) - { - return (-var + (a + epsilon)); - } - if (var <= b-epsilon) - { - return 0.; - } - else - { - return (var - (b - epsilon)); - } -} - - -/** - * @brief Linear penalty function for bounding \c var from below: \f$ a < var \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundFromBelowDerivative - * @return Penalty / cost value that is nonzero if the constraint is not satisfied - */ -inline double penaltyBoundFromBelow(const double& var, const double& a,const double& epsilon) -{ - if (var >= a+epsilon) - { - return 0.; - } - else - { - return (-var + (a+epsilon)); - } -} - -/** - * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ -a < var < a \f$ - * @param var The scalar that should be bounded - * @param a lower and upper absolute bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToInterval - * @return Derivative of the penalty function w.r.t. \c var - */ -inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& epsilon) -{ - if (var < -a+epsilon) - { - return -1; - } - if (var <= a-epsilon) - { - return 0.; - } - else - { - return 1; - } -} - -/** - * @brief Derivative of the linear penalty function for bounding \c var to the interval \f$ a < var < b \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param b upper bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundToInterval - * @return Derivative of the penalty function w.r.t. \c var - */ -inline double penaltyBoundToIntervalDerivative(const double& var,const double& a, const double& b, const double& epsilon) -{ - if (var < a+epsilon) - { - return -1; - } - if (var <= b-epsilon) - { - return 0.; - } - else - { - return 1; - } -} - - -/** - * @brief Derivative of the linear penalty function for bounding \c var from below: \f$ a < var \f$ - * @param var The scalar that should be bounded - * @param a lower bound - * @param epsilon safty margin (move bound to the interior of the interval) - * @see penaltyBoundFromBelow - * @return Derivative of the penalty function w.r.t. \c var - */ -inline double penaltyBoundFromBelowDerivative(const double& var, const double& a,const double& epsilon) -{ - if (var >= a+epsilon) - { - return 0.; - } - else - { - return -1; - } -} - - -} // namespace teb_local_planner - - -#endif // PENALTIES_H diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h deleted file mode 100755 index 781a272..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_pose.h +++ /dev/null @@ -1,229 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef VERTEX_POSE_H_ -#define VERTEX_POSE_H_ - -#include -#include -#include -#include - -#include "teb_local_planner/pose_se2.h" - -namespace teb_local_planner -{ - -/** - * @class VertexPose - * @brief This class stores and wraps a SE2 pose (position and orientation) into a vertex that can be optimized via g2o - * @see PoseSE2 - * @see VertexTimeDiff - */ -class VertexPose : public g2o::BaseVertex<3, PoseSE2 > -{ -public: - - /** - * @brief Default constructor - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(bool fixed = false) - { - setToOriginImpl(); - setFixed(fixed); - } - - /** - * @brief Construct pose using a given PoseSE2 - * @param pose PoseSE2 defining the pose [x, y, angle_rad] - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(const PoseSE2& pose, bool fixed = false) - { - _estimate = pose; - setFixed(fixed); - } - - /** - * @brief Construct pose using a given 2D position vector and orientation - * @param position Eigen::Vector2d containing x and y coordinates - * @param theta yaw-angle - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(const Eigen::Ref& position, double theta, bool fixed = false) - { - _estimate.position() = position; - _estimate.theta() = theta; - setFixed(fixed); - } - - /** - * @brief Construct pose using single components x, y, and the yaw angle - * @param x x-coordinate - * @param y y-coordinate - * @param theta yaw angle in rad - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexPose(double x, double y, double theta, bool fixed = false) - { - _estimate.x() = x; - _estimate.y() = y; - _estimate.theta() = theta; - setFixed(fixed); - } - - /** - * @brief Access the pose - * @see estimate - * @return reference to the PoseSE2 estimate - */ - inline PoseSE2& pose() {return _estimate;} - - /** - * @brief Access the pose (read-only) - * @see estimate - * @return const reference to the PoseSE2 estimate - */ - inline const PoseSE2& pose() const {return _estimate;} - - - /** - * @brief Access the 2D position part - * @see estimate - * @return reference to the 2D position part - */ - inline Eigen::Vector2d& position() {return _estimate.position();} - - /** - * @brief Access the 2D position part (read-only) - * @see estimate - * @return const reference to the 2D position part - */ - inline const Eigen::Vector2d& position() const {return _estimate.position();} - - /** - * @brief Access the x-coordinate the pose - * @return reference to the x-coordinate - */ - inline double& x() {return _estimate.x();} - - /** - * @brief Access the x-coordinate the pose (read-only) - * @return const reference to the x-coordinate - */ - inline const double& x() const {return _estimate.x();} - - /** - * @brief Access the y-coordinate the pose - * @return reference to the y-coordinate - */ - inline double& y() {return _estimate.y();} - - /** - * @brief Access the y-coordinate the pose (read-only) - * @return const reference to the y-coordinate - */ - inline const double& y() const {return _estimate.y();} - - /** - * @brief Access the orientation part (yaw angle) of the pose - * @return reference to the yaw angle - */ - inline double& theta() {return _estimate.theta();} - - /** - * @brief Access the orientation part (yaw angle) of the pose (read-only) - * @return const reference to the yaw angle - */ - inline const double& theta() const {return _estimate.theta();} - - /** - * @brief Set the underlying estimate (2D vector) to zero. - */ - virtual void setToOriginImpl() override - { - _estimate.setZero(); - } - - /** - * @brief Define the update increment \f$ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x} \f$. - * A simple addition for the position. - * The angle is first added to the previous estimated angle and afterwards normalized to the interval \f$ [-\pi \pi] \f$ - * @param update increment that should be added to the previous esimate - */ - virtual void oplusImpl(const double* update) override - { - _estimate.plus(update); - } - - /** - * @brief Read an estimate from an input stream. - * First the x-coordinate followed by y and the yaw angle. - * @param is input stream - * @return always \c true - */ - virtual bool read(std::istream& is) override - { - is >> _estimate.x() >> _estimate.y() >> _estimate.theta(); - return true; - } - - /** - * @brief Write the estimate to an output stream - * First the x-coordinate followed by y and the yaw angle. - * @param os output stream - * @return \c true if the export was successful, otherwise \c false - */ - virtual bool write(std::ostream& os) const override - { - os << _estimate.x() << " " << _estimate.y() << " " << _estimate.theta(); - return os.good(); - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} - -#endif // VERTEX_POSE_H_ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h deleted file mode 100755 index 4eead6c..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/g2o_types/vertex_timediff.h +++ /dev/null @@ -1,145 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Notes: - * The following class is derived from a class defined by the - * g2o-framework. g2o is licensed under the terms of the BSD License. - * Refer to the base class source for detailed licensing information. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef VERTEX_TIMEDIFF_H -#define VERTEX_TIMEDIFF_H - - -#include "g2o/config.h" -#include "g2o/core/base_vertex.h" -#include "g2o/core/hyper_graph_action.h" - -#include - -namespace teb_local_planner -{ - -/** - * @class VertexTimeDiff - * @brief This class stores and wraps a time difference \f$ \Delta T \f$ into a vertex that can be optimized via g2o - */ -class VertexTimeDiff : public g2o::BaseVertex<1, double> -{ -public: - - /** - * @brief Default constructor - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexTimeDiff(bool fixed = false) - { - setToOriginImpl(); - setFixed(fixed); - } - - /** - * @brief Construct the TimeDiff vertex with a value - * @param dt time difference value of the vertex - * @param fixed if \c true, this vertex is considered fixed during optimization [default: \c false] - */ - VertexTimeDiff(double dt, bool fixed = false) - { - _estimate = dt; - setFixed(fixed); - } - - /** - * @brief Access the timediff value of the vertex - * @see estimate - * @return reference to dt - */ - inline double& dt() {return _estimate;} - - /** - * @brief Access the timediff value of the vertex (read-only) - * @see estimate - * @return const reference to dt - */ - inline const double& dt() const {return _estimate;} - - /** - * @brief Set the underlying TimeDiff estimate \f$ \Delta T \f$ to default. - */ - virtual void setToOriginImpl() override - { - _estimate = 0.1; - } - - /** - * @brief Define the update increment \f$ \Delta T_{k+1} = \Delta T_k + update \f$. - * A simple addition implements what we want. - * @param update increment that should be added to the previous esimate - */ - virtual void oplusImpl(const double* update) override - { - _estimate += *update; - } - - /** - * @brief Read an estimate of \f$ \Delta T \f$ from an input stream - * @param is input stream - * @return always \c true - */ - virtual bool read(std::istream& is) override - { - is >> _estimate; - return true; - } - - /** - * @brief Write the estimate \f$ \Delta T \f$ to an output stream - * @param os output stream - * @return \c true if the export was successful, otherwise \c false - */ - virtual bool write(std::ostream& os) const override - { - os << estimate(); - return os.good(); - } - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/graph_search.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/graph_search.h deleted file mode 100644 index 3c4bd7c..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/graph_search.h +++ /dev/null @@ -1,215 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Authors: Christoph Rösmann, Franz Albers - *********************************************************************/ - -#ifndef GRAPH_SEARCH_INTERFACE_H -#define GRAPH_SEARCH_INTERFACE_H - -#ifdef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS - #include -#else - // Workaround for a bug in boost graph library (concerning directed graphs), boost version 1.48: - // boost::add_vertex requires a move constructor/assignment operator in one of the underlying boost objects if C++11 is activated, - // but they are missing. The compiler fails due to an implicit deletion. We just deactivate C++11 default functions for now. - #define BOOST_NO_CXX11_DEFAULTED_FUNCTIONS - #include - #undef BOOST_NO_CXX11_DEFAULTED_FUNCTIONS -#endif - -#include -#include -#include -#include - -#include - -#include - -#include "teb_local_planner/equivalence_relations.h" -#include "teb_local_planner/pose_se2.h" -#include "teb_local_planner/teb_config.h" - -namespace teb_local_planner -{ - -class HomotopyClassPlanner; // Forward declaration - -//! Vertex in the graph that is used to find homotopy classes (only stores 2D positions) -struct HcGraphVertex -{ -public: - Eigen::Vector2d pos; // position of vertices in the map - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//! Abbrev. for the homotopy class search-graph type @see HcGraphVertex -typedef boost::adjacency_list < boost::listS, boost::vecS, boost::directedS, HcGraphVertex, boost::no_property > HcGraph; -//! Abbrev. for vertex type descriptors in the homotopy class search-graph -typedef boost::graph_traits::vertex_descriptor HcGraphVertexType; -//! Abbrev. for edge type descriptors in the homotopy class search-graph -typedef boost::graph_traits::edge_descriptor HcGraphEdgeType; -//! Abbrev. for the vertices iterator of the homotopy class search-graph -typedef boost::graph_traits::vertex_iterator HcGraphVertexIterator; -//! Abbrev. for the edges iterator of the homotopy class search-graph -typedef boost::graph_traits::edge_iterator HcGraphEdgeIterator; -//! Abbrev. for the adjacency iterator that iterates vertices that are adjecent to the specified one -typedef boost::graph_traits::adjacency_iterator HcGraphAdjecencyIterator; - -//!< Inline function used for calculateHSignature() in combination with HCP graph vertex descriptors -inline std::complex getCplxFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph) -{ - return std::complex(graph[vert_descriptor].pos.x(), graph[vert_descriptor].pos.y()); -} - -//!< Inline function used for initializing the TEB in combination with HCP graph vertex descriptors -inline const Eigen::Vector2d& getVector2dFromHcGraph(HcGraphVertexType vert_descriptor, const HcGraph& graph) -{ - return graph[vert_descriptor].pos; -} - -/** - * @brief Base class for graph based path planning / homotopy class sampling - */ -class GraphSearchInterface -{ -public: - - virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false) = 0; - - /** - * @brief Clear any existing graph of the homotopy class search - */ - void clearGraph() {graph_.clear();} - - // HcGraph graph() const {return graph_;} - // Workaround. graph_ is public for now, beacuse otherwise the compilation fails with the same boost bug mentioned above. - HcGraph graph_; //!< Store the graph that is utilized to find alternative homotopy classes. - -protected: - /** - * @brief Protected constructor that should be called by subclasses - */ - GraphSearchInterface(const TebConfig& cfg, HomotopyClassPlanner* hcp) : cfg_(&cfg), hcp_(hcp){} - - /** - * @brief Depth First Search implementation to find all paths between the start and the specified goal vertex. - * - * Complete paths are stored to the internal path container. - * @sa http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ - * @param g Graph on which the depth first should be performed - * @param visited A container that stores visited vertices (pass an empty container, it will be filled inside during recursion). - * @param goal Desired goal vertex - * @param start_orientation Orientation of the first trajectory pose, required to initialize the trajectory/TEB - * @param goal_orientation Orientation of the goal trajectory pose, required to initialize the trajectory/TEB - * @param start_velocity start velocity (optional) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) - */ - - void DepthFirst(HcGraph& g, std::vector& visited, const HcGraphVertexType& goal, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false); - -protected: - const TebConfig* cfg_; //!< Config class that stores and manages all related parameters - HomotopyClassPlanner* const hcp_; //!< Raw pointer to the HomotopyClassPlanner. The HomotopyClassPlanner itself is guaranteed to outlive the graph search class it is holding. - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - - -class lrKeyPointGraph : public GraphSearchInterface -{ -public: - lrKeyPointGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){} - - virtual ~lrKeyPointGraph(){} - - /** - * @brief Create a graph containing points in the global frame that can be used to explore new possible paths between start and goal. - * - * This version of the graph creation places a keypoint on the left and right side of each obstacle w.r.t to the goal heading. \n - * All feasible paths between start and goal point are extracted using a Depth First Search afterwards. \n - * This version works very well for small point obstacles. For more complex obstacles call the createProbRoadmapGraph() - * method that samples keypoints in a predefined area and hopefully finds all relevant alternative paths. - * - * @see createProbRoadmapGraph - * @param start Start pose from wich to start on (e.g. the current robot pose). - * @param goal Goal pose to find paths to (e.g. the robot's goal). - * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization). - * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1] - * @param start_velocity start velocity (optional) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) - */ - virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false); -}; - - - - -class ProbRoadmapGraph : public GraphSearchInterface -{ -public: - ProbRoadmapGraph(const TebConfig& cfg, HomotopyClassPlanner* hcp) : GraphSearchInterface(cfg, hcp){} - - virtual ~ProbRoadmapGraph(){} - - - /** - * @brief Create a graph and sample points in the global frame that can be used to explore new possible paths between start and goal. - * - * This version of the graph samples keypoints in a predefined area (config) in the current frame between start and goal. \n - * Afterwards all feasible paths between start and goal point are extracted using a Depth First Search. \n - * Use the sampling method for complex, non-point or huge obstacles. \n - * You may call createGraph() instead. - * - * @see createGraph - * @param start Start pose from wich to start on (e.g. the current robot pose). - * @param goal Goal pose to find paths to (e.g. the robot's goal). - * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization). - * @param no_samples number of random samples - * @param obstacle_heading_threshold Value of the normalized scalar product between obstacle heading and goal heading in order to take them (obstacles) into account [0,1] - * @param start_velocity start velocity (optional) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) - */ - virtual void createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false); - -private: - boost::random::mt19937 rnd_generator_; //!< Random number generator used by createProbRoadmapGraph to sample graph keypoints. -}; -} // end namespace - -#endif // GRAPH_SEARCH_INTERFACE_H diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/h_signature.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/h_signature.h deleted file mode 100644 index 8a11f0f..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/h_signature.h +++ /dev/null @@ -1,432 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Authors: Christoph Rösmann, Franz Albers - *********************************************************************/ - -#ifndef H_SIGNATURE_H_ -#define H_SIGNATURE_H_ - -#include - -#include "teb_local_planner/equivalence_relations.h" -#include "teb_local_planner/misc.h" -#include "teb_local_planner/obstacles.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/timed_elastic_band.h" - -#include -#include -#include -#include -#include -#include - - -namespace teb_local_planner -{ - -/** - * @brief The H-signature defines an equivalence relation based on homology in terms of complex calculus. - * - * The H-Signature depends on the obstacle configuration and can be utilized - * to check whether two trajectores belong to the same homology class. - * Refer to: \n - * - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010 - */ -class HSignature : public EquivalenceClass -{ - -public: - - /** - * @brief Constructor accepting a TebConfig - * @param cfg TebConfig storing some user configuration options - */ - HSignature(const TebConfig& cfg) : cfg_(&cfg) {} - - - /** - * @brief Calculate the H-Signature of a path - * - * The implemented function accepts generic path descriptions that are restricted to the following structure: \n - * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n - * Provide a unary function with the following signature std::complex< long double > (const T& point_type) - * that returns a complex value for the position (Re(*)=x, Im(*)=y). - * - * T could also be a pointer type, if the passed function also accepts a const T* point_Type. - * - * @param path_start Iterator to the first element in the path - * @param path_end Iterator to the last element in the path - * @param obstacles obstacle container - * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number. - * @tparam BidirIter Bidirectional iterator type - * @tparam Fun function of the form std::complex< long double > (const T& point_type) - */ - template - void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles) - { - if (obstacles->empty()) - { - hsignature_ = std::complex(0,0); - return; - } - - - TEB_ASSERT_MSG(cfg_->hcp.h_signature_prescaler>0.1 && cfg_->hcp.h_signature_prescaler<=1, "Only a prescaler on the interval (0.1,1] ist allowed."); - - // guess values for f0 - // paper proposes a+b=N-1 && |a-b|<=1, 1...N obstacles - int m = std::max( (int)obstacles->size()-1, 5 ); // for only a few obstacles we need a min threshold in order to get significantly high H-Signatures - - int a = (int) std::ceil(double(m)/2.0); - int b = m-a; - - std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points - - typedef std::complex cplx; - // guess map size (only a really really coarse guess is required - // use distance from start to goal as distance to each direction - // TODO: one could move the map determination outside this function, since it remains constant for the whole planning interval - cplx start = fun_cplx_point(*path_start); - cplx end = fun_cplx_point(*path_end); // path_end points to the last point now after calling std::advance before - cplx delta = end-start; - cplx normal(-delta.imag(), delta.real()); - cplx map_bottom_left; - cplx map_top_right; - if (std::abs(delta) < 3.0) - { // set minimum bound on distance (we do not want to have numerical instabilities) and 3.0 performs fine... - map_bottom_left = start + cplx(0, -3); - map_top_right = start + cplx(3, 3); - } - else - { - map_bottom_left = start - normal; - map_top_right = start + delta + normal; - } - - hsignature_ = 0; // reset local signature - - std::vector imag_proposals(5); - - // iterate path - while(path_start != path_end) - { - cplx z1 = fun_cplx_point(*path_start); - cplx z2 = fun_cplx_point(*std::next(path_start)); - - for (std::size_t l=0; lsize(); ++l) // iterate all obstacles - { - cplx obst_l = obstacles->at(l)->getCentroidCplx(); - //cplx f0 = (long double) prescaler * std::pow(obst_l-map_bottom_left,a) * std::pow(obst_l-map_top_right,b); - cplx f0 = (long double) cfg_->hcp.h_signature_prescaler * (long double)a*(obst_l-map_bottom_left) * (long double)b*(obst_l-map_top_right); - - // denum contains product with all obstacles exepct j==l - cplx Al = f0; - for (std::size_t j=0; jsize(); ++j) - { - if (j==l) - continue; - cplx obst_j = obstacles->at(j)->getCentroidCplx(); - cplx diff = obst_l - obst_j; - //if (diff.real()!=0 || diff.imag()!=0) - if (std::abs(diff)<0.05) // skip really close obstacles - continue; - else - Al /= diff; - } - // compute log value - double diff2 = std::abs(z2-obst_l); - double diff1 = std::abs(z1-obst_l); - if (diff2 == 0 || diff1 == 0) - continue; - double log_real = std::log(diff2)-std::log(diff1); - // complex ln has more than one solution -> choose minimum abs angle -> paper - double arg_diff = std::arg(z2-obst_l)-std::arg(z1-obst_l); - imag_proposals.at(0) = arg_diff; - imag_proposals.at(1) = arg_diff+2*M_PI; - imag_proposals.at(2) = arg_diff-2*M_PI; - imag_proposals.at(3) = arg_diff+4*M_PI; - imag_proposals.at(4) = arg_diff-4*M_PI; - double log_imag = *std::min_element(imag_proposals.begin(),imag_proposals.end(),smaller_than_abs); - cplx log_value(log_real,log_imag); - //cplx log_value = std::log(z2-obst_l)-std::log(z1-obst_l); // the principal solution doesn't seem to work - hsignature_ += Al*log_value; - } - ++path_start; - } - } - - - /** - * @brief Check if two candidate classes are equivalent - * @param other The other equivalence class to test with - */ - virtual bool isEqual(const EquivalenceClass& other) const - { - const HSignature* hother = dynamic_cast(&other); // TODO: better architecture without dynamic_cast - if (hother) - { - double diff_real = std::abs(hother->hsignature_.real() - hsignature_.real()); - double diff_imag = std::abs(hother->hsignature_.imag() - hsignature_.imag()); - if (diff_real<=cfg_->hcp.h_signature_threshold && diff_imag<=cfg_->hcp.h_signature_threshold) - return true; // Found! Homotopy class already exists, therefore nothing added - } - else - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature equivalence classes with types other than HSignature."); - - return false; - } - - /** - * @brief Check if the equivalence value is detected correctly - * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur. - */ - virtual bool isValid() const - { - return std::isfinite(hsignature_.real()) && std::isfinite(hsignature_.imag()); - } - - /** - * @brief Check if the trajectory is non-looping around an obstacle. - * @return Returns always true, as this cannot be detected by this kind of H-Signature. - */ - virtual bool isReasonable() const - { - return true; - } - - /** - * @brief Get the current value of the h-signature (read-only) - * @return h-signature in complex-number format - */ - const std::complex& value() const {return hsignature_;} - - -private: - - const TebConfig* cfg_; - std::complex hsignature_; -}; - - - - - -/** - * @brief The H-signature in three dimensions (here: x-y-t) defines an equivalence relation based on homology using theorems from electro magnetism. - * - * The H-Signature depends on the obstacle configuration and can be utilized - * to check whether two trajectores belong to the same homology class. - * Refer to: \n - * - S. Bhattacharya et al.: Identification and Representation of Homotopy Classes of Trajectories for Search-based Path Planning in 3D, 2011 - */ -class HSignature3d : public EquivalenceClass -{ -public: - /** - * @brief Constructor accepting a TebConfig - * @param cfg TebConfig storing some user configuration options - */ - HSignature3d(const TebConfig& cfg) : cfg_(&cfg) {} - - - /** - * @brief Calculate the H-Signature of a path - * - * The implemented function accepts generic path descriptions that are restricted to the following structure: \n - * The path is composed of points T and is represented by a std::vector< T > or similar type (std::list, std::deque, ...). \n - * Provide a unary function with the following signature std::complex< long double > (const T& point_type) - * that returns a complex value for the position (Re(*)=x, Im(*)=y). - * - * T could also be a pointer type, if the passed function also accepts a const T* point_Type. - * - * @param path_start Iterator to the first element in the path - * @param path_end Iterator to the last element in the path - * @param obstacles obstacle container - * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number. - * @tparam BidirIter Bidirectional iterator type - * @tparam Fun function of the form std::complex< long double > (const T& point_type) - */ - template - void calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles, - boost::optional timediff_start, boost::optional timediff_end) - { - hsignature3d_.resize(obstacles->size()); - - std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points) - - constexpr int num_int_steps_per_segment = 10; - - for (std::size_t l = 0; l < obstacles->size(); ++l) // iterate all obstacles - { - double H = 0; - double transition_time = 0; - double next_transition_time = 0; - BidirIter path_iter; - TimeDiffSequence::iterator timediff_iter; - - Eigen::Vector3d s1 (obstacles->at(l)->getCentroid()(0), obstacles->at(l)->getCentroid()(1), 0); - double t = 120; // some large value for defining the end point of the obstacle/"conductor" model - Eigen::Vector3d s2; - obstacles->at(l)->predictCentroidConstantVelocity(t, s2.head(2)); - s2[2] = t; - Eigen::Vector3d ds = s2 - s1; - double ds_sq_norm = ds.squaredNorm(); // by definition not zero as t > 0 (3rd component) - - // iterate path - for (path_iter = path_start, timediff_iter = timediff_start.get(); path_iter != path_end; ++path_iter, ++timediff_iter) - { - std::complex z1 = fun_cplx_point(*path_iter); - std::complex z2 = fun_cplx_point(*std::next(path_iter)); - - transition_time = next_transition_time; - if (timediff_start == boost::none || timediff_end == boost::none) // if no time information is provided yet, approximate transition time - next_transition_time += std::abs(z2 - z1) / cfg_->robot.max_vel_x; // Approximate the time, if no time is known - else // otherwise use the time information from the teb trajectory - { - if (std::distance(path_iter, path_end) != std::distance(timediff_iter, timediff_end.get())) { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Size of poses and timediff vectors does not match. This is a bug."); - } - next_transition_time += (*timediff_iter)->dt(); - } - - Eigen::Vector3d direction_vec; - direction_vec[0] = z2.real() - z1.real(); - direction_vec[1] = z2.imag() - z1.imag(); - direction_vec[2] = next_transition_time - transition_time; - - if(direction_vec.norm() < 1e-15) // Coincident poses - continue; - - Eigen::Vector3d r(z1.real(), z1.imag(), transition_time); - Eigen::Vector3d dl = 1.0/static_cast(num_int_steps_per_segment) * direction_vec; // Integrate with multiple steps between each pose - Eigen::Vector3d p1, p2, d, phi; - for (int i = 0; i < num_int_steps_per_segment; ++i, r += dl) - { - p1 = s1 - r; - p2 = s2 - r; - d = (ds.cross(p1.cross(p2))) / ds_sq_norm; - phi = 1.0 / d.squaredNorm() * ((d.cross(p2) / p2.norm()) - (d.cross(p1) / p1.norm())); - H += phi.dot(dl); - } - } - - // normalize to 1 - hsignature3d_.at(l) = H/(4.0*M_PI); - } - } - - /** - * @brief Check if two candidate classes are equivalent - * - * If the absolute value of the H-Signature is equal or greater than 1, a loop (in x-y) around the obstacle is indicated. - * Positive H-Signature: Obstacle lies on the left hand side of the planned trajectory - * Negative H-Signature: Obstacle lies on the right hand side of the planned trajectory - * H-Signature equals zero: Obstacle lies in infinity, has no influence on trajectory - * - * @param other The other equivalence class to test with - */ - virtual bool isEqual(const EquivalenceClass& other) const - { - const HSignature3d* hother = dynamic_cast(&other); // TODO: better architecture without dynamic_cast - if (hother) - { - if (hsignature3d_.size() == hother->hsignature3d_.size()) - { - for(size_t i = 0; i < hsignature3d_.size(); ++i) - { - // If the H-Signature for one obstacle is below this threshold, that obstacle is far away from the planned trajectory, - // and therefore ignored in the homotopy class planning - if (std::abs(hother->hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold || std::abs(hsignature3d_.at(i)) < cfg_->hcp.h_signature_threshold) - continue; - - if (boost::math::sign(hother->hsignature3d_.at(i)) != boost::math::sign(hsignature3d_.at(i))) - return false; // Signatures are not equal, new homotopy class - } - return true; // Found! Homotopy class already exists, therefore nothing added - } - } - else { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Cannot compare HSignature3d equivalence classes with types other than HSignature3d."); - } - - return false; - } - - /** - * @brief Check if the equivalence value is detected correctly - * @return Returns false, if the equivalence class detection failed, e.g. if nan- or inf values occur. - */ - virtual bool isValid() const - { - for(const double& value : hsignature3d_) - { - if (!std::isfinite(value)) - return false; - } - return true; - } - - /** - * @brief Check if the trajectory is non-looping around any obstacle. Values greater than 1 indicate a looping trajectory. - * @return Returns false, if the trajectory loops around an obstacle - */ - virtual bool isReasonable() const - { - for(const double& value : hsignature3d_) - { - if (value > 1.0) - return false; - } - return true; - } - - /** - * @brief Get the current h-signature (read-only) - * @return h-signature in complex-number format - */ - const std::vector& values() const {return hsignature3d_;} - -private: - const TebConfig* cfg_; - std::vector hsignature3d_; -}; - - -} // namespace teb_local_planner - - -#endif /* H_SIGNATURE_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h deleted file mode 100644 index 841e630..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.h +++ /dev/null @@ -1,591 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef HOMOTOPY_CLASS_PLANNER_H_ -#define HOMOTOPY_CLASS_PLANNER_H_ - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include "teb_local_planner/planner_interface.h" -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/obstacles.h" -#include "teb_local_planner/optimal_planner.h" -#include "teb_local_planner/visualization.h" -#include "teb_local_planner/robot_footprint_model.h" -#include "teb_local_planner/equivalence_relations.h" -#include "teb_local_planner/graph_search.h" - - -namespace teb_local_planner -{ - -//!< Inline function used for calculateHSignature() in combination with VertexPose pointers -inline std::complex getCplxFromVertexPosePtr(const VertexPose* pose) -{ - return std::complex(pose->x(), pose->y()); -}; - - -//!< Inline function used for calculateHSignature() in combination with geometry_msgs::msg::PoseStamped -inline std::complex getCplxFromMsgPoseStamped(const geometry_msgs::msg::PoseStamped& pose) -{ - return std::complex(pose.pose.position.x, pose.pose.position.y); -}; - -/** - * @class HomotopyClassPlanner - * @brief Local planner that explores alternative homotopy classes, create a plan for each alternative - * and finally return the robot controls for the current best path (repeated in each sampling interval) - * - * Equivalence classes (e.g. homotopy) are explored using the help of a search-graph. \n - * A couple of possible candidates are sampled / generated and filtered afterwards such that only a single candidate - * per homotopy class remain. Filtering is applied using the H-Signature, a homotopy (resp. homology) invariant: \n - * - S. Bhattacharya et al.: Search-based Path Planning with Homotopy Class Constraints, AAAI, 2010 - * - C. Rösmann et al.: Planning of Multiple Robot Trajectories in Distinctive Topologies, ECMR, 2015. - * - * Followed by the homotopy class search, each candidate is used as an initialization for the underlying trajectory - * optimization (in this case utilizing the TebOptimalPlanner class with the TimedElasticBand). \n - * Depending on the config parameters, the optimization is performed in parallel. \n - * After the optimization is completed, the best optimized candidate is selected w.r.t. to trajectory cost, since the - * cost already contains important features like clearance from obstacles and transition time. \n - * - * Everyhting is performed by calling one of the overloaded plan() methods. \n - * Afterwards the velocity command to control the robot is obtained from the "best" candidate - * via getVelocityCommand(). \n - * - * All steps are repeated in the subsequent sampling interval with the exception, that already planned (optimized) trajectories - * are preferred against new path initilizations in order to improve the hot-starting capability. - */ -class HomotopyClassPlanner : public PlannerInterface -{ -public: - - using EquivalenceClassContainer = std::vector< std::pair >; - - /** - * @brief Default constructor - */ - HomotopyClassPlanner(); - - /** - * @brief Construct and initialize the HomotopyClassPlanner - * @param node Shared pointer for rclcpp::Node - * @param cfg Const reference to the TebConfig class for internal parameters - * @param obstacles Container storing all relevant obstacles (see Obstacle) - * @param robot_model Shared pointer to the robot shape model used for optimization (optional) - * @param visualization Shared pointer to the TebVisualization class (optional) - * @param via_points Container storing via-points (optional) - */ - HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, - TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); - - /** - * @brief Destruct the HomotopyClassPlanner. - */ - virtual ~HomotopyClassPlanner(); - - /** - * @brief Initialize the HomotopyClassPlanner - * @param node Shared pointer for rclcpp::Node - * @param cfg Const reference to the TebConfig class for internal parameters - * @param obstacles Container storing all relevant obstacles (see Obstacle) - * @param visualization Shared pointer to the TebVisualization class (optional) - * @param via_points Container storing via-points (optional) - */ - void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, - TebVisualizationPtr visualization = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); - - /** @name Plan a trajectory */ - //@{ - - /** - * @brief Plan a trajectory based on an initial reference plan. - * - * Provide this method to create and optimize a trajectory that is initialized - * according to an initial reference plan (given as a container of poses). - * @warning The current implementation extracts only the start and goal pose and calls the overloaded plan() - * @param initial_plan vector of geometry_msgs::msg::PoseStamped (must be valid until clearPlanner() is called!) - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const std::vector& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false); - - /** - * @brief Plan a trajectory between a given start and goal pose (tf::Pose version). - * - * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose. - * @param start tf::Pose containing the start pose of the trajectory - * @param goal tf::Pose containing the goal pose of the trajectory - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - // tf2 doesn't have tf::Pose -// virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false); - - /** - * @brief Plan a trajectory between a given start and goal pose. - * - * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose. - * @param start PoseSE2 containing the start pose of the trajectory - * @param goal PoseSE2 containing the goal pose of the trajectory - * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity). - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false); - - /** - * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. - * @warning Call plan() first and check if the generated plan is feasible. - * @param[out] vx translational velocity [m/s] - * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s] - * @param[out] omega rotational velocity [rad/s] - * @param[in] look_ahead_poses index of the final pose used to compute the velocity command. - * @return \c true if command is valid, \c false otherwise - */ - virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const; - - /** - * @brief Access current best trajectory candidate (that relates to the "best" homotopy class). - * - * If no trajectory is available, the pointer will be empty. - * If only a single trajectory is available, return it. - * Otherwise return the best one, but call selectBestTeb() before to perform the actual selection (part of the plan() methods). - * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand). - */ - TebOptimalPlannerPtr bestTeb() const {return tebs_.empty() ? TebOptimalPlannerPtr() : tebs_.size()==1 ? tebs_.front() : best_teb_;} - - /** - * @brief Check whether the planned trajectory is feasible or not. - * - * This method currently checks only that the trajectory, or a part of the trajectory is collision free. - * Obstacles are here represented as costmap instead of the internal ObstacleContainer. - * @param costmap_model Pointer to the costmap model - * @param footprint_spec The specification of the footprint of the robot in world coordinates - * @param inscribed_radius The radius of the inscribed circle of the robot - * @param circumscribed_radius The radius of the circumscribed circle of the robot - * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. - * @return \c true, if the robot footprint along the first part of the trajectory intersects with - * any obstacle in the costmap, \c false otherwise. - */ - virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector& footprint_spec, - double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0); - - /** - * @brief In case of empty best teb, scores again the available plans to find the best one. - * The best_teb_ variable is updated consequently. - * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand). - * An empty pointer is returned if no plan is available. - */ - TebOptimalPlannerPtr findBestTeb(); - - /** - * @brief Removes the specified teb and the corresponding homotopy class from the list of available ones. - * @param pointer to the teb Band to be removed - * @return Iterator to the next valid teb if available, else to the end of the tebs container. - */ - TebOptPlannerContainer::iterator removeTeb(TebOptimalPlannerPtr& teb); - - //@} - - /** @name Visualization */ - //@{ - - /** - * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence) - * @param visualization shared pointer to a TebVisualization instance - * @see visualizeTeb - */ - void setVisualization(const TebVisualizationPtr & visualization) override; - - /** - * @brief Publish the local plan, pose sequence and additional information via ros topics (e.g. subscribe with rviz). - * - * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor. - * @see setVisualization - */ - virtual void visualize(); - - //@} - - /** @name Important steps that are called during planning */ - //@{ - - - /** - * @brief Explore paths in new equivalence classes (e.g. homotopy classes) and initialize TEBs from them. - * - * This "all-in-one" method creates a graph with position keypoints from which - * feasible paths (with clearance from obstacles) are extracted. \n - * All obtained paths are filted to only keep a single path for each equivalence class. \n - * Each time a new equivalence class is explored (by means of \b no previous trajectory/TEB remain in that equivalence class), - * a new trajectory/TEB will be initialized. \n - * - * Everything is prepared now for the optimization step: see optimizeAllTEBs(). - * @param start Current start pose (e.g. pose of the robot) - * @param goal Goal pose (e.g. robot's goal) - * @param dist_to_obst Allowed distance to obstacles: if not satisfying, the path will be rejected (note, this is not the distance used for optimization). - * @param @param start_velocity start velocity (optional) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) - */ - void exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel = false); - - /** - * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it using a generic 2D reference path - * - * Refer to TimedElasticBand::initTEBtoGoal() for more details about the template parameters. - * @param path_start start iterator of a generic 2d path - * @param path_end end iterator of a generic 2d path - * @param fun_position unary function that returns the Eigen::Vector2d object - * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading) - * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading) - * @param start_velocity start velocity (optional) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) - * @tparam BidirIter Bidirectional iterator type - * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d - * @return Shared pointer to the newly created teb optimal planner - */ - template - TebOptimalPlannerPtr addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false); - - /** - * @brief Add a new Teb to the internal trajectory container, if this teb constitutes a new equivalence class. Initialize it with a simple straight line between a given start and goal - * @param start start pose - * @param goal goal pose - * @param start_velocity start velocity (optional) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) - * @return Shared pointer to the newly created teb optimal planner - */ - TebOptimalPlannerPtr addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false); - - /** - * @brief Add a new Teb to the internal trajectory container , if this teb constitutes a new equivalence class. Initialize it using a PoseStamped container - * @param initial_plan container of poses (start and goal orientation should be valid!) - * @param start_velocity start velocity (optional) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, otherwise the final velocity will be zero (default: false) - * @return Shared pointer to the newly created teb optimal planner - */ - TebOptimalPlannerPtr addAndInitNewTeb(const std::vector& initial_plan, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel = false); - - /** - * @brief Update TEBs with new pose, goal and current velocity. - * @param start New start pose (optional) - * @param goal New goal pose (optional) - * @param start_velocity start velocity (optional) - */ - void updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::msg::Twist* start_velocity); - - - /** - * @brief Optimize all available trajectories by invoking the optimizer on each one. - * - * Depending on the configuration parameters, the optimization is performed either single or multi threaded. - * @param iter_innerloop Number of inner iterations (see TebOptimalPlanner::optimizeTEB()) - * @param iter_outerloop Number of outer iterations (see TebOptimalPlanner::optimizeTEB()) - */ - void optimizeAllTEBs(int iter_innerloop, int iter_outerloop); - - /** - * @brief Returns a shared pointer to the TEB related to the initial plan - * @return A non-empty shared ptr is returned if a match was found; Otherwise the shared ptr is empty. - */ - TebOptimalPlannerPtr getInitialPlanTEB(); - - /** - * @brief In case of multiple, internally stored, alternative trajectories, select the best one according to their cost values. - * - * The trajectory cost includes features such as transition time and clearance from obstacles. \n - * The best trajectory can be accessed later by bestTeb() within the current sampling interval in order to avoid unessary recalculations. - * @return Shared pointer to the best TebOptimalPlanner that contains the selected trajectory (TimedElasticBand). - */ - TebOptimalPlannerPtr selectBestTeb(); - - //@} - - /** - * @brief Reset the planner. - * - * Clear all previously found H-signatures, paths, tebs and the hcgraph. - */ - virtual void clearPlanner() {clearGraph(); equivalence_classes_.clear(); tebs_.clear(); initial_plan_ = NULL;} - - - /** - * @brief Prefer a desired initial turning direction (by penalizing the opposing one) - * - * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two - * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. - * Initial means that the penalty is applied only to the first few poses of the trajectory. - * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none) - */ - virtual void setPreferredTurningDir(RotType dir); - - /** - * @brief Calculate the equivalence class of a path - * - * Currently, only the H-signature (refer to HSignature) is implemented. - * - * @param path_start Iterator to the first element in the path - * @param path_end Iterator to the last element in the path - * @param obstacles obstacle container - * @param fun_cplx_point function accepting the dereference iterator type and that returns the position as complex number. - * @tparam BidirIter Bidirectional iterator type - * @tparam Fun function of the form std::complex< long double > (const T& point_type) - * @return pointer to the equivalence class base type - */ - template - EquivalenceClassPtr calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles = NULL, - boost::optional timediff_start = boost::none, boost::optional timediff_end = boost::none); - - /** - * @brief Read-only access to the internal trajectory container. - * @return read-only reference to the teb container. - */ - const TebOptPlannerContainer& getTrajectoryContainer() const {return tebs_;} - - bool hasDiverged() const override; - - /** - * Compute and return the cost of the current optimization graph (supports multiple trajectories) - * @param[out] cost current cost value for each trajectory - * [for a planner with just a single trajectory: size=1, vector will not be cleared] - * @param obst_cost_scale Specify extra scaling for obstacle costs - * @param viapoint_cost_scale Specify extra scaling for via points. - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time - */ - virtual void computeCurrentCost(std::vector& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); - - /** - * @brief Check if two h-signatures are similar (w.r.t. a certain threshold) - * @param h1 first h-signature - * @param h2 second h-signature - * @return \c true if both h-signatures are similar, false otherwise. - */ - inline static bool isHSignatureSimilar(const std::complex& h1, const std::complex& h2, double threshold) - { - double diff_real = std::abs(h2.real() - h1.real()); - double diff_imag = std::abs(h2.imag() - h1.imag()); - if (diff_real<=threshold && diff_imag<=threshold) - return true; // Found! Homotopy class already exists, therefore nothing added - return false; - } - /** - * @brief Checks if the orientation of the computed trajectories differs from that of the best plan of more than the - * specified threshold and eventually deletes them. - * Also deletes detours with a duration much bigger than the duration of the best_teb (duration / best duration > max_ratio_detours_duration_best_duration). - * @param orient_threshold: Threshold paramter for allowed orientation changes in radians - * @param len_orientation_vector: length of the vector used to compute the start orientation - */ - void deletePlansDetouringBackwards(const double orient_threshold, const double len_orientation_vector); - /** - * @brief Given a plan, computes its start orientation using a vector of length >= len_orientation_vector - * starting from the initial pose. - * @param plan: Teb to be analyzed - * @param len_orientation_vector: min length of the vector used to compute the start orientation - * @param orientation: computed start orientation - * @return: Could the vector for the orientation check be computed? (False if the plan has no pose with a distance - * > len_orientation_vector from the start poseq) - */ - bool computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, double& orientation); - - - /** - * @brief Access config (read-only) - * @return const pointer to the config instance - */ - const TebConfig* config() const {return cfg_;} - - /** - * @brief Access current obstacle container (read-only) - * @return const pointer to the obstacle container instance - */ - const ObstContainer* obstacles() const {return obstacles_;} - - /** - * @brief Returns true if the planner is initialized - */ - bool isInitialized() const {return initialized_;} - - /** - * @brief Clear any existing graph of the homotopy class search - */ - void clearGraph() {if(graph_search_) graph_search_->clearGraph();} - - /** - * @brief find the index of the currently best TEB in the container - * @remarks bestTeb() should be preferred whenever possible - * @return index of the best TEB obtained with bestTEB(), if no TEB is avaiable, it returns -1. - */ - int bestTebIdx() const; - - - /** - * @brief Internal helper function that adds a new equivalence class to the list of known classes only if it is unique. - * @param eq_class equivalence class that should be tested - * @param lock if \c true, exclude the H-signature from deletion. - * @return \c true if the h-signature was added and no duplicate was found, \c false otherwise - */ - bool addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock=false); - - /** - * @brief Return the current set of equivalence erelations (read-only) - * @return reference to the internal set of currently tracked equivalence relations - */ - const EquivalenceClassContainer& getEquivalenceClassRef() const {return equivalence_classes_;} - - bool isInBestTebClass(const EquivalenceClassPtr& eq_class) const; - - int numTebsInClass(const EquivalenceClassPtr& eq_class) const; - - int numTebsInBestTebClass() const; - - /** - * @brief Randomly drop non-optimal TEBs to so we can explore other alternatives - * - * The HCP has a tendency to become "fixated" once its tebs_ list becomes - * fully populated, repeatedly refining and evaluating paths from the same - * few homotopy classes until the robot moves far enough for a teb to become - * invalid. As a result, it can fail to discover a more optimal path. This - * function alleviates this problem by randomly dropping TEBs other than the - * current "best" one with a probability controlled by - * selection_dropping_probability parameter. - */ - void randomlyDropTebs(); - -protected: - - /** @name Explore new paths and keep only a single one for each homotopy class */ - //@{ - - /** - * @brief Check if a h-signature exists already. - * @param eq_class equivalence class that should be tested - * @return \c true if the h-signature is found, \c false otherwise - */ - bool hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const; - - - /** - * @brief Renew all found h-signatures for the new planning step based on existing TEBs. Optionally detours can be discarded. - * - * Calling this method in each new planning interval is really important. - * First all old h-signatures are deleted, since they could be invalid for this planning step (obstacle position may changed). - * Afterwards the h-signatures are calculated for each existing TEB/trajectory and is inserted to the list of known h-signatures. - * Doing this is important to prefer already optimized trajectories in contrast to initialize newly explored coarse paths. - * @param delete_detours if this param is \c true, all existing TEBs are cleared from detour-candidates calling deletePlansGoingBackwards(). - */ - void renewAndAnalyzeOldTebs(bool delete_detours); - - /** - * @brief Associate trajectories with via-points - * - * If \c all_trajectories is true, all trajectory candidates are connected with the set of via_points, - * otherwise only the trajectory sharing the homotopy class of the initial/global plan (and all via-points from alternative trajectories are removed) - * @remarks Requires that the plan method is called with an initial plan provided and that via-points are enabled (config) - * @param all_trajectories see method description - */ - void updateReferenceTrajectoryViaPoints(bool all_trajectories); - - //@} - - // external objects (store weak pointers) - const TebConfig* cfg_; //!< Config class that stores and manages all related parameters - ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning - const ViaPointContainer* via_points_; //!< Store the current list of via-points - - // internal objects (memory management owned) - TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...) - TebOptimalPlannerPtr best_teb_; //!< Store the current best teb. - EquivalenceClassPtr best_teb_eq_class_; //!< Store the equivalence class of the current best teb - RobotFootprintModelPtr robot_model_; //!< Robot model shared instance - - const std::vector* initial_plan_; //!< Store the initial plan if available for a better trajectory initialization - EquivalenceClassPtr initial_plan_eq_class_; //!< Store the equivalence class of the initial plan - TebOptimalPlannerPtr initial_plan_teb_; //!< Store pointer to the TEB related to the initial plan (use method getInitialPlanTEB() since it checks if initial_plan_teb_ is still included in tebs_.) - - TebOptPlannerContainer tebs_; //!< Container that stores multiple local teb planners (for alternative equivalence classes) and their corresponding costs - - EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones. - // The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping). - - std::shared_ptr graph_search_; - - rclcpp::Time last_eq_class_switching_time_; //!< Store the time at which the equivalence class changed recently - - std::default_random_engine random_; - bool initialized_; //!< Keeps track about the correct initialization of this class - - TebOptimalPlannerPtr last_best_teb_; //!< Points to the plan used in the previous control cycle - - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - -}; - -//! Abbrev. for a shared pointer of a HomotopyClassPlanner instance. -typedef std::shared_ptr HomotopyClassPlannerPtr; - - -} // namespace teb_local_planner - -// include template implementations / definitions -#include "teb_local_planner/homotopy_class_planner.hpp" - -#endif /* HOMOTOPY_CLASS_PLANNER_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp deleted file mode 100644 index db7cdef..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/homotopy_class_planner.hpp +++ /dev/null @@ -1,95 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/homotopy_class_planner.h" -#include "teb_local_planner/h_signature.h" - -namespace teb_local_planner -{ - - -template -EquivalenceClassPtr HomotopyClassPlanner::calculateEquivalenceClass(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles, - boost::optional timediff_start, boost::optional timediff_end) -{ - if(cfg_->obstacles.include_dynamic_obstacles) - { - HSignature3d* H = new HSignature3d(*cfg_); - H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles, timediff_start, timediff_end); - return EquivalenceClassPtr(H); - } - else - { - HSignature* H = new HSignature(*cfg_); - H->calculateHSignature(path_start, path_end, fun_cplx_point, obstacles); - return EquivalenceClassPtr(H); - } -} - - -template -TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position, double start_orientation, double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel) -{ - TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_)); - - candidate->teb().initTrajectoryToGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, - cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples, - cfg_->trajectory.allow_init_with_backwards_motion); - - if (start_velocity) - candidate->setVelocityStart(*start_velocity); - - EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_, - candidate->teb().timediffs().begin(), candidate->teb().timediffs().end()); - - - if (free_goal_vel) - candidate->setVelocityGoalFree(); - - if(addEquivalenceClassIfNew(H)) - { - tebs_.push_back(candidate); - return tebs_.back(); - } - - // If the candidate constitutes no new equivalence class, return a null pointer - return TebOptimalPlannerPtr(); -} - -} // namespace teb_local_planner - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/misc.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/misc.h deleted file mode 100644 index 67f5d90..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/misc.h +++ /dev/null @@ -1,210 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef MISC_H -#define MISC_H - -#include - -#include - -#include -#include - -#include - -namespace teb_local_planner -{ -#define SMALL_NUM 0.00000001 - -//! Symbols for left/none/right rotations -enum class RotType { left, none, right }; - -/** - * @brief Check whether two variables (double) are close to each other - * @param a the first value to compare - * @param b the second value to compare - * @param epsilon precision threshold - * @return \c true if |a-b| < epsilon, false otherwise - */ -inline bool is_close(double a, double b, double epsilon = 1e-4) -{ - return std::fabs(a - b) < epsilon; -} - -/** - * @brief Return the average angle of an arbitrary number of given angles [rad] - * @param angles vector containing all angles - * @return average / mean angle, that is normalized to [-pi, pi] - */ -inline double average_angles(const std::vector& angles) -{ - double x=0, y=0; - for (std::vector::const_iterator it = angles.begin(); it!=angles.end(); ++it) - { - x += cos(*it); - y += sin(*it); - } - if(x == 0 && y == 0) - return 0; - else - return std::atan2(y, x); -} - -/** @brief Small helper function: check if |a|<|b| */ -inline bool smaller_than_abs(double i, double j) {return std::fabs(i) -inline double distance_points2d(const P1& point1, const P2& point2) -{ - return std::sqrt( std::pow(point2.x-point1.x,2) + std::pow(point2.y-point1.y,2) ); -} - - -/** - * @brief Calculate the 2d cross product (returns length of the resulting vector along the z-axis in 3d) - * @param v1 object containing public methods x() and y() - * @param v2 object containing fields x() and y() - * @return magnitude that would result in the 3D case (along the z-axis) -*/ -template -inline double cross2d(const V1& v1, const V2& v2) -{ - return v1.x()*v2.y() - v2.x()*v1.y(); -} - -/** - * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. - * - * Return a constant reference for boths input variants (pointer or reference). - * @remarks Makes only sense in combination with the overload getConstReference(const T& val). - * @param ptr pointer of type T - * @tparam T arbitrary type - * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion - */ -template -inline const T& get_const_reference(const T* ptr) {return *ptr;} - -/** - * @brief Helper function that returns the const reference to a value defined by either its raw pointer type or const reference. - * - * Return a constant reference for boths input variants (pointer or reference). - * @remarks Makes only sense in combination with the overload getConstReference(const T* val). - * @param val - * @param dummy SFINAE helper variable - * @tparam T arbitrary type - * @return If \c T is a pointer, return const *T (leading to const T&), otherwise const T& with out pointer-to-ref conversion - */ -template -inline const T& get_const_reference(const T& val, typename std::enable_if_t::value, T>* dummy = nullptr) {return val;} - -inline builtin_interfaces::msg::Duration durationFromSec(double t_sec) -{ - int32_t sec; - uint32_t nsec; - sec = static_cast(floor(t_sec)); - nsec = static_cast(std::round((t_sec - sec) * 1e9)); - // avoid rounding errors - sec += (nsec / 1000000000l); - nsec %= 1000000000l; - - builtin_interfaces::msg::Duration duration; - duration.sec = sec; - duration.nanosec = nsec; - return duration; -} - -struct TebAssertionFailureException : public std::runtime_error -{ - TebAssertionFailureException(const std::string &msg) - : std::runtime_error(msg) - { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), msg.c_str()); - } -}; - -#define TEB_ASSERT_MSG_IMPL(...) \ - { \ - char arg_string[1024]; \ - std::sprintf(arg_string, __VA_ARGS__); \ - const std::string msg(arg_string); \ - throw TebAssertionFailureException(msg); \ - } - -template::value>* = nullptr> -void teb_assert_msg_impl(const T expression, ARGS ...args) { - if(expression == 0) { - char arg_string[1024]; - std::sprintf(arg_string, args..., ""); - const std::string msg(arg_string); - throw TebAssertionFailureException(msg); - } -} - -template::value>* = nullptr> -void teb_assert_msg_impl(const T expression, ARGS ...args) { - if(expression == nullptr) { - char arg_string[1024]; - std::sprintf(arg_string, args..., ""); - const std::string msg(arg_string); - throw TebAssertionFailureException(msg); - } -} - -#define TEB_ASSERT_MSG(expression, ...) teb_assert_msg_impl(expression, __VA_ARGS__) - -} // namespace teb_local_planner - -#endif /* MISC_H */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/obstacles.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/obstacles.h deleted file mode 100644 index 6893589..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/obstacles.h +++ /dev/null @@ -1,1112 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - - -#ifndef OBSTACLES_H -#define OBSTACLES_H - -#include -#include -#include - -#include - -#include -#include -#include - -#include "teb_local_planner/distance_calculations.h" - - -namespace teb_local_planner -{ - -/** - * @class Obstacle - * @brief Abstract class that defines the interface for modelling obstacles - */ -class Obstacle -{ -public: - - /** - * @brief Default constructor of the abstract obstacle class - */ - Obstacle() : dynamic_(false), centroid_velocity_(Eigen::Vector2d::Zero()) - { - } - - /** - * @brief Virtual destructor. - */ - virtual ~Obstacle() - { - } - - - /** @name Centroid coordinates (abstract, obstacle type depending) */ - //@{ - - /** - * @brief Get centroid coordinates of the obstacle - * @return Eigen::Vector2d containing the centroid - */ - virtual const Eigen::Vector2d& getCentroid() const = 0; - - /** - * @brief Get centroid coordinates of the obstacle as complex number - * @return std::complex containing the centroid coordinate - */ - virtual std::complex getCentroidCplx() const = 0; - - //@} - - - /** @name Collision checking and distance calculations (abstract, obstacle type depending) */ - //@{ - - /** - * @brief Check if a given point collides with the obstacle - * @param position 2D reference position that should be checked - * @param min_dist Minimum distance allowed to the obstacle to be collision free - * @return \c true if position is inside the region of the obstacle or if the minimum distance is lower than min_dist - */ - virtual bool checkCollision(const Eigen::Vector2d& position, double min_dist) const = 0; - - /** - * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist) - * @param line_start 2D point for the end of the reference line - * @param line_end 2D point for the end of the reference line - * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free - * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist - */ - virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const = 0; - - /** - * @brief Get the minimum euclidean distance to the obstacle (point as reference) - * @param position 2d reference position - * @return The nearest possible distance to the obstacle - */ - virtual double getMinimumDistance(const Eigen::Vector2d& position) const = 0; - - /** - * @brief Get the minimum euclidean distance to the obstacle (line as reference) - * @param line_start 2d position of the begin of the reference line - * @param line_end 2d position of the end of the reference line - * @return The nearest possible distance to the obstacle - */ - virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const = 0; - - /** - * @brief Get the minimum euclidean distance to the obstacle (polygon as reference) - * @param polygon Vertices (2D points) describing a closed polygon - * @return The nearest possible distance to the obstacle - */ - virtual double getMinimumDistance(const Point2dContainer& polygon) const = 0; - - /** - * @brief Get the closest point on the boundary of the obstacle w.r.t. a specified reference position - * @param position reference 2d position - * @return closest point on the obstacle boundary - */ - virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const = 0; - - //@} - - - - /** @name Velocity related methods for non-static, moving obstacles */ - //@{ - - /** - * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (point as reference) - * @param position 2d reference position - * @param t time, for which the minimum distance to the obstacle is estimated - * @return The nearest possible distance to the obstacle at time t - */ - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const = 0; - - /** - * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (line as reference) - * @param line_start 2d position of the begin of the reference line - * @param line_end 2d position of the end of the reference line - * @param t time, for which the minimum distance to the obstacle is estimated - * @return The nearest possible distance to the obstacle at time t - */ - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const = 0; - - /** - * @brief Get the estimated minimum spatiotemporal distance to the moving obstacle using a constant velocity model (polygon as reference) - * @param polygon Vertices (2D points) describing a closed polygon - * @param t time, for which the minimum distance to the obstacle is estimated - * @return The nearest possible distance to the obstacle at time t - */ - virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const = 0; - - /** - * @brief Predict position of the centroid assuming a constant velocity model - * @param[in] t time in seconds for the prediction (t>=0) - * @param[out] position predicted 2d position of the centroid - */ - virtual void predictCentroidConstantVelocity(double t, Eigen::Ref position) const - { - position = getCentroid() + t * getCentroidVelocity(); - } - - /** - * @brief Check if the obstacle is a moving with a (non-zero) velocity - * @return \c true if the obstacle is not marked as static, \c false otherwise - */ - bool isDynamic() const {return dynamic_;} - - /** - * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid - * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic) - * @param vel 2D vector containing the velocities of the centroid in x and y directions - */ - void setCentroidVelocity(const Eigen::Ref& vel) {centroid_velocity_ = vel; dynamic_=true;} - - /** - * @brief Set the 2d velocity (vx, vy) of the obstacle w.r.t to the centroid - * @remarks Setting the velocity using this function marks the obstacle as dynamic (@see isDynamic) - * @param velocity geometry_msgs::msg::TwistWithCovariance containing the velocity of the obstacle - * @param orientation geometry_msgs::msg::QuaternionStamped containing the orientation of the obstacle - */ - void setCentroidVelocity(const geometry_msgs::msg::TwistWithCovariance& velocity, - const geometry_msgs::msg::Quaternion& orientation) - { - // Set velocity, if obstacle is moving - Eigen::Vector2d vel; - vel.coeffRef(0) = velocity.twist.linear.x; - vel.coeffRef(1) = velocity.twist.linear.y; - - // If norm of velocity is less than 0.001, consider obstacle as not dynamic - // TODO: Get rid of constant - if (vel.norm() < 0.001) - return; - - // currently velocity published by stage is already given in the map frame -// double yaw = tf::getYaw(orientation.quaternion); -// ROS_INFO("Yaw: %f", yaw); -// Eigen::Rotation2Dd rot(yaw); -// vel = rot * vel; - setCentroidVelocity(vel); - } - - void setCentroidVelocity(const geometry_msgs::msg::TwistWithCovariance& velocity, - const geometry_msgs::msg::QuaternionStamped& orientation) - { - setCentroidVelocity(velocity, orientation.quaternion); - } - - /** - * @brief Get the obstacle velocity (vx, vy) (w.r.t. to the centroid) - * @returns 2D vector containing the velocities of the centroid in x and y directions - */ - const Eigen::Vector2d& getCentroidVelocity() const {return centroid_velocity_;} - - //@} - - - - /** @name Helper Functions */ - //@{ - - /** - * @brief Convert the obstacle to a polygon message - * - * Convert the obstacle to a corresponding polygon msg. - * Point obstacles have one vertex, lines have two vertices - * and polygons might are implictly closed such that the start vertex must not be repeated. - * @param[out] polygon the polygon message - */ - virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon) = 0; - - virtual void toTwistWithCovarianceMsg(geometry_msgs::msg::TwistWithCovariance& twistWithCovariance) - { - if (dynamic_) - { - twistWithCovariance.twist.linear.x = centroid_velocity_(0); - twistWithCovariance.twist.linear.y = centroid_velocity_(1); - } - else - { - twistWithCovariance.twist.linear.x = 0; - twistWithCovariance.twist.linear.y = 0; - } - - // TODO:Covariance - } - - //@} - -protected: - - bool dynamic_; //!< Store flag if obstacle is dynamic (resp. a moving obstacle) - Eigen::Vector2d centroid_velocity_; //!< Store the corresponding velocity (vx, vy) of the centroid (zero, if _dynamic is \c true) - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -//! Abbrev. for shared obstacle pointers -typedef std::shared_ptr ObstaclePtr; -//! Abbrev. for shared obstacle const pointers -typedef std::shared_ptr ObstacleConstPtr; -//! Abbrev. for containers storing multiple obstacles -typedef std::vector ObstContainer; - - - -/** - * @class PointObstacle - * @brief Implements a 2D point obstacle - */ -class PointObstacle : public Obstacle -{ -public: - - /** - * @brief Default constructor of the point obstacle class - */ - PointObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero()) - {} - - /** - * @brief Construct PointObstacle using a 2d position vector - * @param position 2d position that defines the current obstacle position - */ - PointObstacle(const Eigen::Ref< const Eigen::Vector2d>& position) : Obstacle(), pos_(position) - {} - - /** - * @brief Construct PointObstacle using x- and y-coordinates - * @param x x-coordinate - * @param y y-coordinate - */ - PointObstacle(double x, double y) : Obstacle(), pos_(Eigen::Vector2d(x,y)) - {} - - - // implements checkCollision() of the base class - virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const - { - return getMinimumDistance(point) < min_dist; - } - - - // implements checkLineIntersection() of the base class - virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const - { - // Distance Line - Circle - // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke - Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x - Eigen::Vector2d b = pos_-line_start; // b=m-x - - // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1 - double t = a.dot(b)/a.dot(a); - if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line - else if (t>1) t=1; - Eigen::Vector2d nearest_point = line_start + a*t; - - // check collision - return checkCollision(nearest_point, min_dist); - } - - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& position) const - { - return (position-pos_).norm(); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const - { - return distance_point_to_segment_2d(pos_, line_start, line_end); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Point2dContainer& polygon) const - { - return distance_point_to_polygon_2d(pos_, polygon); - } - - // implements getMinimumDistanceVec() of the base class - virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const - { - return pos_; - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const - { - return (pos_ + t*centroid_velocity_ - position).norm(); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const - { - return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const - { - return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon); - } - - // implements predictCentroidConstantVelocity() of the base class - virtual void predictCentroidConstantVelocity(double t, Eigen::Ref position) const - { - position = pos_ + t*centroid_velocity_; - } - - // implements getCentroid() of the base class - virtual const Eigen::Vector2d& getCentroid() const - { - return pos_; - } - - // implements getCentroidCplx() of the base class - virtual std::complex getCentroidCplx() const - { - return std::complex(pos_[0],pos_[1]); - } - - // Accessor methods - const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only) - Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle - double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle - const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only) - double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle - const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only) - - // implements toPolygonMsg() of the base class - virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon) - { - polygon.points.resize(1); - polygon.points.front().x = pos_.x(); - polygon.points.front().y = pos_.y(); - polygon.points.front().z = 0; - } - -protected: - - Eigen::Vector2d pos_; //!< Store the position of the PointObstacle - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * @class CircularObstacle - * @brief Implements a 2D circular obstacle (point obstacle plus radius) - */ -class CircularObstacle : public Obstacle -{ -public: - - /** - * @brief Default constructor of the circular obstacle class - */ - CircularObstacle() : Obstacle(), pos_(Eigen::Vector2d::Zero()) - {} - - /** - * @brief Construct CircularObstacle using a 2d center position vector and radius - * @param position 2d position that defines the current obstacle position - * @param radius radius of the obstacle - */ - CircularObstacle(const Eigen::Ref< const Eigen::Vector2d>& position, double radius) : Obstacle(), pos_(position), radius_(radius) - {} - - /** - * @brief Construct CircularObstacle using x- and y-center-coordinates and radius - * @param x x-coordinate - * @param y y-coordinate - * @param radius radius of the obstacle - */ - CircularObstacle(double x, double y, double radius) : Obstacle(), pos_(Eigen::Vector2d(x,y)), radius_(radius) - {} - - - // implements checkCollision() of the base class - virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const - { - return getMinimumDistance(point) < min_dist; - } - - - // implements checkLineIntersection() of the base class - virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const - { - // Distance Line - Circle - // refer to http://www.spieleprogrammierer.de/wiki/2D-Kollisionserkennung#Kollision_Kreis-Strecke - Eigen::Vector2d a = line_end-line_start; // not normalized! a=y-x - Eigen::Vector2d b = pos_-line_start; // b=m-x - - // Now find nearest point to circle v=x+a*t with t=a*b/(a*a) and bound to 0<=t<=1 - double t = a.dot(b)/a.dot(a); - if (t<0) t=0; // bound t (since a is not normalized, t can be scaled between 0 and 1 to parametrize the line - else if (t>1) t=1; - Eigen::Vector2d nearest_point = line_start + a*t; - - // check collision - return checkCollision(nearest_point, min_dist); - } - - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& position) const - { - return (position-pos_).norm() - radius_; - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const - { - return distance_point_to_segment_2d(pos_, line_start, line_end) - radius_; - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Point2dContainer& polygon) const - { - return distance_point_to_polygon_2d(pos_, polygon) - radius_; - } - - // implements getMinimumDistanceVec() of the base class - virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const - { - return pos_ + radius_*(position-pos_).normalized(); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const - { - return (pos_ + t*centroid_velocity_ - position).norm() - radius_; - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const - { - return distance_point_to_segment_2d(pos_ + t*centroid_velocity_, line_start, line_end) - radius_; - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const - { - return distance_point_to_polygon_2d(pos_ + t*centroid_velocity_, polygon) - radius_; - } - - // implements predictCentroidConstantVelocity() of the base class - virtual void predictCentroidConstantVelocity(double t, Eigen::Ref position) const - { - position = pos_ + t*centroid_velocity_; - } - - // implements getCentroid() of the base class - virtual const Eigen::Vector2d& getCentroid() const - { - return pos_; - } - - // implements getCentroidCplx() of the base class - virtual std::complex getCentroidCplx() const - { - return std::complex(pos_[0],pos_[1]); - } - - // Accessor methods - const Eigen::Vector2d& position() const {return pos_;} //!< Return the current position of the obstacle (read-only) - Eigen::Vector2d& position() {return pos_;} //!< Return the current position of the obstacle - double& x() {return pos_.coeffRef(0);} //!< Return the current x-coordinate of the obstacle - const double& x() const {return pos_.coeffRef(0);} //!< Return the current y-coordinate of the obstacle (read-only) - double& y() {return pos_.coeffRef(1);} //!< Return the current x-coordinate of the obstacle - const double& y() const {return pos_.coeffRef(1);} //!< Return the current y-coordinate of the obstacle (read-only) - double& radius() {return radius_;} //!< Return the current radius of the obstacle - const double& radius() const {return radius_;} //!< Return the current radius of the obstacle - - // implements toPolygonMsg() of the base class - virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon) - { - // TODO(roesmann): the polygon message type cannot describe a "perfect" circle - // We could switch to ObstacleMsg if required somewhere... - polygon.points.resize(1); - polygon.points.front().x = pos_.x(); - polygon.points.front().y = pos_.y(); - polygon.points.front().z = 0; - } - -protected: - - Eigen::Vector2d pos_; //!< Store the center position of the CircularObstacle - double radius_ = 0.0; //!< Radius of the obstacle - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** -* @class LineObstacle -* @brief Implements a 2D line obstacle -*/ - -class LineObstacle : public Obstacle -{ -public: - //! Abbrev. for a container storing vertices (2d points defining the edge points of the polygon) - typedef std::vector > VertexContainer; - - /** - * @brief Default constructor of the point obstacle class - */ - LineObstacle() : Obstacle() - { - start_.setZero(); - end_.setZero(); - centroid_.setZero(); - } - - /** - * @brief Construct LineObstacle using 2d position vectors as start and end of the line - * @param line_start 2d position that defines the start of the line obstacle - * @param line_end 2d position that defines the end of the line obstacle - */ - LineObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end) - : Obstacle(), start_(line_start), end_(line_end) - { - calcCentroid(); - } - - /** - * @brief Construct LineObstacle using start and end coordinates - * @param x1 x-coordinate of the start of the line - * @param y1 y-coordinate of the start of the line - * @param x2 x-coordinate of the end of the line - * @param y2 y-coordinate of the end of the line - */ - LineObstacle(double x1, double y1, double x2, double y2) : Obstacle() - { - start_.x() = x1; - start_.y() = y1; - end_.x() = x2; - end_.y() = y2; - calcCentroid(); - } - - // implements checkCollision() of the base class - virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const - { - return getMinimumDistance(point) <= min_dist; - } - - // implements checkLineIntersection() of the base class - virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const - { - return check_line_segments_intersection_2d(line_start, line_end, start_, end_); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& position) const - { - return distance_point_to_segment_2d(position, start_, end_); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const - { - return distance_segment_to_segment_2d(start_, end_, line_start, line_end); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Point2dContainer& polygon) const - { - return distance_segment_to_polygon_2d(start_, end_, polygon); - } - - // implements getMinimumDistanceVec() of the base class - virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const - { - return closest_point_on_line_segment_2d(position, start_, end_); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const - { - Eigen::Vector2d offset = t*centroid_velocity_; - return distance_point_to_segment_2d(position, start_ + offset, end_ + offset); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const - { - Eigen::Vector2d offset = t*centroid_velocity_; - return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const - { - Eigen::Vector2d offset = t*centroid_velocity_; - return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon); - } - - // implements getCentroid() of the base class - virtual const Eigen::Vector2d& getCentroid() const - { - return centroid_; - } - - // implements getCentroidCplx() of the base class - virtual std::complex getCentroidCplx() const - { - return std::complex(centroid_.x(), centroid_.y()); - } - - // Access or modify line - const Eigen::Vector2d& start() const {return start_;} - void setStart(const Eigen::Ref& start) {start_ = start; calcCentroid();} - const Eigen::Vector2d& end() const {return end_;} - void setEnd(const Eigen::Ref& end) {end_ = end; calcCentroid();} - - // implements toPolygonMsg() of the base class - virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon) - { - polygon.points.resize(2); - polygon.points.front().x = start_.x(); - polygon.points.front().y = start_.y(); - - polygon.points.back().x = end_.x(); - polygon.points.back().y = end_.y(); - polygon.points.back().z = polygon.points.front().z = 0; - } - -protected: - void calcCentroid() { centroid_ = 0.5*(start_ + end_); } - -private: - Eigen::Vector2d start_; - Eigen::Vector2d end_; - - Eigen::Vector2d centroid_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -/** -* @class PillObstacle -* @brief Implements a 2D pill/stadium/capsular-shaped obstacle (line + distance/radius) -*/ - -class PillObstacle : public Obstacle -{ -public: - - /** - * @brief Default constructor of the point obstacle class - */ - PillObstacle() : Obstacle() - { - start_.setZero(); - end_.setZero(); - centroid_.setZero(); - } - - /** - * @brief Construct LineObstacle using 2d position vectors as start and end of the line - * @param line_start 2d position that defines the start of the line obstacle - * @param line_end 2d position that defines the end of the line obstacle - */ - PillObstacle(const Eigen::Ref< const Eigen::Vector2d>& line_start, const Eigen::Ref< const Eigen::Vector2d>& line_end, double radius) - : Obstacle(), start_(line_start), end_(line_end), radius_(radius) - { - calcCentroid(); - } - - /** - * @brief Construct LineObstacle using start and end coordinates - * @param x1 x-coordinate of the start of the line - * @param y1 y-coordinate of the start of the line - * @param x2 x-coordinate of the end of the line - * @param y2 y-coordinate of the end of the line - */ - PillObstacle(double x1, double y1, double x2, double y2, double radius) : Obstacle(), radius_(radius) - { - start_.x() = x1; - start_.y() = y1; - end_.x() = x2; - end_.y() = y2; - calcCentroid(); - } - - // implements checkCollision() of the base class - virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const - { - return getMinimumDistance(point) <= min_dist; - } - - // implements checkLineIntersection() of the base class - virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const - { - return check_line_segments_intersection_2d(line_start, line_end, start_, end_); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& position) const - { - return distance_point_to_segment_2d(position, start_, end_) - radius_; - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const - { - return distance_segment_to_segment_2d(start_, end_, line_start, line_end) - radius_; - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Point2dContainer& polygon) const - { - return distance_segment_to_polygon_2d(start_, end_, polygon) - radius_; - } - - // implements getMinimumDistanceVec() of the base class - virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const - { - Eigen::Vector2d closed_point_line = closest_point_on_line_segment_2d(position, start_, end_); - return closed_point_line + radius_*(position-closed_point_line).normalized(); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const - { - Eigen::Vector2d offset = t*centroid_velocity_; - return distance_point_to_segment_2d(position, start_ + offset, end_ + offset) - radius_; - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const - { - Eigen::Vector2d offset = t*centroid_velocity_; - return distance_segment_to_segment_2d(start_ + offset, end_ + offset, line_start, line_end) - radius_; - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const - { - Eigen::Vector2d offset = t*centroid_velocity_; - return distance_segment_to_polygon_2d(start_ + offset, end_ + offset, polygon) - radius_; - } - - // implements getCentroid() of the base class - virtual const Eigen::Vector2d& getCentroid() const - { - return centroid_; - } - - // implements getCentroidCplx() of the base class - virtual std::complex getCentroidCplx() const - { - return std::complex(centroid_.x(), centroid_.y()); - } - - // Access or modify line - const Eigen::Vector2d& start() const {return start_;} - void setStart(const Eigen::Ref& start) {start_ = start; calcCentroid();} - const Eigen::Vector2d& end() const {return end_;} - void setEnd(const Eigen::Ref& end) {end_ = end; calcCentroid();} - - // implements toPolygonMsg() of the base class - virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon) - { - // Currently, we only export the line - // TODO(roesmann): export whole pill - polygon.points.resize(2); - polygon.points.front().x = start_.x(); - polygon.points.front().y = start_.y(); - - polygon.points.back().x = end_.x(); - polygon.points.back().y = end_.y(); - polygon.points.back().z = polygon.points.front().z = 0; - } - -protected: - void calcCentroid() { centroid_ = 0.5*(start_ + end_); } - -private: - Eigen::Vector2d start_; - Eigen::Vector2d end_; - double radius_ = 0.0; - - Eigen::Vector2d centroid_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * @class PolygonObstacle - * @brief Implements a polygon obstacle with an arbitrary number of vertices - * @details If the polygon has only 2 vertices, than it is considered as a line, - * otherwise the polygon will always be closed (a connection between the first and the last vertex - * is included automatically). - */ -class PolygonObstacle : public Obstacle -{ -public: - - /** - * @brief Default constructor of the polygon obstacle class - */ - PolygonObstacle() : Obstacle(), finalized_(false) - { - centroid_.setConstant(NAN); - } - - /** - * @brief Construct polygon obstacle with a list of vertices - */ - PolygonObstacle(const Point2dContainer& vertices) : Obstacle(), vertices_(vertices) - { - finalizePolygon(); - } - - - /* FIXME Not working at the moment due to the aligned allocator version of std::vector - * And it is C++11 code that is disabled atm to ensure compliance with ROS indigo/jade - template - PolygonObstacle(const Vector2dType&... vertices) : _vertices({vertices...}) - { - calcCentroid(); - _finalized = true; - } - */ - - - // implements checkCollision() of the base class - virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const - { - // line case - if (noVertices()==2) - return getMinimumDistance(point) <= min_dist; - - // check if point is in the interior of the polygon - // point in polygon test - raycasting (http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html) - // using the following algorithm we may obtain false negatives on edge-cases, but that's ok for our purposes - int i, j; - bool c = false; - for (i = 0, j = noVertices()-1; i < noVertices(); j = i++) - { - if ( ((vertices_.at(i).y()>point.y()) != (vertices_.at(j).y()>point.y())) && - (point.x() < (vertices_.at(j).x()-vertices_.at(i).x()) * (point.y()-vertices_.at(i).y()) / (vertices_.at(j).y()-vertices_.at(i).y()) + vertices_.at(i).x()) ) - c = !c; - } - if (c>0) return true; - - // If this statement is reached, the point lies outside the polygon or maybe on its edges - // Let us check the minium distance as well - return min_dist == 0 ? false : getMinimumDistance(point) < min_dist; - } - - - /** - * @brief Check if a given line segment between two points intersects with the obstacle (and additionally keeps a safty distance \c min_dist) - * @param line_start 2D point for the end of the reference line - * @param line_end 2D point for the end of the reference line - * @param min_dist Minimum distance allowed to the obstacle to be collision/intersection free - * @remarks we ignore \c min_dist here - * @return \c true if given line intersects the region of the obstacle or if the minimum distance is lower than min_dist - */ - virtual bool checkLineIntersection(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double min_dist=0) const; - - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& position) const - { - return distance_point_to_polygon_2d(position, vertices_); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) const - { - return distance_segment_to_polygon_2d(line_start, line_end, vertices_); - } - - // implements getMinimumDistance() of the base class - virtual double getMinimumDistance(const Point2dContainer& polygon) const - { - return distance_polygon_to_polygon_2d(polygon, vertices_); - } - - // implements getMinimumDistanceVec() of the base class - virtual Eigen::Vector2d getClosestPoint(const Eigen::Vector2d& position) const; - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& position, double t) const - { - Point2dContainer pred_vertices; - predictVertices(t, pred_vertices); - return distance_point_to_polygon_2d(position, pred_vertices); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end, double t) const - { - Point2dContainer pred_vertices; - predictVertices(t, pred_vertices); - return distance_segment_to_polygon_2d(line_start, line_end, pred_vertices); - } - - // implements getMinimumSpatioTemporalDistance() of the base class - virtual double getMinimumSpatioTemporalDistance(const Point2dContainer& polygon, double t) const - { - Point2dContainer pred_vertices; - predictVertices(t, pred_vertices); - return distance_polygon_to_polygon_2d(polygon, pred_vertices); - } - - virtual void predictVertices(double t, Point2dContainer& pred_vertices) const - { - // Predict obstacle (polygon) at time t - pred_vertices.resize(vertices_.size()); - Eigen::Vector2d offset = t*centroid_velocity_; - for (std::size_t i = 0; i < vertices_.size(); i++) - { - pred_vertices[i] = vertices_[i] + offset; - } - } - - // implements getCentroid() of the base class - virtual const Eigen::Vector2d& getCentroid() const - { - assert(finalized_ && "Finalize the polygon after all vertices are added."); - return centroid_; - } - - // implements getCentroidCplx() of the base class - virtual std::complex getCentroidCplx() const - { - assert(finalized_ && "Finalize the polygon after all vertices are added."); - return std::complex(centroid_.coeffRef(0), centroid_.coeffRef(1)); - } - - // implements toPolygonMsg() of the base class - virtual void toPolygonMsg(geometry_msgs::msg::Polygon& polygon); - - - /** @name Define the polygon */ - ///@{ - - // Access or modify polygon - const Point2dContainer& vertices() const {return vertices_;} //!< Access vertices container (read-only) - Point2dContainer& vertices() {return vertices_;} //!< Access vertices container - - /** - * @brief Add a vertex to the polygon (edge-point) - * @remarks You do not need to close the polygon (do not repeat the first vertex) - * @warning Do not forget to call finalizePolygon() after adding all vertices - * @param vertex 2D point defining a new polygon edge - */ - void pushBackVertex(const Eigen::Ref& vertex) - { - vertices_.push_back(vertex); - finalized_ = false; - } - - /** - * @brief Add a vertex to the polygon (edge-point) - * @remarks You do not need to close the polygon (do not repeat the first vertex) - * @warning Do not forget to call finalizePolygon() after adding all vertices - * @param x x-coordinate of the new vertex - * @param y y-coordinate of the new vertex - */ - void pushBackVertex(double x, double y) - { - vertices_.push_back(Eigen::Vector2d(x,y)); - finalized_ = false; - } - - /** - * @brief Call finalizePolygon after the polygon is created with the help of pushBackVertex() methods - */ - void finalizePolygon() - { - fixPolygonClosure(); - calcCentroid(); - finalized_ = true; - } - - /** - * @brief Clear all vertices (Afterwards the polygon is not valid anymore) - */ - void clearVertices() {vertices_.clear(); finalized_ = false;} - - /** - * @brief Get the number of vertices defining the polygon (the first vertex is counted once) - */ - int noVertices() const {return (int)vertices_.size();} - - - ///@} - -protected: - - void fixPolygonClosure(); //!< Check if the current polygon contains the first vertex twice (as start and end) and in that case erase the last redundant one. - - void calcCentroid(); //!< Compute the centroid of the polygon (called inside finalizePolygon()) - - - Point2dContainer vertices_; //!< Store vertices defining the polygon (@see pushBackVertex) - Eigen::Vector2d centroid_; //!< Store the centroid coordinates of the polygon (@see calcCentroid) - - bool finalized_; //!< Flat that keeps track if the polygon was finalized after adding all vertices - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -} // namespace teb_local_planner - -#endif /* OBSTACLES_H */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/optimal_planner.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/optimal_planner.h deleted file mode 100644 index f76efe8..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/optimal_planner.h +++ /dev/null @@ -1,736 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef OPTIMAL_PLANNER_H_ -#define OPTIMAL_PLANNER_H_ - -#include - - -// teb stuff -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/misc.h" -#include "teb_local_planner/timed_elastic_band.h" -#include "teb_local_planner/planner_interface.h" -#include "teb_local_planner/visualization.h" -#include "teb_local_planner/robot_footprint_model.h" - -// g2o lib stuff -#include "g2o/core/sparse_optimizer.h" -#include "g2o/core/block_solver.h" -#include "g2o/core/factory.h" -#include "g2o/core/optimization_algorithm_gauss_newton.h" -#include "g2o/core/optimization_algorithm_levenberg.h" -#include "g2o/solvers/csparse/linear_solver_csparse.h" -#include "g2o/solvers/cholmod/linear_solver_cholmod.h" - -// g2o custom edges and vertices for the TEB planner -#include "teb_local_planner/g2o_types/edge_velocity.h" -#include "teb_local_planner/g2o_types/edge_acceleration.h" -#include "teb_local_planner/g2o_types/edge_kinematics.h" -#include "teb_local_planner/g2o_types/edge_time_optimal.h" -#include "teb_local_planner/g2o_types/edge_shortest_path.h" -#include "teb_local_planner/g2o_types/edge_obstacle.h" -#include "teb_local_planner/g2o_types/edge_dynamic_obstacle.h" -#include "teb_local_planner/g2o_types/edge_via_point.h" -#include "teb_local_planner/g2o_types/edge_prefer_rotdir.h" - -// messages -#include -#include -#include -#include -#include - -#include - -#include - -#include - -namespace teb_local_planner -{ - -//! Typedef for the block solver utilized for optimization -typedef g2o::BlockSolverX TEBBlockSolver; - -//! Typedef for the linear solver utilized for optimization -typedef g2o::LinearSolverCSparse TEBLinearSolver; -//typedef g2o::LinearSolverCholmod TEBLinearSolver; - -//! Typedef for a container storing via-points -typedef std::vector< Eigen::Vector2d, Eigen::aligned_allocator > ViaPointContainer; - - -/** - * @class TebOptimalPlanner - * @brief This class optimizes an internal Timed Elastic Band trajectory using the g2o-framework. - * - * For an introduction and further details about the TEB optimization problem refer to: - * - C. Rösmann et al.: Trajectory modification considering dynamic constraints of autonomous robots, ROBOTIK, 2012. - * - C. Rösmann et al.: Efficient trajectory optimization using a sparse model, ECMR, 2013. - * - R. Kümmerle et al.: G2o: A general framework for graph optimization, ICRA, 2011. - * - * @todo: Call buildGraph() only if the teb structure has been modified to speed up hot-starting from previous solutions. - * @todo: We introduced the non-fast mode with the support of dynamic obstacles - * (which leads to better results in terms of x-y-t homotopy planning). - * However, we have not tested this mode intensively yet, so we keep - * the legacy fast mode as default until we finish our tests. - */ -class TebOptimalPlanner : public PlannerInterface -{ -public: - - /** - * @brief Default constructor - */ - TebOptimalPlanner(); - - /** - * @brief Construct and initialize the TEB optimal planner. - * @param cfg Const reference to the TebConfig class for internal parameters - * @param obstacles Container storing all relevant obstacles (see Obstacle) - * @param visual Shared pointer to the TebVisualization class (optional) - * @param via_points Container storing via-points (optional) - */ - TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, - TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); - - /** - * @brief Destruct the optimal planner. - */ - virtual ~TebOptimalPlanner(); - - /** - * @brief Initializes the optimal planner - * @param node Shared pointer for rclcpp::Node - * @param cfg Const reference to the TebConfig class for internal parameters - * @param obstacles Container storing all relevant obstacles (see Obstacle) - * @param robot_model Shared pointer to the robot shape model used for optimization (optional) - * @param visual Shared pointer to the TebVisualization class (optional) - * @param via_points Container storing via-points (optional) - */ - void initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles = NULL, - TebVisualizationPtr visual = TebVisualizationPtr(), const ViaPointContainer* via_points = NULL); - - /** @name Plan a trajectory */ - //@{ - - /** - * @brief Plan a trajectory based on an initial reference plan. - * - * Call this method to create and optimize a trajectory that is initialized - * according to an initial reference plan (given as a container of poses). \n - * The method supports hot-starting from previous solutions, if avaiable: \n - * - If no trajectory exist yet, a new trajectory is initialized based on the initial plan, - * see TimedElasticBand::initTEBtoGoal - * - If a previous solution is avaiable, update the trajectory based on the initial plan, - * see bool TimedElasticBand::updateAndPruneTEB - * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o - * @param initial_plan vector of geometry_msgs::msg::PoseStamped - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const std::vector& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false); - - /** - * @brief Plan a trajectory between a given start and goal pose (tf::Pose version) - * - * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n - * The method supports hot-starting from previous solutions, if avaiable: \n - * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses, - * see TimedElasticBand::initTEBtoGoal - * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB - * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o - * @param start tf::Pose containing the start pose of the trajectory - * @param goal tf::Pose containing the goal pose of the trajectory - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x, linear.y (holonomic) and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - // tf2 doesn't have tf::Pose -// virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false); - - /** - * @brief Plan a trajectory between a given start and goal pose - * - * Call this method to create and optimize a trajectory that is initialized between a given start and goal pose. \n - * The method supports hot-starting from previous solutions, if avaiable: \n - * - If no trajectory exist yet, a new trajectory is initialized between start and goal poses - * @see TimedElasticBand::initTEBtoGoal - * - If a previous solution is avaiable, update the trajectory @see bool TimedElasticBand::updateAndPruneTEB - * - Afterwards optimize the recently initialized or updated trajectory by calling optimizeTEB() and invoking g2o - * @param start PoseSE2 containing the start pose of the trajectory - * @param goal PoseSE2 containing the goal pose of the trajectory - * @param start_vel Initial velocity at the start pose (twist message containing the translational and angular velocity). - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false); - - - /** - * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. - * @warning Call plan() first and check if the generated plan is feasible. - * @param[out] vx translational velocity [m/s] - * @param[out] vy strafing velocity which can be nonzero for holonomic robots[m/s] - * @param[out] omega rotational velocity [rad/s] - * @param[in] look_ahead_poses index of the final pose used to compute the velocity command. - * @return \c true if command is valid, \c false otherwise - */ - virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const; - - - /** - * @brief Optimize a previously initialized trajectory (actual TEB optimization loop). - * - * optimizeTEB implements the main optimization loop. \n - * It consist of two nested loops: - * - The outer loop resizes the trajectory according to the temporal resolution by invoking TimedElasticBand::autoResize(). - * Afterwards the internal method optimizeGraph() is called that constitutes the innerloop. - * - The inner loop calls the solver (g2o framework, resp. sparse Levenberg-Marquardt) and iterates a specified - * number of optimization calls (\c iterations_innerloop). - * - * The outer loop is repeated \c iterations_outerloop times. \n - * The ratio of inner and outer loop iterations significantly defines the contraction behavior - * and convergence rate of the trajectory optimization. Based on our experiences, 2-6 innerloop iterations are sufficient. \n - * The number of outer loop iterations should be determined by considering the maximum CPU time required to match the control rate. \n - * Optionally, the cost vector can be calculated by specifying \c compute_cost_afterwards, see computeCurrentCost(). - * @remarks This method is usually called from a plan() method - * @param iterations_innerloop Number of iterations for the actual solver loop - * @param iterations_outerloop Specifies how often the trajectory should be resized followed by the inner solver loop. - * @param compute_cost_afterwards if \c true Calculate the cost vector according to computeCurrentCost(), - * the vector can be accessed afterwards using getCurrentCost(). - * @param obst_cost_scale Specify extra scaling for obstacle costs (only used if \c compute_cost_afterwards is true) - * @param viapoint_cost_scale Specify extra scaling for via-point costs (only used if \c compute_cost_afterwards is true) - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time - * (only used if \c compute_cost_afterwards is true). - * @return \c true if the optimization terminates successfully, \c false otherwise - */ - bool optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards = false, - double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); - - //@} - - - /** @name Desired initial and final velocity */ - //@{ - - - /** - * @brief Set the initial velocity at the trajectory's start pose (e.g. the robot's velocity) [twist overload]. - * @remarks Calling this function is not neccessary if the initial velocity is passed via the plan() method - * @param vel_start Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used, - * for holonomic robots also linear.y) - */ - void setVelocityStart(const geometry_msgs::msg::Twist& vel_start); - - /** - * @brief Set the desired final velocity at the trajectory's goal pose. - * @remarks Call this function only if a non-zero velocity is desired and if \c free_goal_vel is set to \c false in plan() - * @param vel_goal twist message containing the translational and angular final velocity - */ - void setVelocityGoal(const geometry_msgs::msg::Twist& vel_goal); - - /** - * @brief Set the desired final velocity at the trajectory's goal pose to be the maximum velocity limit - * @remarks Calling this function is not neccessary if \c free_goal_vel is set to \c false in plan() - */ - void setVelocityGoalFree() {vel_goal_.first = false;} - - //@} - - - /** @name Take obstacles into account */ - //@{ - - - /** - * @brief Assign a new set of obstacles - * @param obst_vector pointer to an obstacle container (can also be a nullptr) - * @remarks This method overrids the obstacle container optinally assigned in the constructor. - */ - void setObstVector(ObstContainer* obst_vector) {obstacles_ = obst_vector;} - - /** - * @brief Access the internal obstacle container. - * @return Const reference to the obstacle container - */ - const ObstContainer& getObstVector() const {return *obstacles_;} - - //@} - - /** @name Take via-points into account */ - //@{ - - - /** - * @brief Assign a new set of via-points - * @param via_points pointer to a via_point container (can also be a nullptr) - * @details Any previously set container will be overwritten. - */ - void setViaPoints(const ViaPointContainer* via_points) {via_points_ = via_points;} - - /** - * @brief Access the internal via-point container. - * @return Const reference to the via-point container - */ - const ViaPointContainer& getViaPoints() const {return *via_points_;} - - //@} - - - /** @name Visualization */ - //@{ - - /** - * @brief Register a TebVisualization class to enable visiualization routines (e.g. publish the local plan and pose sequence) - * @param visualization shared pointer to a TebVisualization instance - * @see visualize - */ - void setVisualization(const TebVisualizationPtr & visualization) override; - - /** - * @brief Publish the local plan and pose sequence via ros topics (e.g. subscribe with rviz). - * - * Make sure to register a TebVisualization instance before using setVisualization() or an overlaoded constructor. - * @see setVisualization - */ - virtual void visualize(); - - //@} - - - /** @name Utility methods and more */ - //@{ - - /** - * @brief Reset the planner by clearing the internal graph and trajectory. - */ - virtual void clearPlanner() - { - clearGraph(); - teb_.clearTimedElasticBand(); - } - - /** - * @brief Prefer a desired initial turning direction (by penalizing the opposing one) - * - * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two - * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. - * Initial means that the penalty is applied only to the first few poses of the trajectory. - * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none) - */ - virtual void setPreferredTurningDir(RotType dir) {prefer_rotdir_=dir;} - - /** - * @brief Register the vertices and edges defined for the TEB to the g2o::Factory. - * - * This allows the user to export the internal graph to a text file for instance. - * Access the optimizer() for more details. - */ - static void registerG2OTypes(); - - /** - * @brief Access the internal TimedElasticBand trajectory. - * @warning In general, the underlying teb must not be modified directly. Use with care... - * @return reference to the teb - */ - TimedElasticBand& teb() {return teb_;}; - - /** - * @brief Access the internal TimedElasticBand trajectory (read-only). - * @return const reference to the teb - */ - const TimedElasticBand& teb() const {return teb_;}; - - /** - * @brief Access the internal g2o optimizer. - * @warning In general, the underlying optimizer must not be modified directly. Use with care... - * @return const shared pointer to the g2o sparse optimizer - */ - std::shared_ptr optimizer() {return optimizer_;}; - - /** - * @brief Access the internal g2o optimizer (read-only). - * @return const shared pointer to the g2o sparse optimizer - */ - std::shared_ptr optimizer() const {return optimizer_;}; - - /** - * @brief Check if last optimization was successful - * @return \c true if the last optimization returned without errors, - * otherwise \c false (also if no optimization has been called before). - */ - bool isOptimized() const {return optimized_;}; - - /** - * @brief Returns true if the planner has diverged. - */ - bool hasDiverged() const override; - - /** - * @brief Compute the cost vector of a given optimization problen (hyper-graph must exist). - * - * Use this method to obtain information about the current edge errors / costs (local cost functions). \n - * The vector of cost values is composed according to the different edge types (time_optimal, obstacles, ...). \n - * Refer to the method declaration for the detailed composition. \n - * The cost for the edges that minimize time differences (EdgeTimeOptimal) corresponds to the sum of all single - * squared time differneces: \f$ \sum_i \Delta T_i^2 \f$. Sometimes, the user may want to get a value that is proportional - * or identical to the actual trajectory transition time \f$ \sum_i \Delta T_i \f$. \n - * Set \c alternative_time_cost to true in order to get the cost calculated using the latter equation, but check the - * implemented definition, if the value is scaled to match the magnitude of other cost values. - * - * @todo Remove the scaling term for the alternative time cost. - * @todo Can we use the last error (chi2) calculated from g2o instead of calculating it by ourself? - * @see getCurrentCost - * @see optimizeTEB - * @param obst_cost_scale Specify extra scaling for obstacle costs. - * @param viapoint_cost_scale Specify extra scaling for via points. - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time. - * @return TebCostVec containing the cost values - */ - void computeCurrentCost(double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false); - - /** - * Compute and return the cost of the current optimization graph (supports multiple trajectories) - * @param[out] cost current cost value for each trajectory - * [for a planner with just a single trajectory: size=1, vector will not be cleared] - * @param obst_cost_scale Specify extra scaling for obstacle costs - * @param viapoint_cost_scale Specify extra scaling for via points. - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time - */ - virtual void computeCurrentCost(std::vector& cost, double obst_cost_scale=1.0, double viapoint_cost_scale=1.0, bool alternative_time_cost=false) - { - computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); - cost.push_back( getCurrentCost() ); - } - - /** - * @brief Access the cost vector. - * - * The accumulated cost value previously calculated using computeCurrentCost - * or by calling optimizeTEB with enabled cost flag. - * @return const reference to the TebCostVec. - */ - double getCurrentCost() const {return cost_;} - - - /** - * @brief Extract the velocity from consecutive poses and a time difference (including strafing velocity for holonomic robots) - * - * The velocity is extracted using finite differences. - * The direction of the translational velocity is also determined. - * @param pose1 pose at time k - * @param pose2 consecutive pose at time k+1 - * @param dt actual time difference between k and k+1 (must be >0 !!!) - * @param[out] vx translational velocity - * @param[out] vy strafing velocity which can be nonzero for holonomic robots - * @param[out] omega rotational velocity - */ - inline void extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const; - - /** - * @brief Compute the velocity profile of the trajectory - * - * This method computes the translational and rotational velocity for the complete - * planned trajectory. - * The first velocity is the one that is provided as initial velocity (fixed). - * Velocities at index k=2...end-1 are related to the transition from pose_{k-1} to pose_k. - * The last velocity is the final velocity (fixed). - * The number of Twist objects is therefore sizePoses()+1; - * In summary: - * v[0] = v_start, - * v[1,...end-1] = +-(pose_{k+1}-pose{k})/dt, - * v(end) = v_goal - * It can be used for evaluation and debugging purposes or - * for open-loop control. For computing the velocity required for controlling the robot - * to the next step refer to getVelocityCommand(). - * @param[out] velocity_profile velocity profile will be written to this vector (after clearing any existing content) with the size=no_poses+1 - */ - void getVelocityProfile(std::vector& velocity_profile) const; - - /** - * @brief Return the complete trajectory including poses, velocity profiles and temporal information - * - * It is useful for evaluation and debugging purposes or for open-loop control. - * Since the velocity obtained using difference quotients is the mean velocity between consecutive poses, - * the velocity at each pose at time stamp k is obtained by taking the average between both velocities. - * The velocity of the first pose is v_start (provided initial value) and the last one is v_goal (usually zero, if free_goal_vel is off). - * See getVelocityProfile() for the list of velocities between consecutive points. - * @todo The acceleration profile is not added at the moment. - * @param[out] trajectory the resulting trajectory - */ - void getFullTrajectory(std::vector& trajectory) const; - - /** - * @brief Check whether the planned trajectory is feasible or not. - * - * This method currently checks only that the trajectory, or a part of the trajectory is collision free. - * Obstacles are here represented as costmap instead of the internal ObstacleContainer. - * @param costmap_model Pointer to the costmap model - * @param footprint_spec The specification of the footprint of the robot in world coordinates - * @param inscribed_radius The radius of the inscribed circle of the robot - * @param circumscribed_radius The radius of the circumscribed circle of the robot - * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. - * @return \c true, if the robot footprint along the first part of the trajectory intersects with - * any obstacle in the costmap, \c false otherwise. - */ - virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector& footprint_spec, double inscribed_radius = 0.0, - double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1); - - /** - * @brief Check whether the footprint of the robot at the pose touches an obstacle or not. - * - * @param pose2d Pose to check - * @param costmap_model Pointer to the costmap model - * @param footprint_spec The specification of the footprint of the robot in world coordinates - * @return \c true, if the robot pose is valid, \c false otherwise. - */ - virtual bool isPoseValid(geometry_msgs::msg::Pose2D pose2d, dwb_critics::ObstacleFootprintCritic* costmap_model, - const std::vector& footprint_spec); - - //@} - -protected: - - /** @name Hyper-Graph creation and optimization */ - //@{ - - /** - * @brief Build the hyper-graph representing the TEB optimization problem. - * - * This method creates the optimization problem according to the hyper-graph formulation. \n - * For more details refer to the literature cited in the TebOptimalPlanner class description. - * @see optimizeGraph - * @see clearGraph - * @param weight_multiplier Specify a weight multipler for selected weights in optimizeGraph - * This might be used for weight adapation strategies. - * Currently, only obstacle collision weights are considered. - * @return \c true, if the graph was created successfully, \c false otherwise. - */ - bool buildGraph(double weight_multiplier=1.0); - - /** - * @brief Optimize the previously constructed hyper-graph to deform / optimize the TEB. - * - * This method invokes the g2o framework to solve the optimization problem considering dedicated sparsity patterns. \n - * The current implementation calls a non-constrained sparse Levenberg-Marquardt algorithm. Constraints are considered - * by utilizing penalty approximations. Refer to the literature cited in the TebOptimalPlanner class description. - * @see buildGraph - * @see clearGraph - * @param no_iterations Number of solver iterations - * @param clear_after Clear the graph after optimization. - * @return \c true, if optimization terminates successfully, \c false otherwise. - */ - bool optimizeGraph(int no_iterations, bool clear_after=true); - - /** - * @brief Clear an existing internal hyper-graph. - * @see buildGraph - * @see optimizeGraph - */ - void clearGraph(); - - /** - * @brief Add all relevant vertices to the hyper-graph as optimizable variables. - * - * Vertices (if unfixed) represent the variables that will be optimized. \n - * In case of the Timed-Elastic-Band poses and time differences form the vertices of the hyper-graph. \n - * The order of insertion of vertices (to the graph) is important for efficiency, - * since it affect the sparsity pattern of the underlying hessian computed for optimization. - * @see VertexPose - * @see VertexTimeDiff - * @see buildGraph - * @see optimizeGraph - */ - void AddTEBVertices(); - - /** - * @brief Add all edges (local cost functions) for limiting the translational and angular velocity. - * @see EdgeVelocity - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesVelocity(); - - /** - * @brief Add all edges (local cost functions) for limiting the translational and angular acceleration. - * @see EdgeAcceleration - * @see EdgeAccelerationStart - * @see EdgeAccelerationGoal - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesAcceleration(); - - /** - * @brief Add all edges (local cost functions) for minimizing the transition time (resp. minimize time differences) - * @see EdgeTimeOptimal - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesTimeOptimal(); - - /** - * @brief Add all edges (local cost functions) for minimizing the path length - * @see EdgeShortestPath - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesShortestPath(); - - /** - * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles - * @warning do not combine with AddEdgesInflatedObstacles - * @see EdgeObstacle - * @see buildGraph - * @see optimizeGraph - * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) - */ - void AddEdgesObstacles(double weight_multiplier=1.0); - - /** - * @brief Add all edges (local cost functions) related to keeping a distance from static obstacles (legacy association strategy) - * @see EdgeObstacle - * @see buildGraph - * @see optimizeGraph - * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) - */ - void AddEdgesObstaclesLegacy(double weight_multiplier=1.0); - - /** - * @brief Add all edges (local cost functions) related to minimizing the distance to via-points - * @see EdgeViaPoint - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesViaPoints(); - - /** - * @brief Add all edges (local cost functions) related to keeping a distance from dynamic (moving) obstacles. - * @warning experimental - * @todo Should we also add neighbors to decrease jiggling/oscillations - * @see EdgeDynamicObstacle - * @see buildGraph - * @see optimizeGraph - * @param weight_multiplier Specify an additional weight multipler (in addition to the the config weight) - - */ - void AddEdgesDynamicObstacles(double weight_multiplier=1.0); - - /** - * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a differential drive robot - * @warning do not combine with AddEdgesKinematicsCarlike() - * @see AddEdgesKinematicsCarlike - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesKinematicsDiffDrive(); - - /** - * @brief Add all edges (local cost functions) for satisfying kinematic constraints of a carlike robot - * @warning do not combine with AddEdgesKinematicsDiffDrive() - * @see AddEdgesKinematicsDiffDrive - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesKinematicsCarlike(); - - /** - * @brief Add all edges (local cost functions) for prefering a specifiy turning direction (by penalizing the other one) - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesPreferRotDir(); - - /** - * @brief Add all edges (local cost function) for reducing the velocity of a vertex due to its associated obstacles - * @see buildGraph - * @see optimizeGraph - */ - void AddEdgesVelocityObstacleRatio(); - - //@} - - - /** - * @brief Initialize and configure the g2o sparse optimizer. - * @return shared pointer to the g2o::SparseOptimizer instance - */ - std::shared_ptr initOptimizer(); - - - // external objects (store weak pointers) - const TebConfig* cfg_; //!< Config class that stores and manages all related parameters - ObstContainer* obstacles_; //!< Store obstacles that are relevant for planning - const ViaPointContainer* via_points_; //!< Store via points for planning - std::vector obstacles_per_vertex_; //!< Store the obstacles associated with the n-1 initial vertices - - double cost_; //!< Store cost value of the current hyper-graph - RotType prefer_rotdir_; //!< Store whether to prefer a specific initial rotation in optimization (might be activated in case the robot oscillates) - - // internal objects (memory management owned) - TebVisualizationPtr visualization_; //!< Instance of the visualization class - TimedElasticBand teb_; //!< Actual trajectory object - RobotFootprintModelPtr robot_model_; //!< Robot model - std::shared_ptr optimizer_; //!< g2o optimizer for trajectory optimization - std::pair vel_start_; //!< Store the initial velocity at the start pose - std::pair vel_goal_; //!< Store the final velocity at the goal pose - - bool initialized_; //!< Keeps track about the correct initialization of this class - bool optimized_; //!< This variable is \c true as long as the last optimization has been completed successful - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//! Abbrev. for shared instances of the TebOptimalPlanner -typedef std::shared_ptr TebOptimalPlannerPtr; -//! Abbrev. for shared const TebOptimalPlanner pointers -typedef std::shared_ptr TebOptimalPlannerConstPtr; -//! Abbrev. for containers storing multiple teb optimal planners -typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer; - -} // namespace teb_local_planner - -#endif /* OPTIMAL_PLANNER_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/planner_interface.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/planner_interface.h deleted file mode 100644 index 268ba84..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/planner_interface.h +++ /dev/null @@ -1,218 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef PLANNER_INTERFACE_H_ -#define PLANNER_INTERFACE_H_ - -#include - -// ros -#include - -#include -// this package -#include -#include - -#include - -#include - -#include - -// messages -#include -#include -#include - -// this package -#include "teb_local_planner/pose_se2.h" -#include "teb_local_planner/visualization.h" - -namespace teb_local_planner -{ - - -/** - * @class PlannerInterface - * @brief This abstract class defines an interface for local planners - */ -class PlannerInterface -{ -public: - - /** - * @brief Default constructor - */ - PlannerInterface() - { - } - /** - * @brief Virtual destructor. - */ - virtual ~PlannerInterface() - { - } - - - /** @name Plan a trajectory */ - //@{ - - /** - * @brief Plan a trajectory based on an initial reference plan. - * - * Provide this method to create and optimize a trajectory that is initialized - * according to an initial reference plan (given as a container of poses). - * @param initial_plan vector of geometry_msgs::msg::PoseStamped - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const std::vector& initial_plan, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0; - - /** - * @brief Plan a trajectory between a given start and goal pose (tf::Pose version). - * - * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose. - * @param start tf::Pose containing the start pose of the trajectory - * @param goal tf::Pose containing the goal pose of the trajectory - * @param start_vel Current start velocity (e.g. the velocity of the robot, only linear.x and angular.z are used) - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - // tf2 doesn't have tf2::Pose - //virtual bool plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0; - - /** - * @brief Plan a trajectory between a given start and goal pose. - * - * Provide this method to create and optimize a trajectory that is initialized between a given start and goal pose. - * @param start PoseSE2 containing the start pose of the trajectory - * @param goal PoseSE2 containing the goal pose of the trajectory - * @param start_vel Initial velocity at the start pose (twist msg containing the translational and angular velocity). - * @param free_goal_vel if \c true, a nonzero final velocity at the goal pose is allowed, - * otherwise the final velocity will be zero (default: false) - * @return \c true if planning was successful, \c false otherwise - */ - virtual bool plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel = NULL, bool free_goal_vel=false) = 0; - - /** - * @brief Get the velocity command from a previously optimized plan to control the robot at the current sampling interval. - * @warning Call plan() first and check if the generated plan is feasible. - * @param[out] vx translational velocity [m/s] - * @param[out] vy strafing velocity which can be nonzero for holonomic robots [m/s] - * @param[out] omega rotational velocity [rad/s] - * @param[in] look_ahead_poses index of the final pose used to compute the velocity command. - * @return \c true if command is valid, \c false otherwise - */ - virtual bool getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const = 0; - - //@} - - - /** - * @brief Reset the planner. - */ - virtual void clearPlanner() = 0; - - /** - * @brief Prefer a desired initial turning direction (by penalizing the opposing one) - * - * A desired (initial) turning direction might be specified in case the planned trajectory oscillates between two - * solutions (in the same equivalence class!) with similar cost. Check the parameters in order to adjust the weight of the penalty. - * Initial means that the penalty is applied only to the first few poses of the trajectory. - * @param dir This parameter might be RotType::left (prefer left), RotType::right (prefer right) or RotType::none (prefer none) - */ - virtual void setPreferredTurningDir(RotType dir) { - RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), - "setPreferredTurningDir() not implemented for this planner.");} - - /** - * @brief Visualize planner specific stuff. - * Overwrite this method to provide an interface to perform all planner related visualizations at once. - */ - virtual void visualize() - { - } - - virtual void setVisualization(const TebVisualizationPtr & visualization) = 0; - - /** - * @brief Check whether the planned trajectory is feasible or not. - * - * This method currently checks only that the trajectory, or a part of the trajectory is collision free. - * Obstacles are here represented as costmap instead of the internal ObstacleContainer. - * @param costmap_model Pointer to the costmap model - * @param footprint_spec The specification of the footprint of the robot in world coordinates - * @param inscribed_radius The radius of the inscribed circle of the robot - * @param circumscribed_radius The radius of the circumscribed circle of the robot - * @param look_ahead_idx Number of poses along the trajectory that should be verified, if -1, the complete trajectory will be checked. - * @return \c true, if the robot footprint along the first part of the trajectory intersects with - * any obstacle in the costmap, \c false otherwise. - */ - virtual bool isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector& footprint_spec, - double inscribed_radius = 0.0, double circumscribed_radius=0.0, int look_ahead_idx=-1, double feasibility_check_lookahead_distance=-1.0) = 0; - - /** - * Compute and return the cost of the current optimization graph (supports multiple trajectories) - * @param[out] cost current cost value for each trajectory - * [for a planner with just a single trajectory: size=1, vector will not be cleared] - * @param obst_cost_scale Specify extra scaling for obstacle costs - * @param alternative_time_cost Replace the cost for the time optimal objective by the actual (weighted) transition time - */ - virtual void computeCurrentCost(std::vector& cost, double obst_cost_scale=1.0, bool alternative_time_cost=false) - { - } - - /** - * @brief Returns true if the planner has diverged. - */ - virtual bool hasDiverged() const = 0; - - nav2_util::LifecycleNode::SharedPtr node_{nullptr}; -}; - -//! Abbrev. for shared instances of PlannerInterface or it's subclasses -typedef std::shared_ptr PlannerInterfacePtr; - - -} // namespace teb_local_planner - -#endif /* PLANNER_INTERFACE_H__ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/pose_se2.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/pose_se2.h deleted file mode 100755 index 55888c7..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/pose_se2.h +++ /dev/null @@ -1,433 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef POSE_SE2_H_ -#define POSE_SE2_H_ - -#include - -#include -#include "teb_local_planner/misc.h" -#include -#include - -#include -#include -#include - -namespace teb_local_planner -{ - -/** - * @class PoseSE2 - * @brief This class implements a pose in the domain SE2: \f$ \mathbb{R}^2 \times S^1 \f$ - * The pose consist of the position x and y and an orientation given as angle theta [-pi, pi]. - */ -class PoseSE2 -{ -public: - - /** @name Construct PoseSE2 instances */ - ///@{ - - /** - * @brief Default constructor - */ - PoseSE2() - { - setZero(); - } - - /** - * @brief Construct pose given a position vector and an angle theta - * @param position 2D position vector - * @param theta angle given in rad - */ - PoseSE2(const Eigen::Ref& position, double theta) - { - _position = position; - _theta = theta; - } - - /** - * @brief Construct pose using single components x, y, and the yaw angle - * @param x x-coordinate - * @param y y-coordinate - * @param theta yaw angle in rad - */ - PoseSE2(double x, double y, double theta) - { - _position.coeffRef(0) = x; - _position.coeffRef(1) = y; - _theta = theta; - } - - /** - * @brief Construct pose using a geometry_msgs::msg::Pose - * @param pose geometry_msgs::msg::Pose object - */ - PoseSE2(const geometry_msgs::msg::PoseStamped& pose) - :PoseSE2(pose.pose) - { - - } - - /** - * @brief Construct pose using a geometry_msgs::msg::Pose - * @param pose geometry_msgs::msg::Pose object - */ - PoseSE2(const geometry_msgs::msg::Pose& pose) - { - _position.coeffRef(0) = pose.position.x; - _position.coeffRef(1) = pose.position.y; - _theta = tf2::getYaw( pose.orientation ); - } - - /** - * @brief Construct pose using a geometry_msgs::msg::Pose2D - * @param pose geometry_msgs::msg::Pose2D object - */ - PoseSE2(const geometry_msgs::msg::Pose2D& pose) - { - _position.coeffRef(0) = pose.x; - _position.coeffRef(1) = pose.y; - _theta = pose.theta; - } - - /** - * @brief Copy constructor - * @param pose PoseSE2 instance - */ - PoseSE2(const PoseSE2& pose) - { - _position = pose._position; - _theta = pose._theta; - } - - ///@} - - - /** - * @brief Destructs the PoseSE2 - */ - ~PoseSE2() {} - - - /** @name Access and modify values */ - ///@{ - - /** - * @brief Access the 2D position part - * @see estimate - * @return reference to the 2D position part - */ - Eigen::Vector2d& position() {return _position;} - - /** - * @brief Access the 2D position part (read-only) - * @see estimate - * @return const reference to the 2D position part - */ - const Eigen::Vector2d& position() const {return _position;} - - /** - * @brief Access the x-coordinate the pose - * @return reference to the x-coordinate - */ - double& x() {return _position.coeffRef(0);} - - /** - * @brief Access the x-coordinate the pose (read-only) - * @return const reference to the x-coordinate - */ - const double& x() const {return _position.coeffRef(0);} - - /** - * @brief Access the y-coordinate the pose - * @return reference to the y-coordinate - */ - double& y() {return _position.coeffRef(1);} - - /** - * @brief Access the y-coordinate the pose (read-only) - * @return const reference to the y-coordinate - */ - const double& y() const {return _position.coeffRef(1);} - - /** - * @brief Access the orientation part (yaw angle) of the pose - * @return reference to the yaw angle - */ - double& theta() {return _theta;} - - /** - * @brief Access the orientation part (yaw angle) of the pose (read-only) - * @return const reference to the yaw angle - */ - const double& theta() const {return _theta;} - - /** - * @brief Set pose to [0,0,0] - */ - void setZero() - { - _position.setZero(); - _theta = 0; - } - - /** - * @brief Convert PoseSE2 to a geometry_msgs::msg::Pose - * @param[out] pose Pose message - */ - void toPoseMsg(geometry_msgs::msg::Pose& pose) const - { - pose.position.x = _position.x(); - pose.position.y = _position.y(); - pose.position.z = 0; - tf2::Quaternion q; - q.setRPY(0, 0, _theta); - pose.orientation = tf2::toMsg(q); - } - - /** - * @brief Convert PoseSE2 to a geometry_msgs::msg::Pose2D - * @param[out] pose Pose message - */ - void toPoseMsg(geometry_msgs::msg::Pose2D& pose) const - { - pose.x = _position.x(); - pose.y = _position.y(); - pose.theta = _theta; - } - - /** - * @brief Return the unit vector of the current orientation - * @returns [cos(theta), sin(theta))]^T - */ - Eigen::Vector2d orientationUnitVec() const {return Eigen::Vector2d(std::cos(_theta), std::sin(_theta));} - - ///@} - - - /** @name Arithmetic operations for which operators are not always reasonable */ - ///@{ - - /** - * @brief Scale all SE2 components (x,y,theta) and normalize theta afterwards to [-pi, pi] - * @param factor scale factor - */ - void scale(double factor) - { - _position *= factor; - _theta = g2o::normalize_theta( _theta*factor ); - } - - /** - * @brief Increment the pose by adding a double[3] array - * The angle is normalized afterwards - * @param pose_as_array 3D double array [x, y, theta] - */ - void plus(const double* pose_as_array) - { - _position.coeffRef(0) += pose_as_array[0]; - _position.coeffRef(1) += pose_as_array[1]; - _theta = g2o::normalize_theta( _theta + pose_as_array[2] ); - } - - /** - * @brief Get the mean / average of two poses and store it in the caller class - * For the position part: 0.5*(x1+x2) - * For the angle: take the angle of the mean direction vector - * @param pose1 first pose to consider - * @param pose2 second pose to consider - */ - void averageInPlace(const PoseSE2& pose1, const PoseSE2& pose2) - { - _position = (pose1._position + pose2._position)/2; - _theta = g2o::average_angle(pose1._theta, pose2._theta); - } - - /** - * @brief Get the mean / average of two poses and return the result (static) - * For the position part: 0.5*(x1+x2) - * For the angle: take the angle of the mean direction vector - * @param pose1 first pose to consider - * @param pose2 second pose to consider - * @return mean / average of \c pose1 and \c pose2 - */ - static PoseSE2 average(const PoseSE2& pose1, const PoseSE2& pose2) - { - return PoseSE2( (pose1._position + pose2._position)/2 , g2o::average_angle(pose1._theta, pose2._theta) ); - } - - /** - * @brief Rotate pose globally - * - * Compute [pose_x, pose_y] = Rot(\c angle) * [pose_x, pose_y]. - * if \c adjust_theta, pose_theta is also rotated by \c angle - * @param angle the angle defining the 2d rotation - * @param adjust_theta if \c true, the orientation theta is also rotated - */ - void rotateGlobal(double angle, bool adjust_theta=true) - { - double new_x = std::cos(angle)*_position.x() - std::sin(angle)*_position.y(); - double new_y = std::sin(angle)*_position.x() + std::cos(angle)*_position.y(); - _position.x() = new_x; - _position.y() = new_y; - if (adjust_theta) - _theta = g2o::normalize_theta(_theta+angle); - } - - ///@} - - - /** @name Operator overloads / Allow some arithmetic operations */ - ///@{ - - /** - * @brief Asignment operator - * @param rhs PoseSE2 instance - * @todo exception safe version of the assignment operator - */ - PoseSE2& operator=( const PoseSE2& rhs ) - { - if (&rhs != this) - { - _position = rhs._position; - _theta = rhs._theta; - } - return *this; - } - - /** - * @brief Compound assignment operator (addition) - * @param rhs addend - */ - PoseSE2& operator+=(const PoseSE2& rhs) - { - _position += rhs._position; - _theta = g2o::normalize_theta(_theta + rhs._theta); - return *this; - } - - /** - * @brief Arithmetic operator overload for additions - * @param lhs First addend - * @param rhs Second addend - */ - friend PoseSE2 operator+(PoseSE2 lhs, const PoseSE2& rhs) - { - return lhs += rhs; - } - - /** - * @brief Compound assignment operator (subtraction) - * @param rhs value to subtract - */ - PoseSE2& operator-=(const PoseSE2& rhs) - { - _position -= rhs._position; - _theta = g2o::normalize_theta(_theta - rhs._theta); - return *this; - } - - /** - * @brief Arithmetic operator overload for subtractions - * @param lhs First term - * @param rhs Second term - */ - friend PoseSE2 operator-(PoseSE2 lhs, const PoseSE2& rhs) - { - return lhs -= rhs; - } - - /** - * @brief Multiply pose with scalar and return copy without normalizing theta - * This operator is useful for calculating velocities ... - * @param pose pose to scale - * @param scalar factor to multiply with - * @warning theta is not normalized after multiplying - */ - friend PoseSE2 operator*(PoseSE2 pose, double scalar) - { - pose._position *= scalar; - pose._theta *= scalar; - return pose; - } - - /** - * @brief Multiply pose with scalar and return copy without normalizing theta - * This operator is useful for calculating velocities ... - * @param scalar factor to multiply with - * @param pose pose to scale - * @warning theta is not normalized after multiplying - */ - friend PoseSE2 operator*(double scalar, PoseSE2 pose) - { - pose._position *= scalar; - pose._theta *= scalar; - return pose; - } - - /** - * @brief Output stream operator - * @param stream output stream - * @param pose to be used - */ - friend std::ostream& operator<< (std::ostream& stream, const PoseSE2& pose) - { - stream << "x: " << pose._position[0] << " y: " << pose._position[1] << " theta: " << pose._theta; - return stream; - } - - ///@} - - -private: - - Eigen::Vector2d _position; - double _theta; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -} // namespace teb_local_planner - -#endif // POSE_SE2_H_ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/recovery_behaviors.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/recovery_behaviors.h deleted file mode 100644 index 2e7607d..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/recovery_behaviors.h +++ /dev/null @@ -1,134 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef RECOVERY_BEHAVIORS_H__ -#define RECOVERY_BEHAVIORS_H__ - - -#include -#include -#include - -namespace teb_local_planner -{ - - -/** - * @class FailureDetector - * @brief This class implements methods in order to detect if the robot got stucked or is oscillating - * - * The StuckDetector analyzes the last N commanded velocities in order to detect whether the robot - * might got stucked or oscillates between left/right/forward/backwards motions. - */ -class FailureDetector -{ -public: - - /** - * @brief Default constructor - */ - FailureDetector() {} - - /** - * @brief destructor. - */ - ~FailureDetector() {} - - /** - * @brief Set buffer length (measurement history) - * @param length number of measurements to be kept - */ - void setBufferLength(int length) {buffer_.set_capacity(length);} - - /** - * @brief Add a new twist measurement to the internal buffer and compute a new decision - * @param twist geometry_msgs::msg::Twist velocity information - * @param v_max maximum forward translational velocity - * @param v_backwards_max maximum backward translational velocity - * @param omega_max maximum angular velocity - * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1) - * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1) - */ - void update(const geometry_msgs::msg::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps); - - /** - * @brief Check if the robot got stucked - * - * This call does not compute the actual decision, - * since it is computed within each update() invocation. - * @return true if the robot got stucked, false otherwise. - */ - bool isOscillating() const; - - /** - * @brief Clear the current internal state - * - * This call also resets the internal buffer - */ - void clear(); - -protected: - - /** Variables to be monitored */ - struct VelMeasurement - { - double v = 0; - double omega = 0; - }; - - /** - * @brief Detect if the robot got stucked based on the current buffer content - * - * Afterwards the status might be checked using gotStucked(); - * @param v_eps Threshold for the average normalized linear velocity in (0,1) that must not be exceded (e.g. 0.1) - * @param omega_eps Threshold for the average normalized angular velocity in (0,1) that must not be exceded (e.g. 0.1) - * @return true if the robot got stucked, false otherwise - */ - bool detect(double v_eps, double omega_eps); - -private: - - boost::circular_buffer buffer_; //!< Circular buffer to store the last measurements @see setBufferLength - bool oscillating_ = false; //!< Current state: true if robot is oscillating - -}; - - -} // namespace teb_local_planner - -#endif /* RECOVERY_BEHAVIORS_H__ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/robot_footprint_model.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/robot_footprint_model.h deleted file mode 100644 index 65200f2..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/robot_footprint_model.h +++ /dev/null @@ -1,684 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - - -#ifndef ROBOT_FOOTPRINT_MODEL_H -#define ROBOT_FOOTPRINT_MODEL_H - -#include "teb_local_planner/pose_se2.h" -#include "teb_local_planner/obstacles.h" -#include - -namespace teb_local_planner -{ - -/** - * @class BaseRobotFootprintModel - * @brief Abstract class that defines the interface for robot footprint/contour models - * - * The robot model class is currently used in optimization only, since - * taking the navigation stack footprint into account might be - * inefficient. The footprint is only used for checking feasibility. - */ -class BaseRobotFootprintModel -{ -public: - - /** - * @brief Default constructor of the abstract obstacle class - */ - BaseRobotFootprintModel() - { - } - - /** - * @brief Virtual destructor. - */ - virtual ~BaseRobotFootprintModel() - { - } - - - /** - * @brief Calculate the distance between the robot and an obstacle - * @param current_pose Current robot pose - * @param obstacle Pointer to the obstacle - * @return Euclidean distance to the robot - */ - virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const = 0; - - /** - * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t - * @param current_pose robot pose, from which the distance to the obstacle is estimated - * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed) - * @param t time, for which the predicted distance to the obstacle is calculated - * @return Euclidean distance to the robot - */ - virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const = 0; - - /** - * @brief Visualize the robot using a markers - * - * Fill a marker message with all necessary information (type, pose, scale and color). - * The header, namespace, id and marker lifetime will be overwritten. - * @param current_pose Current robot pose - * @param[out] markers container of marker messages describing the robot shape - * @param color Color of the footprint - */ - virtual void visualizeRobot(const PoseSE2& current_pose, std::vector& markers, const std_msgs::msg::ColorRGBA& color) const {} - - - /** - * @brief Compute the inscribed radius of the footprint model - * @return inscribed radius - */ - virtual double getInscribedRadius() = 0; - - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - - -//! Abbrev. for shared obstacle pointers -typedef std::shared_ptr RobotFootprintModelPtr; -//! Abbrev. for shared obstacle const pointers -typedef std::shared_ptr RobotFootprintModelConstPtr; - - - -/** - * @class PointRobotShape - * @brief Class that defines a point-robot - * - * Instead of using a CircularRobotFootprint this class might - * be utitilzed and the robot radius can be added to the mininum distance - * parameter. This avoids a subtraction of zero each time a distance is calculated. - */ -class PointRobotFootprint : public BaseRobotFootprintModel -{ -public: - - /** - * @brief Default constructor of the abstract obstacle class - */ - PointRobotFootprint() {} - - /** - * @brief Virtual destructor. - */ - virtual ~PointRobotFootprint() {} - - /** - * @brief Calculate the distance between the robot and an obstacle - * @param current_pose Current robot pose - * @param obstacle Pointer to the obstacle - * @return Euclidean distance to the robot - */ - virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const - { - return obstacle->getMinimumDistance(current_pose.position()); - } - - /** - * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t - * @param current_pose robot pose, from which the distance to the obstacle is estimated - * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed) - * @param t time, for which the predicted distance to the obstacle is calculated - * @return Euclidean distance to the robot - */ - virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const - { - return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t); - } - - /** - * @brief Compute the inscribed radius of the footprint model - * @return inscribed radius - */ - virtual double getInscribedRadius() {return 0.0;} - -}; - - -/** - * @class CircularRobotFootprint - * @brief Class that defines the a robot of circular shape - */ -class CircularRobotFootprint : public BaseRobotFootprintModel -{ -public: - - /** - * @brief Default constructor of the abstract obstacle class - * @param radius radius of the robot - */ - CircularRobotFootprint(double radius) : radius_(radius) { } - - /** - * @brief Virtual destructor. - */ - virtual ~CircularRobotFootprint() { } - - /** - * @brief Set radius of the circular robot - * @param radius radius of the robot - */ - void setRadius(double radius) {radius_ = radius;} - - /** - * @brief Calculate the distance between the robot and an obstacle - * @param current_pose Current robot pose - * @param obstacle Pointer to the obstacle - * @return Euclidean distance to the robot - */ - virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const - { - return obstacle->getMinimumDistance(current_pose.position()) - radius_; - } - - /** - * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t - * @param current_pose robot pose, from which the distance to the obstacle is estimated - * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed) - * @param t time, for which the predicted distance to the obstacle is calculated - * @return Euclidean distance to the robot - */ - virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const - { - return obstacle->getMinimumSpatioTemporalDistance(current_pose.position(), t) - radius_; - } - - /** - * @brief Visualize the robot using a markers - * - * Fill a marker message with all necessary information (type, pose, scale and color). - * The header, namespace, id and marker lifetime will be overwritten. - * @param current_pose Current robot pose - * @param[out] markers container of marker messages describing the robot shape - * @param color Color of the footprint - */ - virtual void visualizeRobot(const PoseSE2& current_pose, std::vector& markers, const std_msgs::msg::ColorRGBA& color) const - { - markers.resize(1); - visualization_msgs::msg::Marker& marker = markers.back(); - marker.type = visualization_msgs::msg::Marker::CYLINDER; - current_pose.toPoseMsg(marker.pose); - marker.scale.x = marker.scale.y = 2*radius_; // scale = diameter - marker.scale.z = 0.05; - marker.color = color; - } - - /** - * @brief Compute the inscribed radius of the footprint model - * @return inscribed radius - */ - virtual double getInscribedRadius() {return radius_;} - -private: - - double radius_; -}; - - -/** - * @class TwoCirclesRobotFootprint - * @brief Class that approximates the robot with two shifted circles - */ -class TwoCirclesRobotFootprint : public BaseRobotFootprintModel -{ -public: - - /** - * @brief Default constructor of the abstract obstacle class - * @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters) - * @param front_radius radius of the front circle - * @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters) - * @param rear_radius radius of the front circle - */ - TwoCirclesRobotFootprint(double front_offset, double front_radius, double rear_offset, double rear_radius) - : front_offset_(front_offset), front_radius_(front_radius), rear_offset_(rear_offset), rear_radius_(rear_radius) { } - - /** - * @brief Virtual destructor. - */ - virtual ~TwoCirclesRobotFootprint() { } - - /** - * @brief Set parameters of the contour/footprint - * @param front_offset shift the center of the front circle along the robot orientation starting from the center at the rear axis (in meters) - * @param front_radius radius of the front circle - * @param rear_offset shift the center of the rear circle along the opposite robot orientation starting from the center at the rear axis (in meters) - * @param rear_radius radius of the front circle - */ - void setParameters(double front_offset, double front_radius, double rear_offset, double rear_radius) - {front_offset_=front_offset; front_radius_=front_radius; rear_offset_=rear_offset; rear_radius_=rear_radius;} - - /** - * @brief Calculate the distance between the robot and an obstacle - * @param current_pose Current robot pose - * @param obstacle Pointer to the obstacle - * @return Euclidean distance to the robot - */ - virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const - { - Eigen::Vector2d dir = current_pose.orientationUnitVec(); - double dist_front = obstacle->getMinimumDistance(current_pose.position() + front_offset_*dir) - front_radius_; - double dist_rear = obstacle->getMinimumDistance(current_pose.position() - rear_offset_*dir) - rear_radius_; - return std::min(dist_front, dist_rear); - } - - /** - * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t - * @param current_pose robot pose, from which the distance to the obstacle is estimated - * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed) - * @param t time, for which the predicted distance to the obstacle is calculated - * @return Euclidean distance to the robot - */ - virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const - { - Eigen::Vector2d dir = current_pose.orientationUnitVec(); - double dist_front = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() + front_offset_*dir, t) - front_radius_; - double dist_rear = obstacle->getMinimumSpatioTemporalDistance(current_pose.position() - rear_offset_*dir, t) - rear_radius_; - return std::min(dist_front, dist_rear); - } - - /** - * @brief Visualize the robot using a markers - * - * Fill a marker message with all necessary information (type, pose, scale and color). - * The header, namespace, id and marker lifetime will be overwritten. - * @param current_pose Current robot pose - * @param[out] markers container of marker messages describing the robot shape - * @param color Color of the footprint - */ - virtual void visualizeRobot(const PoseSE2& current_pose, std::vector& markers, const std_msgs::msg::ColorRGBA& color) const - { - Eigen::Vector2d dir = current_pose.orientationUnitVec(); - if (front_radius_>0) - { - markers.push_back(visualization_msgs::msg::Marker()); - visualization_msgs::msg::Marker& marker1 = markers.front(); - marker1.type = visualization_msgs::msg::Marker::CYLINDER; - current_pose.toPoseMsg(marker1.pose); - marker1.pose.position.x += front_offset_*dir.x(); - marker1.pose.position.y += front_offset_*dir.y(); - marker1.scale.x = marker1.scale.y = 2*front_radius_; // scale = diameter -// marker1.scale.z = 0.05; - marker1.color = color; - - } - if (rear_radius_>0) - { - markers.push_back(visualization_msgs::msg::Marker()); - visualization_msgs::msg::Marker& marker2 = markers.back(); - marker2.type = visualization_msgs::msg::Marker::CYLINDER; - current_pose.toPoseMsg(marker2.pose); - marker2.pose.position.x -= rear_offset_*dir.x(); - marker2.pose.position.y -= rear_offset_*dir.y(); - marker2.scale.x = marker2.scale.y = 2*rear_radius_; // scale = diameter -// marker2.scale.z = 0.05; - marker2.color = color; - } - } - - /** - * @brief Compute the inscribed radius of the footprint model - * @return inscribed radius - */ - virtual double getInscribedRadius() - { - double min_longitudinal = std::min(rear_offset_ + rear_radius_, front_offset_ + front_radius_); - double min_lateral = std::min(rear_radius_, front_radius_); - return std::min(min_longitudinal, min_lateral); - } - -private: - - double front_offset_; - double front_radius_; - double rear_offset_; - double rear_radius_; - -}; - - - -/** - * @class LineRobotFootprint - * @brief Class that approximates the robot with line segment (zero-width) - */ -class LineRobotFootprint : public BaseRobotFootprintModel -{ -public: - - /** - * @brief Default constructor of the abstract obstacle class - * @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0)) - * @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0)) - */ - LineRobotFootprint(const geometry_msgs::msg::Point& line_start, const geometry_msgs::msg::Point& line_end) - { - setLine(line_start, line_end); - } - - /** - * @brief Default constructor of the abstract obstacle class (Eigen Version) - * @param line_start start coordinates (only x and y) of the line (w.r.t. robot center at (0,0)) - * @param line_end end coordinates (only x and y) of the line (w.r.t. robot center at (0,0)) - */ - LineRobotFootprint(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) - { - setLine(line_start, line_end); - } - - /** - * @brief Virtual destructor. - */ - virtual ~LineRobotFootprint() { } - - /** - * @brief Set vertices of the contour/footprint - * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end) - */ - void setLine(const geometry_msgs::msg::Point& line_start, const geometry_msgs::msg::Point& line_end) - { - line_start_.x() = line_start.x; - line_start_.y() = line_start.y; - line_end_.x() = line_end.x; - line_end_.y() = line_end.y; - } - - /** - * @brief Set vertices of the contour/footprint (Eigen version) - * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end) - */ - void setLine(const Eigen::Vector2d& line_start, const Eigen::Vector2d& line_end) - { - line_start_ = line_start; - line_end_ = line_end; - } - - /** - * @brief Calculate the distance between the robot and an obstacle - * @param current_pose Current robot pose - * @param obstacle Pointer to the obstacle - * @return Euclidean distance to the robot - */ - virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const - { - Eigen::Vector2d line_start_world; - Eigen::Vector2d line_end_world; - transformToWorld(current_pose, line_start_world, line_end_world); - return obstacle->getMinimumDistance(line_start_world, line_end_world); - } - - /** - * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t - * @param current_pose robot pose, from which the distance to the obstacle is estimated - * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed) - * @param t time, for which the predicted distance to the obstacle is calculated - * @return Euclidean distance to the robot - */ - virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const - { - Eigen::Vector2d line_start_world; - Eigen::Vector2d line_end_world; - transformToWorld(current_pose, line_start_world, line_end_world); - return obstacle->getMinimumSpatioTemporalDistance(line_start_world, line_end_world, t); - } - - /** - * @brief Visualize the robot using a markers - * - * Fill a marker message with all necessary information (type, pose, scale and color). - * The header, namespace, id and marker lifetime will be overwritten. - * @param current_pose Current robot pose - * @param[out] markers container of marker messages describing the robot shape - * @param color Color of the footprint - */ - virtual void visualizeRobot(const PoseSE2& current_pose, std::vector& markers, const std_msgs::msg::ColorRGBA& color) const - { - markers.push_back(visualization_msgs::msg::Marker()); - visualization_msgs::msg::Marker& marker = markers.front(); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame! - - // line - geometry_msgs::msg::Point line_start_world; - line_start_world.x = line_start_.x(); - line_start_world.y = line_start_.y(); - line_start_world.z = 0; - marker.points.push_back(line_start_world); - - geometry_msgs::msg::Point line_end_world; - line_end_world.x = line_end_.x(); - line_end_world.y = line_end_.y(); - line_end_world.z = 0; - marker.points.push_back(line_end_world); - - marker.scale.x = 0.05; - marker.color = color; - } - - /** - * @brief Compute the inscribed radius of the footprint model - * @return inscribed radius - */ - virtual double getInscribedRadius() - { - return 0.0; // lateral distance = 0.0 - } - -private: - - /** - * @brief Transforms a line to the world frame manually - * @param current_pose Current robot pose - * @param[out] line_start line_start_ in the world frame - * @param[out] line_end line_end_ in the world frame - */ - void transformToWorld(const PoseSE2& current_pose, Eigen::Vector2d& line_start_world, Eigen::Vector2d& line_end_world) const - { - double cos_th = std::cos(current_pose.theta()); - double sin_th = std::sin(current_pose.theta()); - line_start_world.x() = current_pose.x() + cos_th * line_start_.x() - sin_th * line_start_.y(); - line_start_world.y() = current_pose.y() + sin_th * line_start_.x() + cos_th * line_start_.y(); - line_end_world.x() = current_pose.x() + cos_th * line_end_.x() - sin_th * line_end_.y(); - line_end_world.y() = current_pose.y() + sin_th * line_end_.x() + cos_th * line_end_.y(); - } - - Eigen::Vector2d line_start_; - Eigen::Vector2d line_end_; - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - -}; - - - -/** - * @class PolygonRobotFootprint - * @brief Class that approximates the robot with a closed polygon - */ -class PolygonRobotFootprint : public BaseRobotFootprintModel -{ -public: - - - - /** - * @brief Default constructor of the abstract obstacle class - * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end) - */ - PolygonRobotFootprint(const Point2dContainer& vertices) : vertices_(vertices) { } - - /** - * @brief Virtual destructor. - */ - virtual ~PolygonRobotFootprint() { } - - /** - * @brief Set vertices of the contour/footprint - * @param vertices footprint vertices (only x and y) around the robot center (0,0) (do not repeat the first and last vertex at the end) - */ - void setVertices(const Point2dContainer& vertices) {vertices_ = vertices;} - - /** - * @brief Calculate the distance between the robot and an obstacle - * @param current_pose Current robot pose - * @param obstacle Pointer to the obstacle - * @return Euclidean distance to the robot - */ - virtual double calculateDistance(const PoseSE2& current_pose, const Obstacle* obstacle) const - { - Point2dContainer polygon_world(vertices_.size()); - transformToWorld(current_pose, polygon_world); - return obstacle->getMinimumDistance(polygon_world); - } - - /** - * @brief Estimate the distance between the robot and the predicted location of an obstacle at time t - * @param current_pose robot pose, from which the distance to the obstacle is estimated - * @param obstacle Pointer to the dynamic obstacle (constant velocity model is assumed) - * @param t time, for which the predicted distance to the obstacle is calculated - * @return Euclidean distance to the robot - */ - virtual double estimateSpatioTemporalDistance(const PoseSE2& current_pose, const Obstacle* obstacle, double t) const - { - Point2dContainer polygon_world(vertices_.size()); - transformToWorld(current_pose, polygon_world); - return obstacle->getMinimumSpatioTemporalDistance(polygon_world, t); - } - - /** - * @brief Visualize the robot using a markers - * - * Fill a marker message with all necessary information (type, pose, scale and color). - * The header, namespace, id and marker lifetime will be overwritten. - * @param current_pose Current robot pose - * @param[out] markers container of marker messages describing the robot shape - * @param color Color of the footprint - */ - virtual void visualizeRobot(const PoseSE2& current_pose, std::vector& markers, const std_msgs::msg::ColorRGBA& color) const - { - if (vertices_.empty()) - return; - - markers.push_back(visualization_msgs::msg::Marker()); - visualization_msgs::msg::Marker& marker = markers.front(); - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - current_pose.toPoseMsg(marker.pose); // all points are transformed into the robot frame! - - for (std::size_t i = 0; i < vertices_.size(); ++i) - { - geometry_msgs::msg::Point point; - point.x = vertices_[i].x(); - point.y = vertices_[i].y(); - point.z = 0; - marker.points.push_back(point); - } - // add first point again in order to close the polygon - geometry_msgs::msg::Point point; - point.x = vertices_.front().x(); - point.y = vertices_.front().y(); - point.z = 0; - marker.points.push_back(point); - - marker.scale.x = 0.025; - marker.color = color; - - } - - /** - * @brief Compute the inscribed radius of the footprint model - * @return inscribed radius - */ - virtual double getInscribedRadius() - { - double min_dist = std::numeric_limits::max(); - Eigen::Vector2d center(0.0, 0.0); - - if (vertices_.size() <= 2) - return 0.0; - - for (int i = 0; i < (int)vertices_.size() - 1; ++i) - { - // compute distance from the robot center point to the first vertex - double vertex_dist = vertices_[i].norm(); - double edge_dist = distance_point_to_segment_2d(center, vertices_[i], vertices_[i+1]); - min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist)); - } - - // we also need to check the last vertex and the first vertex - double vertex_dist = vertices_.back().norm(); - double edge_dist = distance_point_to_segment_2d(center, vertices_.back(), vertices_.front()); - return std::min(min_dist, std::min(vertex_dist, edge_dist)); - } - -private: - - /** - * @brief Transforms a polygon to the world frame manually - * @param current_pose Current robot pose - * @param[out] polygon_world polygon in the world frame - */ - void transformToWorld(const PoseSE2& current_pose, Point2dContainer& polygon_world) const - { - double cos_th = std::cos(current_pose.theta()); - double sin_th = std::sin(current_pose.theta()); - for (std::size_t i=0; i -#include -#include -#include -#include -#include -#include "teb_local_planner/robot_footprint_model.h" -#include - -// Definitions -#define USE_ANALYTIC_JACOBI // if available for a specific edge, use analytic jacobi - -namespace teb_local_planner -{ -/** - * @class TebConfig - * @brief Config class for the teb_local_planner and its components. - */ -class TebConfig -{ -public: - using UniquePtr = std::unique_ptr; - - std::string odom_topic; //!< Topic name of the odometry message, provided by the robot driver or simulator - std::string map_frame; //!< Global planning frame - std::string node_name; //!< node name used for parameter event callback - - RobotFootprintModelPtr robot_model; - std::string model_name; - double radius; - std::vector line_start, line_end; - double front_offset, front_radius, rear_offset, rear_radius; - std::string footprint_string; - - //! Trajectory related parameters - struct Trajectory - { - double teb_autosize; //!< Enable automatic resizing of the trajectory w.r.t to the temporal resolution (recommended) - double dt_ref; //!< Desired temporal resolution of the trajectory (should be in the magniture of the underlying control rate) - double dt_hysteresis; //!< Hysteresis for automatic resizing depending on the current temporal resolution (dt): usually 10% of dt_ref - int min_samples; //!< Minimum number of samples (should be always greater than 2) - int max_samples; //!< Maximum number of samples; Warning: if too small the discretization/resolution might not be sufficient for the given robot model or obstacle avoidance does not work anymore. - bool global_plan_overwrite_orientation; //!< Overwrite orientation of local subgoals provided by the global planner - bool allow_init_with_backwards_motion; //!< If true, the underlying trajectories might be initialized with backwards motions in case the goal is behind the start within the local costmap (this is only recommended if the robot is equipped with rear sensors) - double global_plan_viapoint_sep; //!< Min. separation between each two consecutive via-points extracted from the global plan (if negative: disabled) - bool via_points_ordered; //!< If true, the planner adheres to the order of via-points in the storage container - double max_global_plan_lookahead_dist; //!< Specify maximum length (cumulative Euclidean distances) of the subset of the global plan taken into account for optimization [if <=0: disabled; the length is also bounded by the local costmap size!] - double global_plan_prune_distance; //!< Distance between robot and via_points of global plan which is used for pruning - bool exact_arc_length; //!< If true, the planner uses the exact arc length in velocity, acceleration and turning rate computations [-> increased cpu time], otherwise the euclidean approximation is used. - double force_reinit_new_goal_dist; //!< Reinitialize the trajectory if a previous goal is updated with a seperation of more than the specified value in meters (skip hot-starting) - double force_reinit_new_goal_angular; //!< Reinitialize the trajectory if a previous goal is updated with an angular difference of more than the specified value in radians (skip hot-starting) - int feasibility_check_no_poses; //!< Specify up to which pose (under the feasibility_check_lookahead_distance) on the predicted plan the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_lookahead_distance are checked. - double feasibility_check_lookahead_distance; //!< Specify up to which distance (and with an index below feasibility_check_no_poses) from the robot the feasibility should be checked each sampling interval; if -1, all poses up to feasibility_check_no_poses are checked. - bool publish_feedback; //!< Publish planner feedback containing the full trajectory and a list of active obstacles (should be enabled only for evaluation or debugging purposes) - double min_resolution_collision_check_angular; //! Min angular resolution used during the costmap collision check. If not respected, intermediate samples are added. [rad] - int control_look_ahead_poses; //! Index of the pose used to extract the velocity command - } trajectory; //!< Trajectory related parameters - - //! Robot related parameters - struct Robot - { - double base_max_vel_x; //!< Maximum translational velocity of the robot before speed limit is applied - double base_max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards before speed limit is applied - double base_max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) before speed limit is applied - double base_max_vel_theta; //!< Maximum angular velocity of the robot before speed limit is applied - double max_vel_x; //!< Maximum translational velocity of the robot - double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards - double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) - double max_vel_theta; //!< Maximum angular velocity of the robot - double acc_lim_x; //!< Maximum translational acceleration of the robot - double acc_lim_y; //!< Maximum strafing acceleration of the robot - double acc_lim_theta; //!< Maximum angular acceleration of the robot - double min_turning_radius; //!< Minimum turning radius of a carlike robot (diff-drive robot: zero); - double wheelbase; //!< The distance between the drive shaft and steering axle (only required for a carlike robot with 'cmd_angle_instead_rotvel' enabled); The value might be negative for back-wheeled robots! - bool cmd_angle_instead_rotvel; //!< Substitute the rotational velocity in the commanded velocity message by the corresponding steering angle (check 'axles_distance') - bool is_footprint_dynamic; // currently only activated if an oscillation is detected, see 'oscillation_recovery' - - double weight_adapt_factor; //!< Some special weights (currently 'weight_obstacle') are repeatedly scaled by this factor in each outer TEB iteration (weight_new = weight_old*factor); Increasing weights iteratively instead of setting a huge value a-priori leads to better numerical conditions of the underlying optimization problem. - double obstacle_cost_exponent; //!< Exponent for nonlinear obstacle cost (cost = linear_cost * obstacle_cost_exponent). Set to 1 to disable nonlinear cost (default) - } optim; //!< Optimization related parameters - - - struct HomotopyClasses - { - bool enable_homotopy_class_planning; //!< Activate homotopy class planning (Requires much more resources that simple planning, since multiple trajectories are optimized at once). - bool enable_multithreading; //!< Activate multiple threading for planning multiple trajectories in parallel. - bool simple_exploration; //!< If true, distinctive trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal. - int max_number_classes; //!< Specify the maximum number of allowed alternative homotopy classes (limits computational effort) - int max_number_plans_in_current_class; //!< Specify the maximum number of trajectories to try that are in the same homotopy class as the current trajectory (helps avoid local minima) - double selection_cost_hysteresis; //!< Specify how much trajectory cost must a new candidate have w.r.t. a previously selected trajectory in order to be selected (selection if new_cost < old_cost*factor). - double selection_prefer_initial_plan; //!< Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan. - double selection_obst_cost_scale; //!< Extra scaling of obstacle cost terms just for selecting the 'best' candidate. - double selection_viapoint_cost_scale; //!< Extra scaling of via-point cost terms just for selecting the 'best' candidate. - bool selection_alternative_time_cost; //!< If true, time cost is replaced by the total transition time. - double selection_dropping_probability; //!< At each planning cycle, TEBs other than the current 'best' one will be randomly dropped with this probability. Prevents becoming 'fixated' on sub-optimal alternative homotopies. - double switching_blocking_period; //!< Specify a time duration in seconds that needs to be expired before a switch to new equivalence class is allowed - - int roadmap_graph_no_samples; //! < Specify the number of samples generated for creating the roadmap graph, if simple_exploration is turend off. - double roadmap_graph_area_width; //!< Random keypoints/waypoints are sampled in a rectangular region between start and goal. Specify the width of that region in meters. - double roadmap_graph_area_length_scale; //!< The length of the rectangular region is determined by the distance between start and goal. This parameter further scales the distance such that the geometric center remains equal! - double h_signature_prescaler; //!< Scale number of obstacle value in order to allow huge number of obstacles. Do not choose it extremly low, otherwise obstacles cannot be distinguished from each other (0.2 this - } hcp; - - //! Recovery/backup related parameters - struct Recovery - { - bool shrink_horizon_backup; //!< Allows the planner to shrink the horizon temporary (50%) in case of automatically detected issues. - double shrink_horizon_min_duration; //!< Specify minimum duration for the reduced horizon in case an infeasible trajectory is detected. - bool oscillation_recovery; //!< Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards) - double oscillation_v_eps; //!< Threshold for the average normalized linear velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected - double oscillation_omega_eps; //!< Threshold for the average normalized angular velocity: if oscillation_v_eps and oscillation_omega_eps are not exceeded both, a possible oscillation is detected - double oscillation_recovery_min_duration; //!< Minumum duration [sec] for which the recovery mode is activated after an oscillation is detected. - double oscillation_filter_duration; //!< Filter length/duration [sec] for the detection of oscillations - bool divergence_detection_enable; //!< True to enable divergence detection. - int divergence_detection_max_chi_squared; //!< Maximum acceptable Mahalanobis distance above which it is assumed that the optimization diverged. - } recovery; //!< Parameters related to recovery and backup strategies - - - /** - * @brief Construct the TebConfig using default values. - * @warning If the \b rosparam server or/and \b dynamic_reconfigure (rqt_reconfigure) node are used, - * the default variables will be overwritten: \n - * E.g. if \e base_local_planner is utilized as plugin for the navigation stack, the initialize() method will register a - * dynamic_reconfigure server. A subset (not all but most) of the parameters are considered for dynamic modifications. - * All parameters considered by the dynamic_reconfigure server (and their \b default values) are - * set in \e PROJECT_SRC/cfg/TebLocalPlannerReconfigure.cfg. \n - * In addition the rosparam server can be queried to get parameters e.g. defiend in a launch file. - * The plugin source (or a possible binary source) can call loadRosParamFromNodeHandle() to update the parameters. - * In \e summary, default parameters are loaded in the following order (the right one overrides the left ones): \n - * TebConfig Constructor defaults << dynamic_reconfigure defaults << rosparam server defaults - */ - TebConfig() - { - - odom_topic = "odom"; - map_frame = "odom"; - - // Trajectory - - trajectory.teb_autosize = true; - trajectory.dt_ref = 0.3; - trajectory.dt_hysteresis = 0.1; - trajectory.min_samples = 3; - trajectory.max_samples = 500; - trajectory.global_plan_overwrite_orientation = true; - trajectory.allow_init_with_backwards_motion = false; - trajectory.global_plan_viapoint_sep = -1; - trajectory.via_points_ordered = false; - trajectory.max_global_plan_lookahead_dist = 1; - trajectory.global_plan_prune_distance = 1; - trajectory.exact_arc_length = false; - trajectory.force_reinit_new_goal_dist = 1; - trajectory.force_reinit_new_goal_angular = 0.5 * M_PI; - trajectory.feasibility_check_no_poses = 5; - trajectory.feasibility_check_lookahead_distance = -1; - trajectory.publish_feedback = false; - trajectory.min_resolution_collision_check_angular = M_PI; - trajectory.control_look_ahead_poses = 1; - - // Robot - - robot.max_vel_x = 0.4; - robot.max_vel_x_backwards = 0.2; - robot.max_vel_y = 0.0; - robot.max_vel_theta = 0.3; - robot.base_max_vel_x = robot.max_vel_x; - robot.base_max_vel_x_backwards = robot.base_max_vel_x_backwards; - robot.base_max_vel_y = robot.base_max_vel_y; - robot.base_max_vel_theta = robot.base_max_vel_theta; - robot.acc_lim_x = 0.5; - robot.acc_lim_y = 0.5; - robot.acc_lim_theta = 0.5; - robot.min_turning_radius = 0; - robot.wheelbase = 1.0; - robot.cmd_angle_instead_rotvel = false; - robot.is_footprint_dynamic = false; - robot.use_proportional_saturation = false; - - // GoalTolerance - - goal_tolerance.xy_goal_tolerance = 0.2; - goal_tolerance.free_goal_vel = false; - - // Obstacles - - obstacles.min_obstacle_dist = 0.5; - obstacles.inflation_dist = 0.6; - obstacles.dynamic_obstacle_inflation_dist = 0.6; - obstacles.include_dynamic_obstacles = true; - obstacles.include_costmap_obstacles = true; - obstacles.costmap_obstacles_behind_robot_dist = 1.5; - obstacles.obstacle_poses_affected = 25; - obstacles.legacy_obstacle_association = false; - obstacles.obstacle_association_force_inclusion_factor = 1.5; - obstacles.obstacle_association_cutoff_factor = 5; - obstacles.costmap_converter_plugin = ""; - obstacles.costmap_converter_spin_thread = true; - obstacles.costmap_converter_rate = 5; - obstacles.obstacle_proximity_ratio_max_vel = 1; - obstacles.obstacle_proximity_lower_bound = 0; - obstacles.obstacle_proximity_upper_bound = 0.5; - - // Optimization - - optim.no_inner_iterations = 5; - optim.no_outer_iterations = 4; - optim.optimization_activate = true; - optim.optimization_verbose = false; - optim.penalty_epsilon = 0.05; - optim.weight_max_vel_x = 2; //1 - optim.weight_max_vel_y = 2; - optim.weight_max_vel_theta = 1; - optim.weight_acc_lim_x = 1; - optim.weight_acc_lim_y = 1; - optim.weight_acc_lim_theta = 1; - optim.weight_kinematics_nh = 1000; - optim.weight_kinematics_forward_drive = 1; - optim.weight_kinematics_turning_radius = 1; - optim.weight_optimaltime = 1; - optim.weight_shortest_path = 0; - optim.weight_obstacle = 50; - optim.weight_inflation = 0.1; - optim.weight_dynamic_obstacle = 50; - optim.weight_dynamic_obstacle_inflation = 0.1; - optim.weight_velocity_obstacle_ratio = 0; - optim.weight_viapoint = 1; - optim.weight_prefer_rotdir = 50; - - optim.weight_adapt_factor = 2.0; - optim.obstacle_cost_exponent = 1.0; - - // Homotopy Class Planner - - hcp.enable_homotopy_class_planning = true; - hcp.enable_multithreading = true; - hcp.simple_exploration = false; - hcp.max_number_classes = 5; - hcp.selection_cost_hysteresis = 1.0; - hcp.selection_prefer_initial_plan = 0.95; - hcp.selection_obst_cost_scale = 100.0; - hcp.selection_viapoint_cost_scale = 1.0; - hcp.selection_alternative_time_cost = false; - hcp.selection_dropping_probability = 0.0; - - hcp.obstacle_keypoint_offset = 0.1; - hcp.obstacle_heading_threshold = 0.45; - hcp.roadmap_graph_no_samples = 15; - hcp.roadmap_graph_area_width = 6; // [m] - hcp.roadmap_graph_area_length_scale = 1.0; - hcp.h_signature_prescaler = 1; - hcp.h_signature_threshold = 0.1; - hcp.switching_blocking_period = 0.0; - - hcp.viapoints_all_candidates = true; - - hcp.visualize_hc_graph = false; - hcp.visualize_with_time_as_z_axis_scale = 0.0; - hcp.delete_detours_backwards = true; - hcp.detours_orientation_tolerance = M_PI / 2.0; - hcp.length_start_orientation_vector = 0.4; - hcp.max_ratio_detours_duration_best_duration = 3.0; - - // Recovery - - recovery.shrink_horizon_backup = true; - recovery.shrink_horizon_min_duration = 10; - recovery.oscillation_recovery = true; - recovery.oscillation_v_eps = 0.1; - recovery.oscillation_omega_eps = 0.1; - recovery.oscillation_recovery_min_duration = 10; - recovery.oscillation_filter_duration = 10; - recovery.divergence_detection_enable = false; - recovery.divergence_detection_max_chi_squared = 10; - } - - void declareParameters(const nav2_util::LifecycleNode::SharedPtr, const std::string name); - - /** - * @brief Load parmeters from the ros param server. - * @param nh const reference to the local rclcpp::Node::SharedPtr - */ - void loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name); - - /** - * @brief Callback executed when a paramter change is detected - * @param parameters list of changed parameters - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - - /** - * @brief Check parameters and print warnings in case of discrepancies - * - * Call this method whenever parameters are changed using public interfaces to inform the user - * about some improper uses. - */ - void checkParameters() const; - - /** - * @brief Check if some deprecated parameters are found and print warnings - * @param nh const reference to the local rclcpp::Node::SharedPtr - */ - void checkDeprecated(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) const; - - /** - * @brief Return the internal config mutex - */ - std::mutex& configMutex() {return config_mutex_;} - -private: - std::mutex config_mutex_; //!< Mutex for config accesses and changes - rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")}; -}; -} // namespace teb_local_planner - -#endif diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h deleted file mode 100644 index 5e67792..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h +++ /dev/null @@ -1,426 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef TEB_LOCAL_PLANNER_ROS_H_ -#define TEB_LOCAL_PLANNER_ROS_H_ - -#include - -#include - -// Navigation2 local planner base class and utilities -#include - -// timed-elastic-band related classes -#include "teb_local_planner/optimal_planner.h" -#include "teb_local_planner/homotopy_class_planner.h" -#include "teb_local_planner/visualization.h" -#include "teb_local_planner/recovery_behaviors.h" - -// message types -#include -#include -#include -#include -#include -#include - -// transforms -#include -#include - -// costmap -#include -#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" - -#include -#include -#include -#include "rcl_interfaces/msg/set_parameters_result.hpp" -// dynamic reconfigure -//#include "teb_local_planner/TebLocalPlannerReconfigureConfig.h> -//#include - - -namespace teb_local_planner -{ -using TFBufferPtr = std::shared_ptr; -using CostmapROSPtr = std::shared_ptr; - -/** - * @class TebLocalPlannerROS - * @brief Implements the actual abstract navigation stack routines of the teb_local_planner plugin - * @todo Escape behavior, more efficient obstacle handling - */ -class TebLocalPlannerROS : public nav2_core::Controller -{ - -public: - /** - * @brief Constructor of the teb plugin - */ - TebLocalPlannerROS(); - - /** - * @brief Destructor of the plugin - */ - ~TebLocalPlannerROS(); - - /** - * @brief Configure the teb plugin - * - * @param node The node of the instance - * @param tf Pointer to a transform listener - * @param costmap_ros Cost map representing occupied and free space - */ - void configure( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, - std::shared_ptr tf, - std::shared_ptr costmap_ros) override; - void activate() override; - void deactivate() override; - void cleanup() override; - - /** - * @brief Initializes the teb plugin - */ - void initialize(nav2_util::LifecycleNode::SharedPtr node); - - /** - * @brief Set the plan that the teb local planner is following - * @param orig_global_plan The plan to pass to the local planner - * @return - */ - void setPlan(const nav_msgs::msg::Path & orig_global_plan) override; - - /** - * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base - * @param pose is the current position - * @param velocity is the current velocity - * @return velocity commands to send to the base - */ - geometry_msgs::msg::TwistStamped computeVelocityCommands( - const geometry_msgs::msg::PoseStamped &pose, - const geometry_msgs::msg::Twist &velocity, - nav2_core::GoalChecker * goal_checker); - - - /** @name Public utility functions/methods */ - //@{ - - /** - * @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities. - * - * Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component). - * @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle) - * @return Translational and angular velocity combined into an Eigen::Vector2d - */ -// static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel); - - /** - * @brief Get the current robot footprint/contour model - * @param nh const reference to the local rclcpp::Node::SharedPtr - * @return Robot footprint model used for optimization - */ - RobotFootprintModelPtr getRobotFootprintFromParamServer(nav2_util::LifecycleNode::SharedPtr node); - - /** - * @brief Set the footprint from the given XmlRpcValue. - * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros - * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::msg::Point - * @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the - * sub-arrays should all have exactly 2 elements (x and y coordinates). - * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came. - * It is used only for reporting errors. - * @return container of vertices describing the polygon - */ -// Using ROS2 parameter server -// static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name); - - /** - * @brief Get a number from the given XmlRpcValue. - * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros - * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::msg::Point - * @param value double value type - * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came. - * It is used only for reporting errors. - * @returns double value - */ -// Using ROS2 parameter server -// static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name); - - //@} - -protected: - - /** - * @brief Update internal obstacle vector based on occupied costmap cells - * @remarks All occupied cells will be added as point obstacles. - * @remarks All previous obstacles are cleared. - * @sa updateObstacleContainerWithCostmapConverter - * @todo Include temporal coherence among obstacle msgs (id vector) - * @todo Include properties for dynamic obstacles (e.g. using constant velocity model) - */ - void updateObstacleContainerWithCostmap(); - - /** - * @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin - * @remarks Requires a loaded costmap_converter plugin. - * @remarks All previous obstacles are cleared. - * @sa updateObstacleContainerWithCostmap - */ - void updateObstacleContainerWithCostmapConverter(); - - /** - * @brief Update internal obstacle vector based on custom messages received via subscriber - * @remarks All previous obstacles are NOT cleared. Call this method after other update methods. - * @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter - */ - void updateObstacleContainerWithCustomObstacles(); - - - /** - * @brief Update internal via-point container based on the current reference plan - * @remarks All previous via-points will be cleared. - * @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame) - * @param min_separation minimum separation between two consecutive via-points - */ - void updateViaPointsContainer(const std::vector& transformed_plan, double min_separation); - - - /** - * @brief Callback for the dynamic_reconfigure node. - * - * This callback allows to modify parameters dynamically at runtime without restarting the node - * @param config Reference to the dynamic reconfigure config - * @param level Dynamic reconfigure level - */ - // TODO : dynamic reconfigure is not supported on ROS2 -// void reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level); - - - /** - * @brief Callback for custom obstacles that are not obtained from the costmap - * @param obst_msg pointer to the message containing a list of polygon shaped obstacles - */ - void customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg); - - /** - * @brief Callback for custom via-points - * @param via_points_msg pointer to the message containing a list of via-points - */ - void customViaPointsCB(const nav_msgs::msg::Path::ConstSharedPtr via_points_msg); - - /** - * @brief Prune global plan such that already passed poses are cut off - * - * The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform. - * If no valid transformation can be found, the method returns \c false. - * The global plan is pruned until the distance to the robot is at least \c dist_behind_robot. - * If no pose within the specified treshold \c dist_behind_robot can be found, - * nothing will be pruned and the method returns \c false. - * @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned. - * @param global_pose The global pose of the robot - * @param[in,out] global_plan The plan to be transformed - * @param dist_behind_robot Distance behind the robot that should be kept [meters] - * @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold - */ - bool pruneGlobalPlan(const geometry_msgs::msg::PoseStamped& global_pose, - std::vector& global_plan, double dist_behind_robot=1); - - /** - * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). - * - * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h - * such that the index of the current goal pose is returned as well as - * the transformation between the global plan and the planning frame. - * @param global_plan The plan to be transformed - * @param global_pose The global pose of the robot - * @param costmap A reference to the costmap being used so the window size for transforming can be computed - * @param global_frame The frame to transform the plan to - * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!] - * @param[out] transformed_plan Populated with the transformed plan - * @param[out] current_goal_idx Index of the current (local) goal pose in the global plan - * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame - * @return \c true if the global plan is transformed, \c false otherwise - */ - bool transformGlobalPlan(const std::vector& global_plan, - const geometry_msgs::msg::PoseStamped& global_pose, const nav2_costmap_2d::Costmap2D& costmap, - const std::string& global_frame, double max_plan_length, std::vector& transformed_plan, - int* current_goal_idx = NULL, geometry_msgs::msg::TransformStamped* tf_plan_to_global = NULL) const; - - /** - * @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner. - * - * If the current (local) goal point is not the final one (global) - * substitute the goal orientation by the angle of the direction vector between - * the local goal and the subsequent pose of the global plan. - * This is often helpful, if the global planner does not consider orientations. \n - * A moving average filter is utilized to smooth the orientation. - * @param global_plan The global plan - * @param local_goal Current local goal - * @param current_goal_idx Index of the current (local) goal pose in the global plan - * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame - * @param moving_average_length number of future poses of the global plan to be taken into account - * @return orientation (yaw-angle) estimate - */ - double estimateLocalGoalOrientation(const std::vector& global_plan, const geometry_msgs::msg::PoseStamped& local_goal, - int current_goal_idx, const geometry_msgs::msg::TransformStamped& tf_plan_to_global, int moving_average_length=3) const; - - - /** - * @brief Saturate the translational and angular velocity to given limits. - * - * The limit of the translational velocity for backwards driving can be changed independently. - * Do not choose max_vel_x_backwards <= 0. If no backward driving is desired, change the optimization weight for - * penalizing backwards driving instead. - * @param[in,out] vx The translational velocity that should be saturated. - * @param[in,out] vy Strafing velocity which can be nonzero for holonomic robots - * @param[in,out] omega The angular velocity that should be saturated. - * @param max_vel_x Maximum translational velocity for forward driving - * @param max_vel_y Maximum strafing velocity (for holonomic robots) - * @param max_vel_theta Maximum (absolute) angular velocity - * @param max_vel_x_backwards Maximum translational velocity for backwards driving - */ - void saturateVelocity(double& vx, double& vy, double& omega, double max_vel_x, double max_vel_y, - double max_vel_theta, double max_vel_x_backwards) const; - - - /** - * @brief Convert translational and rotational velocities to a steering angle of a carlike robot - * - * The conversion is based on the following equations: - * - The turning radius is defined by \f$ R = v/omega \f$ - * - For a car like robot withe a distance L between both axles, the relation is: \f$ tan(\phi) = L/R \f$ - * - phi denotes the steering angle. - * @remarks You might provide distances instead of velocities, since the temporal information is not required. - * @param v translational velocity [m/s] - * @param omega rotational velocity [rad/s] - * @param wheelbase distance between both axles (drive shaft and steering axle), the value might be negative for back_wheeled robots - * @param min_turning_radius Specify a lower bound on the turning radius - * @return Resulting steering angle in [rad] inbetween [-pi/2, pi/2] - */ - double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0) const; - - /** - * @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint - * - * This method prints warnings if validation fails. - * @remarks Currently, we only validate the inscribed radius of the footprints - * @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization - * @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap - * @param min_obst_dist desired distance to obstacles - */ - void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); - - - void configureBackupModes(std::vector& transformed_plan, int& goal_idx); - - /** - * @brief Limits the maximum linear speed of the robot. - * @param speed_limit expressed in absolute value (in m/s) - * or in percentage from maximum robot speed. - * @param percentage Setting speed limit in percentage if true - * or in absolute values in false case. - */ - void setSpeedLimit(const double & speed_limit, const bool & percentage); - -private: - // Definition of member variables - rclcpp_lifecycle::LifecycleNode::WeakPtr nh_; - rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")}; - rclcpp::Clock::SharedPtr clock_; - rclcpp::Node::SharedPtr intra_proc_node_; - // external objects (store weak pointers) - CostmapROSPtr costmap_ros_; //!< Pointer to the costmap ros wrapper, received from the navigation stack - nav2_costmap_2d::Costmap2D* costmap_; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper) - TFBufferPtr tf_; //!< pointer to Transform Listener - TebConfig::UniquePtr cfg_; //!< Config class that stores and manages all related parameters - - // internal objects (memory management owned) - PlannerInterfacePtr planner_; //!< Instance of the underlying optimal planner class - ObstContainer obstacles_; //!< Obstacle vector that should be considered during local trajectory optimization - ViaPointContainer via_points_; //!< Container of via-points that should be considered during local trajectory optimization - TebVisualizationPtr visualization_; //!< Instance of the visualization class (local/global plan, obstacles, ...) - std::shared_ptr costmap_model_; - FailureDetector failure_detector_; //!< Detect if the robot got stucked - - std::vector global_plan_; //!< Store the current global plan - - pluginlib::ClassLoader costmap_converter_loader_; //!< Load costmap converter plugins at runtime - std::shared_ptr costmap_converter_; //!< Store the current costmap_converter - - //std::shared_ptr< dynamic_reconfigure::Server > dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime - rclcpp::Subscription::SharedPtr custom_obst_sub_; //!< Subscriber for custom obstacles received via a ObstacleMsg. - std::mutex custom_obst_mutex_; //!< Mutex that locks the obstacle array (multi-threaded) - costmap_converter_msgs::msg::ObstacleArrayMsg custom_obstacle_msg_; //!< Copy of the most recent obstacle message - - rclcpp::Subscription::SharedPtr via_points_sub_; //!< Subscriber for custom via-points received via a Path msg. - bool custom_via_points_active_; //!< Keep track whether valid via-points have been received from via_points_sub_ - std::mutex via_point_mutex_; //!< Mutex that locks the via_points container (multi-threaded) - - PoseSE2 robot_pose_; //!< Store current robot pose - PoseSE2 robot_goal_; //!< Store current robot goal - geometry_msgs::msg::Twist robot_vel_; //!< Store current robot translational and angular velocity (vx, vy, omega) - rclcpp::Time time_last_infeasible_plan_; //!< Store at which time stamp the last infeasible plan was detected - int no_infeasible_plans_; //!< Store how many times in a row the planner failed to find a feasible plan. - rclcpp::Time time_last_oscillation_; //!< Store at which time stamp the last oscillation was detected - RotType last_preferred_rotdir_; //!< Store recent preferred turning direction - geometry_msgs::msg::Twist last_cmd_; //!< Store the last control command generated in computeVelocityCommands() - - std::vector footprint_spec_; //!< Store the footprint of the robot - double robot_inscribed_radius_; //!< The radius of the inscribed circle of the robot (collision possible) - double robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot - - // flags - bool initialized_; //!< Keeps track about the correct initialization of this class - std::string name_; //!< Name of plugin ID - -protected: - // Dynamic parameters handler - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler; -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -}; // end namespace teb_local_planner - -#endif // TEB_LOCAL_PLANNER_ROS_H_ - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.h deleted file mode 100644 index f720906..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.h +++ /dev/null @@ -1,636 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef TIMED_ELASTIC_BAND_H_ -#define TIMED_ELASTIC_BAND_H_ -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#include "teb_local_planner/obstacles.h" - -// G2O Types -#include "teb_local_planner/g2o_types/vertex_pose.h" -#include "teb_local_planner/g2o_types/vertex_timediff.h" - - -namespace teb_local_planner -{ - -//! Container of poses that represent the spatial part of the trajectory -typedef std::vector PoseSequence; -//! Container of time differences that define the temporal of the trajectory -typedef std::vector TimeDiffSequence; - - -/** - * @class TimedElasticBand - * @brief Class that defines a trajectory modeled as an elastic band with augmented tempoarl information. - * - * All trajectory related methods (initialization, modifying, ...) are implemented inside this class. \n - * Let \f$ Q = \lbrace \mathbf{s}_i \rbrace_{i=0...n},\ n \in \mathbb{N} \f$ be a sequence of poses, \n - * in which \f$ \mathbf{s}_i = [x_i, y_i, \beta_i]^T \in \mathbb{R}^2 \times S^1 \f$ denotes a single pose of the robot. \n - * The Timed Elastic Band (TEB) augments this sequence of poses by incorporating time intervals between - * two consecutive poses, resuting in a sequence of \c n-1 time intervals \f$ \Delta T_i \f$: \n - * \f$ \tau = \lbrace \Delta T_i \rbrace_{i=0...n-1} \f$. \n - * Each time interval (time diff) denotes the time that the robot requires to transit from the current configuration to the next configuration. - * The tuple of both sequences defines the underlying trajectory. - * - * Poses and time differences are wrapped into a g2o::Vertex class in order to enable the efficient optimization in TebOptimalPlanner. \n - * TebOptimalPlanner utilizes this Timed_Elastic_band class for representing an optimizable trajectory. - * - * @todo Move decision if the start or goal state should be marked as fixed or unfixed for the optimization to the TebOptimalPlanner class. - */ -class TimedElasticBand -{ -public: - - /** - * @brief Construct the class - */ - TimedElasticBand(); - - /** - * @brief Destruct the class - */ - virtual ~TimedElasticBand(); - - - - /** @name Access pose and timediff sequences */ - //@{ - - /** - * @brief Access the complete pose sequence - * @return reference to the pose sequence - */ - PoseSequence& poses() {return pose_vec_;}; - - /** - * @brief Access the complete pose sequence (read-only) - * @return const reference to the pose sequence - */ - const PoseSequence& poses() const {return pose_vec_;}; - - /** - * @brief Access the complete timediff sequence - * @return reference to the dimediff sequence - */ - TimeDiffSequence& timediffs() {return timediff_vec_;}; - - /** - * @brief Access the complete timediff sequence - * @return reference to the dimediff sequence - */ - const TimeDiffSequence& timediffs() const {return timediff_vec_;}; - - /** - * @brief Access the time difference at pos \c index of the time sequence - * @param index element position inside the internal TimeDiffSequence - * @return reference to the time difference at pos \c index - */ - double& TimeDiff(int index) - { - assert(indexdt(); - } - - /** - * @brief Access the time difference at pos \c index of the time sequence (read-only) - * @param index element position inside the internal TimeDiffSequence - * @return const reference to the time difference at pos \c index - */ - const double& TimeDiff(int index) const - { - assert(indexdt(); - } - - /** - * @brief Access the pose at pos \c index of the pose sequence - * @param index element position inside the internal PoseSequence - * @return reference to the pose at pos \c index - */ - PoseSE2& Pose(int index) - { - assert(indexpose(); - } - - /** - * @brief Access the pose at pos \c index of the pose sequence (read-only) - * @param index element position inside the internal PoseSequence - * @return const reference to the pose at pos \c index - */ - const PoseSE2& Pose(int index) const - { - assert(indexpose(); - } - - /** - * @brief Access the last PoseSE2 in the pose sequence - */ - PoseSE2& BackPose() {return pose_vec_.back()->pose(); } - - /** - * @brief Access the last PoseSE2 in the pose sequence (read-only) - */ - const PoseSE2& BackPose() const {return pose_vec_.back()->pose();} - - /** - * @brief Access the last TimeDiff in the time diff sequence - */ - double& BackTimeDiff() {return timediff_vec_.back()->dt(); } - - /** - * @brief Access the last TimeDiff in the time diff sequence (read-only) - */ - const double& BackTimeDiff() const {return timediff_vec_.back()->dt(); } - - /** - * @brief Access the vertex of a pose at pos \c index for optimization purposes - * @param index element position inside the internal PoseSequence - * @return Weak raw pointer to the pose vertex at pos \c index - */ - VertexPose* PoseVertex(int index) - { - assert(index& position, double theta, bool fixed=false); - - /** - * @brief Append a new pose vertex to the back of the pose sequence - * @param x x-coordinate of the position part - * @param y y-coordinate of the position part - * @param theta yaw angle representing the orientation part - * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner) - */ - void addPose(double x, double y, double theta, bool fixed=false); - - /** - * @brief Append a new time difference vertex to the back of the time diff sequence - * @param dt time difference value to push back on the internal TimeDiffSequence - * @param fixed Mark the pose to be fixed or unfixed during trajectory optimization (important for the TebOptimalPlanner) - */ - void addTimeDiff(double dt, bool fixed=false); - - /** - * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences) - * @param pose PoseSE2 to push back on the internal PoseSequence - * @param dt time difference value to push back on the internal TimeDiffSequence - * @warning Since the timediff is defined to connect two consecutive poses, this call is only - * allowed if there exist already n poses and n-1 timediffs in the sequences (n=1,2,...): - * therefore add a single pose using addPose() first! - */ - void addPoseAndTimeDiff(const PoseSE2& pose, double dt); - - /** - * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences) - * @param position 2D vector representing the position part - * @param theta yaw angle representing the orientation part - * @param dt time difference value to push back on the internal TimeDiffSequence - * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt) - */ - void addPoseAndTimeDiff(const Eigen::Ref& position, double theta, double dt); - - /** - * @brief Append a (pose, timediff) vertex pair to the end of the current trajectory (pose and timediff sequences) - * @param x x-coordinate of the position part - * @param y y-coordinate of the position part - * @param theta yaw angle representing the orientation part - * @param dt time difference value to push back on the internal TimeDiffSequence - * @warning see addPoseAndTimeDiff(const PoseSE2& pose, double dt) - */ - void addPoseAndTimeDiff(double x, double y, double theta, double dt); - - //@} - - - /** @name Insert new elements and remove elements of the pose and timediff sequences */ - //@{ - - /** - * @brief Insert a new pose vertex at pos. \c index to the pose sequence - * @param index element position inside the internal PoseSequence - * @param pose PoseSE2 element to insert into the internal PoseSequence - */ - void insertPose(int index, const PoseSE2& pose); - - /** - * @brief Insert a new pose vertex at pos. \c index to the pose sequence - * @param index element position inside the internal PoseSequence - * @param position 2D vector representing the position part - * @param theta yaw-angle representing the orientation part - */ - void insertPose(int index, const Eigen::Ref& position, double theta); - - /** - * @brief Insert a new pose vertex at pos. \c index to the pose sequence - * @param index element position inside the internal PoseSequence - * @param x x-coordinate of the position part - * @param y y-coordinate of the position part - * @param theta yaw-angle representing the orientation part - */ - void insertPose(int index, double x, double y, double theta); - - /** - * @brief Insert a new timediff vertex at pos. \c index to the timediff sequence - * @param index element position inside the internal TimeDiffSequence - * @param dt timediff value - */ - void insertTimeDiff(int index, double dt); - - /** - * @brief Delete pose at pos. \c index in the pose sequence - * @param index element position inside the internal PoseSequence - */ - void deletePose(int index); - - /** - * @brief Delete multiple (\c number) poses starting at pos. \c index in the pose sequence - * @param index first element position inside the internal PoseSequence - * @param number number of elements that should be deleted - */ - void deletePoses(int index, int number); - - /** - * @brief Delete pose at pos. \c index in the timediff sequence - * @param index element position inside the internal TimeDiffSequence - */ - void deleteTimeDiff(int index); - - /** - * @brief Delete multiple (\c number) time differences starting at pos. \c index in the timediff sequence - * @param index first element position inside the internal TimeDiffSequence - * @param number number of elements that should be deleted - */ - void deleteTimeDiffs(int index, int number); - - //@} - - - /** @name Init the trajectory */ - //@{ - - /** - * @brief Initialize a trajectory between a given start and goal pose. - * - * The implemented algorithm subsamples the straight line between - * start and goal using a given discretiziation width. \n - * The discretization width can be defined in the euclidean space - * using the \c diststep parameter. Each time difference between two consecutive - * poses is initialized to \c timestep. \n - * If the \c diststep is chosen to be zero, - * the resulting trajectory contains the start and goal pose only. - * @param start PoseSE2 defining the start of the trajectory - * @param goal PoseSE2 defining the goal of the trajectory (final pose) - * @param diststep euclidean distance between two consecutive poses (if 0, no intermediate samples are inserted despite min_samples) - * @param max_vel_x maximum translational velocity used for determining time differences - * @param min_samples Minimum number of samples that should be initialized at least - * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot - * @return true if everything was fine, false otherwise - */ - bool initTrajectoryToGoal(const PoseSE2& start, const PoseSE2& goal, double diststep=0, double max_vel_x=0.5, int min_samples = 3, bool guess_backwards_motion = false); - - - /** - * @brief Initialize a trajectory from a generic 2D reference path. - * - * The temporal information is determined using a given maximum velocity - * (2D vector containing the translational and angular velocity). \n - * A constant velocity profile is implemented. \n - * A possible maximum acceleration is considered if \c max_acceleration param is provided. - * - * Since the orientation is not included in the reference path, it can be provided seperately - * (e.g. from the robot pose and robot goal). \n - * Otherwise the goal heading will be used as start and goal orientation. \n - * The orientation along the trajectory will be determined using the connection vector - * between two consecutive positions of the reference path. - * - * The reference path is provided using a start and end iterator to a container class. - * You must provide a unary function that accepts the dereferenced iterator and returns - * a copy or (const) reference to an Eigen::Vector2d type containing the 2d position. - * - * @param path_start start iterator of a generic 2d path - * @param path_end end iterator of a generic 2d path - * @param fun_position unary function that returns the Eigen::Vector2d object - * @param max_vel_x maximum translational velocity used for determining time differences - * @param max_vel_theta maximum angular velocity used for determining time differences - * @param max_acc_x specify to satisfy a maxmimum transl. acceleration and decceleration (optional) - * @param max_acc_theta specify to satisfy a maxmimum angular acceleration and decceleration (optional) - * @param start_orientation Orientation of the first pose of the trajectory (optional, otherwise use goal heading) - * @param goal_orientation Orientation of the last pose of the trajectory (optional, otherwise use goal heading) - * @param min_samples Minimum number of samples that should be initialized at least - * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot - * @tparam BidirIter Bidirectional iterator type - * @tparam Fun unyary function that transforms the dereferenced iterator into an Eigen::Vector2d - * @return true if everything was fine, false otherwise - * @remarks Use \c boost::none to skip optional arguments. - */ - template - bool initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta, - boost::optional max_acc_x, boost::optional max_acc_theta, - boost::optional start_orientation, boost::optional goal_orientation, int min_samples = 3, bool guess_backwards_motion = false); - - /** - * @brief Initialize a trajectory from a reference pose sequence (positions and orientations). - * - * This method initializes the timed elastic band using a pose container - * (e.g. as local plan from the ros navigation stack). \n - * The initial time difference between two consecutive poses can be uniformly set - * via the argument \c dt. - * @param plan vector of geometry_msgs::msg::PoseStamped - * @param max_vel_x maximum translational velocity used for determining time differences - * @param max_vel_theta maximum rotational velocity used for determining time differences - * @param estimate_orient if \c true, calculate orientation using the straight line distance vector between consecutive poses - * (only copy start and goal orientation; recommended if no orientation data is available). - * @param min_samples Minimum number of samples that should be initialized at least - * @param guess_backwards_motion Allow the initialization of backwards oriented trajectories if the goal heading is pointing behind the robot (this parameter is used only if \c estimate_orient is enabled. - * @return true if everything was fine, false otherwise - */ - bool initTrajectoryToGoal(const std::vector& plan, double max_vel_x, double max_vel_theta, bool estimate_orient=false, int min_samples = 3, bool guess_backwards_motion = false); - - //@} - - /** @name Update and modify the trajectory */ - //@{ - - - /** - * @brief Hot-Start from an existing trajectory with updated start and goal poses. - * - * This method updates a previously optimized trajectory with a new start and/or a new goal pose. \n - * The current simple implementation cuts of pieces of the trajectory that are already passed due to the new start. \n - * Afterwards the start and goal pose are replaced by the new ones. The resulting discontinuity will not be smoothed. - * The optimizer has to smooth the trajectory in TebOptimalPlanner. \n - * - * @todo Smooth the trajectory here and test the performance improvement of the optimization. - * @todo Implement a updateAndPruneTEB based on a new reference path / pose sequence. - * - * @param new_start New start pose (optional) - * @param new_goal New goal pose (optional) - * @param min_samples Specify the minimum number of samples that should at least remain in the trajectory - */ - void updateAndPruneTEB(boost::optional new_start, boost::optional new_goal, int min_samples = 3); - - - /** - * @brief Resize the trajectory by removing or inserting a (pose,dt) pair depending on a reference temporal resolution. - * - * Resizing the trajectory is helpful e.g. for the following scenarios: - * - * - Obstacles requires the teb to be extended in order to - * satisfy the given discritization width (plan resolution) - * and to avoid undesirable behavior due to a large/small discretization step widths \f$ \Delta T_i \f$ - * After clearance of obstacles, the teb should (re-) contract to its (time-)optimal version. - * - If the distance to the goal state is getting smaller, - * dt is decreasing as well. This leads to a heavily - * fine-grained discretization in combination with many - * discrete poses. Thus, the computation time will - * be/remain high and in addition numerical instabilities - * can appear (e.g. due to the division by a small \f$ \Delta T_i \f$). - * - * The implemented strategy checks all timediffs \f$ \Delta T_i \f$ and - * - * - inserts a new sample if \f$ \Delta T_i > \Delta T_{ref} + \Delta T_{hyst} \f$ - * - removes a sample if \f$ \Delta T_i < \Delta T_{ref} - \Delta T_{hyst} \f$ - * - * Each call only one new sample (pose-dt-pair) is inserted or removed. - * @param dt_ref reference temporal resolution - * @param dt_hysteresis hysteresis to avoid oscillations - * @param min_samples minimum number of samples that should be remain in the trajectory after resizing - * @param max_samples maximum number of samples that should not be exceeded during resizing - * @param fast_mode if true, the trajectory is iterated once to insert or erase points; if false the trajectory - * is repeatedly iterated until no poses are added or removed anymore - */ - void autoResize(double dt_ref, double dt_hysteresis, int min_samples = 3, int max_samples=1000, bool fast_mode=false); - - /** - * @brief Set a pose vertex at pos \c index of the pose sequence to be fixed or unfixed during optimization. - * @param index index to the pose vertex - * @param status if \c true, the vertex will be fixed, otherwise unfixed - */ - void setPoseVertexFixed(int index, bool status); - - /** - * @brief Set a timediff vertex at pos \c index of the timediff sequence to be fixed or unfixed during optimization. - * @param index index to the timediff vertex - * @param status if \c true, the vertex will be fixed, otherwise unfixed - */ - void setTimeDiffVertexFixed(int index, bool status); - - /** - * @brief clear all poses and timediffs from the trajectory. - * The pose and timediff sequences will be empty and isInit() will return \c false - */ - void clearTimedElasticBand(); - - //@} - - - /** @name Utility and status methods */ - //@{ - - /** - * @brief Find the closest point on the trajectory w.r.t. to a provided reference point. - * - * This function can be useful to find the part of a trajectory that is close to an obstacle. - * - * @todo implement a more efficient version that first performs a coarse search. - * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance: - * Allows simple comparisons starting from the middle of the trajectory. - * - * @param ref_point reference point (2D position vector) - * @param[out] distance [optional] the resulting minimum distance - * @param begin_idx start search at this pose index - * @return Index to the closest pose in the pose sequence - */ - int findClosestTrajectoryPose(const Eigen::Ref& ref_point, double* distance = NULL, int begin_idx=0) const; - - /** - * @brief Find the closest point on the trajectory w.r.t. to a provided reference line. - * - * This function can be useful to find the part of a trajectory that is close to an (line) obstacle. - * - * @todo implement a more efficient version that first performs a coarse search. - * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance: - * Allows simple comparisons starting from the middle of the trajectory. - * - * @param ref_line_start start of the reference line (2D position vector) - * @param ref_line_end end of the reference line (2D position vector) - * @param[out] distance [optional] the resulting minimum distance - * @return Index to the closest pose in the pose sequence - */ - int findClosestTrajectoryPose(const Eigen::Ref& ref_line_start, const Eigen::Ref& ref_line_end, double* distance = NULL) const; - - /** - * @brief Find the closest point on the trajectory w.r.t. to a provided reference polygon. - * - * This function can be useful to find the part of a trajectory that is close to an (polygon) obstacle. - * - * @todo implement a more efficient version that first performs a coarse search. - * @todo implement a fast approximation that assumes only one local minima w.r.t to the distance: - * Allows simple comparisons starting from the middle of the trajectory. - * - * @param vertices vertex container containing Eigen::Vector2d points (the last and first point are connected) - * @param[out] distance [optional] the resulting minimum distance - * @return Index to the closest pose in the pose sequence - */ - int findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance = NULL) const; - - /** - * @brief Find the closest point on the trajectory w.r.t to a provided obstacle type - * - * This function can be useful to find the part of a trajectory that is close to an obstacle. - * The method is calculates appropriate distance metrics for point, line and polygon obstacles. - * For all unknown obstacles the centroid is used. - * - * @param obstacle Subclass of the Obstacle base class - * @param[out] distance [optional] the resulting minimum distance - * @return Index to the closest pose in the pose sequence - */ - int findClosestTrajectoryPose(const Obstacle& obstacle, double* distance = NULL) const; - - - /** - * @brief Get the length of the internal pose sequence - */ - int sizePoses() const {return (int)pose_vec_.size();}; - - /** - * @brief Get the length of the internal timediff sequence - */ - int sizeTimeDiffs() const {return (int)timediff_vec_.size();}; - - /** - * @brief Check whether the trajectory is initialized (nonzero pose and timediff sequences) - */ - bool isInit() const {return !timediff_vec_.empty() && !pose_vec_.empty();} - - /** - * @brief Calculate the total transition time (sum over all time intervals of the timediff sequence) - */ - double getSumOfAllTimeDiffs() const; - - /** - * @brief Calculate the estimated transition time up to the pose denoted by index - * @param index Index of the pose up to which the transition times are summed up - * @return Estimated transition time up to pose index - */ - double getSumOfTimeDiffsUpToIdx(int index) const; - - /** - * @brief Calculate the length (accumulated euclidean distance) of the trajectory - */ - double getAccumulatedDistance() const; - - /** - * @brief Check if all trajectory points are contained in a specific region - * - * The specific region is a circle around the current robot position (Pose(0)) with given radius \c radius. - * This method investigates a different radius for points behind the robot if \c max_dist_behind_robot >= 0. - * @param radius radius of the region with the robot position (Pose(0)) as center - * @param max_dist_behind_robot A separate radius for trajectory points behind the robot, activated if 0 or positive - * @param skip_poses If >0: the specified number of poses are skipped for the test, e.g. Pose(0), Pose(0+skip_poses+1), Pose(2*skip_poses+2), ... are tested. - * @return \c true, if all tested trajectory points are inside the specified region, \c false otherwise. - */ - bool isTrajectoryInsideRegion(double radius, double max_dist_behind_robot=-1, int skip_poses=0); - - - - //@} - -protected: - PoseSequence pose_vec_; //!< Internal container storing the sequence of optimzable pose vertices - TimeDiffSequence timediff_vec_; //!< Internal container storing the sequence of optimzable timediff vertices - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -} // namespace teb_local_planner - - -// include template implementations / definitions -#include "teb_local_planner/timed_elastic_band.hpp" - - -#endif /* TIMED_ELASTIC_BAND_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp deleted file mode 100644 index 9db4a25..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/timed_elastic_band.hpp +++ /dev/null @@ -1,191 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/timed_elastic_band.h" - -#include - -namespace teb_local_planner -{ -template -bool TimedElasticBand::initTrajectoryToGoal(BidirIter path_start, BidirIter path_end, Fun fun_position, double max_vel_x, double max_vel_theta, - boost::optional max_acc_x, boost::optional max_acc_theta, - boost::optional start_orientation, boost::optional goal_orientation, int min_samples, bool guess_backwards_motion) -{ - Eigen::Vector2d start_position = fun_position( *path_start ); - Eigen::Vector2d goal_position = fun_position( *std::prev(path_end) ); - - bool backwards = false; - - double start_orient, goal_orient; - if (start_orientation) - { - start_orient = *start_orientation; - - // check if the goal is behind the start pose (w.r.t. start orientation) - if (guess_backwards_motion && (goal_position-start_position).dot(Eigen::Vector2d(std::cos(start_orient), std::sin(start_orient))) < 0) - backwards = true; - } - else - { - Eigen::Vector2d start2goal = goal_position - start_position; - start_orient = atan2(start2goal[1],start2goal[0]); - } - - double timestep = 1; // TODO: time - - if (goal_orientation) - { - goal_orient = *goal_orientation; - } - else - { - goal_orient = start_orient; - } - - if (!isInit()) - { - addPose(start_position, start_orient, true); // add starting point and mark it as fixed for optimization - - // we insert middle points now (increase start by 1 and decrease goal by 1) - std::advance(path_start,1); - std::advance(path_end,-1); - int idx=0; - for (; path_start != path_end; ++path_start) // insert middle-points - { - //Eigen::Vector2d point_to_goal = path.back()-*it; - //double dir_to_goal = atan2(point_to_goal[1],point_to_goal[0]); // direction to goal - // Alternative: Direction from last path - Eigen::Vector2d curr_point = fun_position(*path_start); - Eigen::Vector2d diff_last = curr_point - Pose(idx).position(); // we do not use std::prev(*path_start) for those cases, - // where fun_position() does not return a reference or is expensive. - double diff_norm = diff_last.norm(); - - double timestep_vel = diff_norm/max_vel_x; // constant velocity - double timestep_acc; - - if (max_acc_x) - { - timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration - if (timestep_vel < timestep_acc && max_acc_x) timestep = timestep_acc; - else timestep = timestep_vel; - } - else timestep = timestep_vel; - - if (timestep<=0) timestep=0.2; // TODO: this is an assumption - - double yaw = atan2(diff_last[1],diff_last[0]); - if (backwards) - yaw = g2o::normalize_theta(yaw + M_PI); - addPoseAndTimeDiff(curr_point, yaw ,timestep); - - /* - // TODO: the following code does not seem to hot-start the optimizer. Instead it recudes convergence time. - - Eigen::Vector2d diff_next = fun_position(*std::next(path_start))-curr_point; // TODO maybe store the std::next for the following iteration - double ang_diff = std::abs( g2o::normalize_theta( atan2(diff_next[1],diff_next[0]) - -atan2(diff_last[1],diff_last[0]) ) ); - - timestep_vel = ang_diff/max_vel_theta; // constant velocity - if (max_acc_theta) - { - timestep_acc = sqrt(2*ang_diff/(*max_acc_theta)); // constant acceleration - if (timestep_vel < timestep_acc) timestep = timestep_acc; - else timestep = timestep_vel; - } - else timestep = timestep_vel; - - if (timestep<=0) timestep=0.2; // TODO: this is an assumption - - yaw = atan2(diff_last[1],diff_last[0]); // TODO redundant right now, not yet finished - if (backwards) - yaw = g2o::normalize_theta(yaw + M_PI); - addPoseAndTimeDiff(curr_point, yaw ,timestep); - - */ - - ++idx; - } - Eigen::Vector2d diff = goal_position-Pose(idx).position(); - double diff_norm = diff.norm(); - double timestep_vel = diff_norm/max_vel_x; // constant velocity - if (max_acc_x) - { - double timestep_acc = sqrt(2*diff_norm/(*max_acc_x)); // constant acceleration - if (timestep_vel < timestep_acc) timestep = timestep_acc; - else timestep = timestep_vel; - } - else timestep = timestep_vel; - - - PoseSE2 goal(goal_position, goal_orient); - - // if number of samples is not larger than min_samples, insert manually - if ( sizePoses() < min_samples-1 ) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples..."); - while (sizePoses() < min_samples-1) // subtract goal point that will be added later - { - // Each inserted point bisects the remaining distance. Thus the timestep is also bisected. - timestep /= 2; - // simple strategy: interpolate between the current pose and the goal - addPoseAndTimeDiff( PoseSE2::average(BackPose(), goal), timestep ); // let the optimier correct the timestep (TODO: better initialization - } - } - - // now add goal - addPoseAndTimeDiff(goal, timestep); // add goal point - setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization - } - else // size!=0 - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!"); - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs()); - return false; - } - return true; -} - - -} // namespace teb_local_planner - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.h b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.h deleted file mode 100644 index c46dbe8..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.h +++ /dev/null @@ -1,272 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#ifndef VISUALIZATION_H_ -#define VISUALIZATION_H_ - - - -// teb stuff -#include "teb_local_planner/teb_config.h" -#include "teb_local_planner/timed_elastic_band.h" -#include "teb_local_planner/robot_footprint_model.h" - -#include - -// boost -#include -#include - -// std -#include - -#include - -#include - -// messages -#include -#include -#include -#include -#include -#include -#include - -namespace teb_local_planner -{ - -class TebOptimalPlanner; //!< Forward Declaration - - -/** - * @class TebVisualization - * @brief Visualize stuff from the teb_local_planner - */ -class TebVisualization -{ -public: - /** - * @brief Constructor that initializes the class and registers topics - * @param nh local rclcpp::Node::SharedPtr - * @param cfg const reference to the TebConfig class for parameters - */ - TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg); - - /** @name Publish to topics */ - //@{ - - /** - * @brief Publish a given global plan to the ros topic \e ../../global_plan - * @param global_plan Pose array describing the global plan - */ - void publishGlobalPlan(const std::vector& global_plan) const; - - /** - * @brief Publish a given local plan to the ros topic \e ../../local_plan - * @param local_plan Pose array describing the local plan - */ - void publishLocalPlan(const std::vector& local_plan) const; - - /** - * @brief Publish Timed_Elastic_Band related stuff (local plan, pose sequence). - * - * Given a Timed_Elastic_Band instance, publish the local plan to \e ../../local_plan - * and the pose sequence to \e ../../teb_poses. - * @param teb const reference to a Timed_Elastic_Band - */ - void publishLocalPlanAndPoses(const TimedElasticBand& teb) const; - - /** - * @brief Publish the visualization of the robot model - * - * @param current_pose Current pose of the robot - * @param robot_model Subclass of BaseRobotFootprintModel - * @param ns Namespace for the marker objects - * @param color Color of the footprint - */ - void publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns = "RobotFootprintModel", - const std_msgs::msg::ColorRGBA& color = toColorMsg(0.5, 0.0, 0.8, 0.0)); - - /** - * @brief Publish the robot footprints related to infeasible poses - * - * @param current_pose Current pose of the robot - * @param robot_model Subclass of BaseRobotFootprintModel - */ - void publishInfeasibleRobotPose(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model); - - /** - * @brief Publish obstacle positions to the ros topic \e ../../teb_markers - * @todo Move filling of the marker message to polygon class in order to avoid checking types. - * @param obstacles Obstacle container - */ - void publishObstacles(const ObstContainer& obstacles) const; - - /** - * @brief Publish via-points to the ros topic \e ../../teb_markers - * @param via_points via-point container - */ - void publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator >& via_points, const std::string& ns = "ViaPoints") const; - - /** - * @brief Publish a boost::adjacency_list (boost's graph datatype) via markers. - * @remarks Make sure that vertices of the graph contain a member \c pos as \c Eigen::Vector2d type - * to query metric position values. - * @param graph Const reference to the boost::adjacency_list (graph) - * @param ns_prefix Namespace prefix for the marker objects (the strings "Edges" and "Vertices" will be appended) - * @tparam GraphType boost::graph object in which vertices has the field/member \c pos. - */ - template - void publishGraph(const GraphType& graph, const std::string& ns_prefix = "Graph"); - - /** - * @brief Publish multiple 2D paths (each path given as a point sequence) from a container class. - * - * Provide a std::vector< std::vector< T > > in which T.x() and T.y() exist - * and std::vector could be individually substituded by std::list / std::deque /... - * - * A common point-type for object T could be Eigen::Vector2d. - * - * T could be also a raw pointer std::vector< std::vector< T* > >. - * - * @code - * typedef std::vector > PathType; // could be a list or deque as well ... - * std::vector path_container(2); // init 2 empty paths; the container could be a list or deque as well ... - * // Fill path_container.at(0) with Eigen::Vector2d elements, we skip that here - * // Fill path_container.at(1) with Eigen::Vector2d elements, we skip that here - * publishPathContainer( path_container.begin(), path_container.end() ); - * @endcode - * - * @remarks Actually the underlying path does not necessarily need to be a Eigen::Vector2d sequence. - * Eigen::Vector2d can be replaced with any datatype that implement public x() and y() methods.\n - * @param first Bidirectional iterator pointing to the begin of the path - * @param last Bidirectional iterator pointing to the end of the path - * @param ns Namespace for the marker objects (the strings "Edges" and "Vertices" will be appended) - * @tparam BidirIter Bidirectional iterator to a 2D path (sequence of Eigen::Vector2d elements) in a container - */ - template - void publishPathContainer(BidirIter first, BidirIter last, const std::string& ns = "PathContainer"); - - /** - * @brief Publish multiple Tebs from a container class (publish as marker message). - * - * @param teb_planner Container of std::shared_ptr< TebOptPlannerPtr > - * @param ns Namespace for the marker objects - */ - void publishTebContainer(const std::vector< std::shared_ptr >& teb_planner, const std::string& ns = "TebContainer"); - - /** - * @brief Publish a feedback message (multiple trajectory version) - * - * The feedback message contains the all planned trajectory candidates (e.g. if planning in distinctive topologies is turned on). - * Each trajectory is composed of the sequence of poses, the velocity profile and temporal information. - * The feedback message also contains a list of active obstacles. - * @param teb_planners container with multiple tebs (resp. their planner instances) - * @param selected_trajectory_idx Idx of the currently selected trajectory in \c teb_planners - * @param obstacles Container of obstacles - */ - void publishFeedbackMessage(const std::vector< std::shared_ptr >& teb_planners, unsigned int selected_trajectory_idx, const ObstContainer& obstacles); - - /** - * @brief Publish a feedback message (single trajectory overload) - * - * The feedback message contains the planned trajectory - * that is composed of the sequence of poses, the velocity profile and temporal information. - * The feedback message also contains a list of active obstacles. - * @param teb_planner the planning instance - * @param obstacles Container of obstacles - */ - void publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles); - - nav2_util::CallbackReturn on_configure(); - nav2_util::CallbackReturn on_activate(); - nav2_util::CallbackReturn on_deactivate(); - nav2_util::CallbackReturn on_cleanup(); - - //@} - - /** - * @brief Helper function to generate a color message from single values - * @param a Alpha value - * @param r Red value - * @param g Green value - * @param b Blue value - * @return Color message - */ - static std_msgs::msg::ColorRGBA toColorMsg(double a, double r, double g, double b); - -protected: - - /** - * @brief Small helper function that checks if initialize() has been called and prints an error message if not. - * @return \c true if not initialized, \c false if everything is ok - */ - bool printErrorWhenNotInitialized() const; - - nav2_util::LifecycleNode::SharedPtr nh_; - - rclcpp_lifecycle::LifecyclePublisher::SharedPtr global_plan_pub_; //!< Publisher for the global plan - rclcpp_lifecycle::LifecyclePublisher::SharedPtr local_plan_pub_; //!< Publisher for the local plan - rclcpp_lifecycle::LifecyclePublisher::SharedPtr teb_poses_pub_; //!< Publisher for the trajectory pose sequence - rclcpp_lifecycle::LifecyclePublisher::SharedPtr teb_marker_pub_; //!< Publisher for visualization markers - rclcpp_lifecycle::LifecyclePublisher::SharedPtr feedback_pub_; //!< Publisher for the feedback message for analysis and debug purposes - - const TebConfig* cfg_; //!< Config class that stores and manages all related parameters - - bool initialized_; //!< Keeps track about the correct initialization of this class - - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -//! Abbrev. for shared instances of the TebVisualization -typedef std::shared_ptr TebVisualizationPtr; - -//! Abbrev. for shared instances of the TebVisualization (read-only) -typedef std::shared_ptr TebVisualizationConstPtr; - - -} // namespace teb_local_planner - - -// Include template method implementations / definitions -#include "teb_local_planner/visualization.hpp" - -#endif /* VISUALIZATION_H_ */ diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.hpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.hpp deleted file mode 100644 index 9b45a54..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/include/teb_local_planner/visualization.hpp +++ /dev/null @@ -1,227 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/visualization.h" - -#include -#include - -namespace teb_local_planner -{ - - -template -void TebVisualization::publishGraph(const GraphType& graph, const std::string& ns_prefix) -{ - if ( printErrorWhenNotInitialized() ) - return; - - typedef typename boost::graph_traits::vertex_iterator GraphVertexIterator; - typedef typename boost::graph_traits::edge_iterator GraphEdgeIterator; - - // Visualize Edges - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = ns_prefix + "Edges"; - marker.id = 0; - marker.pose.orientation.w = 1.0; -// #define TRIANGLE -#ifdef TRIANGLE - marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; -#else - marker.type = visualization_msgs::msg::Marker::LINE_LIST; -#endif - marker.action = visualization_msgs::msg::Marker::ADD; - - GraphEdgeIterator it_edge, end_edges; - for (boost::tie(it_edge,end_edges) = boost::edges(graph); it_edge!=end_edges; ++it_edge) - { -#ifdef TRIANGLE - geometry_msgs::msg::Point point_start1; - point_start1.x = graph[boost::source(*it_edge,graph)].pos[0]+0.05; - point_start1.y = graph[boost::source(*it_edge,graph)].pos[1]-0.05; - point_start1.z = 0; - marker.points.push_back(point_start1); - geometry_msgs::msg::Point point_start2; - point_start2.x = graph[boost::source(*it_edge,graph)].pos[0]-0.05; - point_start2.y = graph[boost::source(*it_edge,graph)].pos[1]+0.05; - point_start2.z = 0; - marker.points.push_back(point_start2); - -#else - geometry_msgs::msg::Point point_start; - point_start.x = graph[boost::source(*it_edge,graph)].pos[0]; - point_start.y = graph[boost::source(*it_edge,graph)].pos[1]; - point_start.z = 0; - marker.points.push_back(point_start); -#endif - geometry_msgs::msg::Point point_end; - point_end.x = graph[boost::target(*it_edge,graph)].pos[0]; - point_end.y = graph[boost::target(*it_edge,graph)].pos[1]; - point_end.z = 0; - marker.points.push_back(point_end); - - // add color - std_msgs::msg::ColorRGBA color; - color.a = 1.0; - color.r = 0; - color.g = 0; - color.b = 1; - marker.colors.push_back(color); - marker.colors.push_back(color); -#ifdef TRIANGLE - marker.colors.push_back(color); -#endif - } - -#ifdef TRIANGLE - marker.scale.x = 1; - marker.scale.y = 1; - marker.scale.z = 1; -#else - marker.scale.x = 0.01; -#endif - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - // Now publish edge markers - teb_marker_pub_->publish( marker ); - - // Visualize vertices - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = ns_prefix + "Vertices"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - - GraphVertexIterator it_vert, end_vert; - for (boost::tie(it_vert,end_vert) = boost::vertices(graph); it_vert!=end_vert; ++it_vert) - { - geometry_msgs::msg::Point point; - point.x = graph[*it_vert].pos[0]; - point.y = graph[*it_vert].pos[1]; - point.z = 0; - marker.points.push_back(point); - // add color - - std_msgs::msg::ColorRGBA color; - color.a = 1.0; - if (it_vert==end_vert-1) - { - color.r = 1; - color.g = 0; - color.b = 0; - } - else - { - color.r = 0; - color.g = 1; - color.b = 0; - } - marker.colors.push_back(color); - } - // set first color (start vertix) to blue - if (!marker.colors.empty()) - { - marker.colors.front().b = 1; - marker.colors.front().g = 0; - } - - marker.scale.x = 0.1; - marker.scale.y = 0.1; - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - // Now publish vertex markers - teb_marker_pub_->publish( marker ); -} - -template -void TebVisualization::publishPathContainer(BidirIter first, BidirIter last, const std::string& ns) -{ - if ( printErrorWhenNotInitialized() ) - return; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = ns; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - - typedef typename std::iterator_traits::value_type PathType; // Get type of the path (point container) - - // Iterate through path container - while(first != last) - { - // iterate single path points - typename PathType::const_iterator it_point, end_point; - for (it_point = first->begin(), end_point = std::prev(first->end()); it_point != end_point; ++it_point) - { - geometry_msgs::msg::Point point_start; - point_start.x = get_const_reference(*it_point).x(); - point_start.y = get_const_reference(*it_point).y(); - point_start.z = 0; - marker.points.push_back(point_start); - - geometry_msgs::msg::Point point_end; - point_end.x = get_const_reference(*std::next(it_point)).x(); - point_end.y = get_const_reference(*std::next(it_point)).y(); - point_end.z = 0; - marker.points.push_back(point_end); - } - ++first; - } - marker.scale.x = 0.01; - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - teb_marker_pub_->publish( marker ); -} - - -} // namespace teb_local_planner diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/package.xml b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/package.xml deleted file mode 100644 index 770a32a..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/package.xml +++ /dev/null @@ -1,53 +0,0 @@ - - - teb_local_planner - - 0.9.1 - - - The teb_local_planner package implements a plugin - to the base_local_planner of the 2D navigation stack. - The underlying method called Timed Elastic Band locally optimizes - the robot's trajectory with respect to trajectory execution time, - separation from obstacles and compliance with kinodynamic constraints at runtime. - - - Christoph Rösmann - - BSD - - http://wiki.ros.org/teb_local_planner - - Christoph Rösmann - - ament_cmake - - costmap_converter - costmap_converter_msgs - - geometry_msgs - libg2o - dwb_critics - nav2_core - nav2_costmap_2d - nav2_msgs - nav2_util - pluginlib - rclcpp - rclcpp_action - rclcpp_lifecycle - std_msgs - teb_msgs - tf2 - tf2_eigen - visualization_msgs - builtin_interfaces - nav2_bringup - - ament_cmake_gtest - - - ament_cmake - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/params/teb_params.yaml b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/params/teb_params.yaml deleted file mode 100644 index 4fc447b..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/params/teb_params.yaml +++ /dev/null @@ -1,109 +0,0 @@ -controller_server: - ros__parameters: - odom_topic: /odom - use_sim_time: True - controller_frequency: 5.0 - controller_plugin_types: ["teb_local_planner::TebLocalPlannerROS"] - controller_plugins: ["FollowPath"] - FollowPath: - plugin: teb_local_planner::TebLocalPlannerROS - - teb_autosize: 1.0 - dt_ref: 0.3 - dt_hysteresis: 0.1 - max_samples: 500 - global_plan_overwrite_orientation: False - allow_init_with_backwards_motion: False - max_global_plan_lookahead_dist: 3.0 - global_plan_viapoint_sep: 0.3 - global_plan_prune_distance: 1.0 - exact_arc_length: False - feasibility_check_no_poses: 2 - publish_feedback: False - - # Robot - - max_vel_x: 0.26 - max_vel_theta: 1.0 - acc_lim_x: 2.5 - acc_lim_theta: 3.2 - - footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" - type: "circular" - radius: 0.17 # for type "circular" - - # GoalTolerance - - free_goal_vel: False - - # Obstacles - - min_obstacle_dist: 0.27 - inflation_dist: 0.6 - include_costmap_obstacles: True - costmap_obstacles_behind_robot_dist: 1.0 - obstacle_poses_affected: 15 - - dynamic_obstacle_inflation_dist: 0.6 - include_dynamic_obstacles: True - - costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" - costmap_converter_spin_thread: True - costmap_converter_rate: 5 - - # Optimization - - no_inner_iterations: 5 - no_outer_iterations: 4 - optimization_activate: True - optimization_verbose: False - penalty_epsilon: 0.1 - obstacle_cost_exponent: 4.0 - weight_max_vel_x: 0.5 - weight_max_vel_theta: 0.5 - weight_acc_lim_x: 0.5 - weight_acc_lim_theta: 10.5 - weight_kinematics_nh: 1000.0 - weight_kinematics_forward_drive: 3.0 - weight_kinematics_turning_radius: 1.0 - weight_optimaltime: 1.0 # must be > 0 - weight_shortest_path: 0.0 - weight_obstacle: 100.0 - weight_inflation: 0.2 - weight_dynamic_obstacle: 10.0 # not in use yet - weight_dynamic_obstacle_inflation: 0.2 - weight_viapoint: 50.0 - weight_adapt_factor: 2.0 - - # Homotopy Class Planner - - enable_homotopy_class_planning: True - enable_multithreading: True - max_number_classes: 4 - selection_cost_hysteresis: 5.0 - selection_prefer_initial_plan: 1.0 - selection_obst_cost_scale: 1.0 - selection_alternative_time_cost: True - - roadmap_graph_no_samples: 15 - roadmap_graph_area_width: 5.0 - roadmap_graph_area_length_scale: 1.0 - h_signature_prescaler: 0.5 - h_signature_threshold: 0.1 - obstacle_heading_threshold: 0.45 - switching_blocking_period: 0.0 - viapoints_all_candidates: True - delete_detours_backwards: True - max_ratio_detours_duration_best_duration: 3.0 - visualize_hc_graph: False - visualize_with_time_as_z_axis_scale: 0.0 - - # Recovery - - shrink_horizon_backup: True - shrink_horizon_min_duration: 10.0 - oscillation_recovery: True - oscillation_v_eps: 0.1 - oscillation_omega_eps: 0.1 - oscillation_recovery_min_duration: 10.0 - oscillation_filter_duration: 10.0 diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py deleted file mode 100755 index d8c5b5f..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/cmd_vel_to_ackermann_drive.py +++ /dev/null @@ -1,60 +0,0 @@ -#!/usr/bin/env python3 - -# Author: christoph.roesmann@tu-dortmund.de - -import rclpy, math -from geometry_msgs.msg import Twist -from ackermann_msgs.msg import AckermannDriveStamped - - -def convert_trans_rot_vel_to_steering_angle(v, omega, wheelbase): - if omega == 0 or v == 0: - return 0 - - radius = v / omega - return math.atan(wheelbase / radius) - - -def cmd_callback(data): - global wheelbase - global ackermann_cmd_topic - global frame_id - global pub - global cmd_angle_instead_rotvel - - v = data.linear.x - # if cmd_angle_instead_rotvel is true, the rotational velocity is already converted in the C++ node - # in this case this script only needs to do the msg conversion from twist to Ackermann drive - if cmd_angle_instead_rotvel: - steering = data.angular.z - else: - steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase) - - msg = AckermannDriveStamped() - msg.header.stamp = node.get_clock().now().to_msg() - msg.header.frame_id = frame_id - msg.drive.steering_angle = float(steering) - msg.drive.speed = float(v) - - pub.publish(msg) - - - - - -if __name__ == '__main__': - rclpy.init() - global node - node = rclpy.create_node('cmd_vel_to_ackermann_drive') - - twist_cmd_topic = node.declare_parameter("twist_cmd_topic", "/cmd_vel").value - ackermann_cmd_topic = node.declare_parameter("ackermann_cmd_topic", "/ackermann_cmd").value - wheelbase = node.declare_parameter("wheelbase", 1.0).value - frame_id = node.declare_parameter('frame_id', 'odom').value - cmd_angle_instead_rotvel = node.declare_parameter('cmd_angle_instead_rotvel', False).value - - node.create_subscription(Twist, twist_cmd_topic, cmd_callback, 1) - pub = node.create_publisher(AckermannDriveStamped, ackermann_cmd_topic, 1) - - rclpy.spin(node) - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_mat.py b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_mat.py deleted file mode 100755 index 1cc3351..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_mat.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python - -# This small script subscribes to the FeedbackMsg message of teb_local_planner -# and exports data to a mat file. -# publish_feedback must be turned on such that the planner publishes this information. -# Author: christoph.roesmann@tu-dortmund.de - -import rospy, math -from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg -from geometry_msgs.msg import PolygonStamped, Point32, Quaternion -from tf.transformations import euler_from_quaternion -import numpy as np -import scipy.io as sio -import time - -def feedback_callback(data): - global got_data - - if not data.trajectories: # empty - trajectory = [] - return - - if got_data: - return - - got_data = True - - # copy trajectory - trajectories = [] - for traj in data.trajectories: - trajectory = [] -# # store as struct and cell array -# for point in traj.trajectory: -# (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w]) -# pose = {'x': point.pose.position.x, 'y': point.pose.position.y, 'theta': yaw} -# velocity = {'v': point.velocity.linear.x, 'omega': point.velocity.angular.z} -# time_from_start = point.time_from_start.to_sec() -# trajectory.append({'pose': pose, 'velocity': velocity, 'time_from_start': time_from_start}) - - # store as all-in-one mat - arr = np.zeros([6, len(traj.trajectory)], dtype='double'); # x, y, theta, v, omega, t - for index, point in enumerate(traj.trajectory): - arr[0,index] = point.pose.position.x - arr[1,index] = point.pose.position.y - (roll,pitch,yaw) = euler_from_quaternion([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w]) - arr[2,index] = yaw - arr[3,index] = point.velocity.linear.x - arr[4,index] = point.velocity.angular.z - arr[5,index] = point.time_from_start.to_sec() - -# trajectories.append({'raw': trajectory, 'mat': arr}) - trajectories.append({'data': arr, 'legend': ['x','y','theta','v','omega','t']}) - - # copy obstacles - obstacles = [] - for obst_id, obst in enumerate(data.obstacle_msg.obstacles): - #polygon = [] - #for point in obst.polygon.points: - # polygon.append({'x': point.x, 'y': point.y, 'z': point.z}) - obst_arr = np.zeros([4, len(obst.polygon.points)], dtype='double'); # x, y - for index, point in enumerate(obst.polygon.points): - obst_arr[0, index] = point.x - obst_arr[1, index] = point.y - obst_arr[2, index] = data.obstacle_msg.velocities[obst_id].twist.linear.x - obst_arr[3, index] = data.obstacle_msg.velocities[obst_id].twist.linear.y - - #obstacles.append(polygon) - obstacles.append({'data': obst_arr, 'legend': ['x','y', 'v_x', 'v_y']}) - - - # create main struct: - mat = {'selected_trajectory_idx': data.selected_trajectory_idx, 'trajectories': trajectories, 'obstacles': obstacles} - - timestr = time.strftime("%Y%m%d_%H%M%S") - filename = '/home/albers/MasterThesis/Matlab/Homotopie/test_optim_node/' + 'teb_data_' + timestr + '.mat' - - rospy.loginfo("Saving mat-file '%s'.", filename) - sio.savemat(filename, mat) - - - - - -def feedback_exporter(): - global got_data - - rospy.init_node("export_to_mat", anonymous=True) - - - topic_name = "/test_optim_node/teb_feedback" # define feedback topic here! - - rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) - - rospy.loginfo("Waiting for feedback message on topic %s.", topic_name) - - r = rospy.Rate(2) # define rate here - while not rospy.is_shutdown(): - - if got_data: - rospy.loginfo("Data export completed.") - return - - r.sleep() - -if __name__ == '__main__': - try: - global got_data - got_data = False - feedback_exporter() - except rospy.ROSInterruptException: - pass - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_svg.py b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_svg.py deleted file mode 100755 index 8d5edf2..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/export_to_svg.py +++ /dev/null @@ -1,244 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -""" -======================================================================================== -# This small script subscribes to the FeedbackMsg message of teb_local_planner -# and converts the current scene to a svg-image -# publish_feedback must be turned on such that the planner publishes this information. -# Author: christoph.roesmann@tu-dortmund.de - -It is recommendable to start this node after initialization of TEB is completed. - -Requirements: -svgwrite: A Python library to create SVG drawings. http://pypi.python.org/pypi/svgwrite -======================================================================================= -""" -import roslib; -import rospy -import svgwrite -import math -import sys -import time -import random -from svgwrite import cm, mm -from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg -from geometry_msgs.msg import PolygonStamped, Point32, Quaternion - - -# ================= PARAMETERS ================== -# TODO: In case of a more general node, change parameter to ros-parameter -# Drawing parameters: -SCALE = 200 # Overall scaling: 100 pixel = 1 m -MIN_POSE_DISTANCE = 0.3 # Distance between two consecutive poses in SVG-image -SCALE_VELOCITY_VEC = 0.4 # Scaling of velocity vectors -> 1 cell = 1/SCALE_VELOCITY_VEC m/s -GRID_X_MIN = -2 # Define, how many cells your grid should contain in each direction. -GRID_X_MAX = 2 -GRID_Y_MIN = -2 -GRID_Y_MAX = 1 - -# TEB parameters: -OBSTACLE_DIST = 50 *SCALE/100 # cm - - -# ================= FUNCTIONS =================== - -def sign(number): - """ - Signum function: get sign of a number - - @param number: get sign of this number - @type number: numeric type (eg. integer) - @return: sign of number - @rtype: integer {1, -1, 0} - """ - return cmp(number,0) - -def arrowMarker(color='green', orientation='auto'): - """ - Create an arrow marker with svgwrite - - @return: arrow marker - @rtype: svg_write marker object - """ - arrow = svg.marker(insert=(1,5), size=(4,3), orient=orientation) - arrow.viewbox(width=10, height=10) - arrow.add(svg.polyline([(0,0),(10,5),(0,10),(1,5)], fill=color, opacity=1.0)) - svg.defs.add(arrow) - return arrow - -def quaternion2YawDegree(orientation): - """ - Get yaw angle [degree] from quaternion representation - - @param orientation: orientation in quaternions to read from - @type orientation: geometry_msgs/Quaternion - @return: yaw angle [degree] - @rtype: float - """ - yawRad = math.atan2(2*(orientation.x*orientation.y+orientation.z*orientation.w),1-2*(pow(orientation.y,2)+pow(orientation.z,2))) - return yawRad*180/math.pi - - -def feedback_callback(data): - """ - Callback for receiving TEB and obstacle information - - @param data: Received feedback message - @type data: visualization_msgs/Marker - - @globalparam tebList: Received TEB List - @globaltype tebList: teb_local_planner/FeedbackMsg - """ - # TODO: Remove global variables - global feedbackMsg - - if not feedbackMsg: - feedbackMsg = data - rospy.loginfo("TEB feedback message received...") - - -# ================ MAIN FUNCTION ================ - -if __name__ == '__main__': - rospy.init_node('export_to_svg', anonymous=True) - - topic_name = "/test_optim_node/teb_feedback" # define feedback topic here! - - rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) - - rospy.loginfo("Waiting for feedback message on topic %s.", topic_name) - - rate = rospy.Rate(10.0) - feedbackMsg = [] - - timestr = time.strftime("%Y%m%d_%H%M%S") - filename_string = "teb_svg_" + timestr + '.svg' - - rospy.loginfo("SVG will be written to '%s'.", filename_string) - - random.seed(0) - - svg=svgwrite.Drawing(filename=filename_string, debug=True) - - # Create viewbox -> this box defines the size of the visible drawing - svg.viewbox(GRID_X_MIN*SCALE-1*SCALE,GRID_Y_MIN*SCALE-1*SCALE,GRID_X_MAX*SCALE-GRID_X_MIN*SCALE+2*SCALE,GRID_Y_MAX*SCALE-GRID_Y_MIN*SCALE+2*SCALE) - - # Draw grid: - hLines = svg.add(svg.g(id='hLines', stroke='black')) - hLines.add(svg.line(start=(GRID_X_MIN*SCALE, 0), end=(GRID_X_MAX*SCALE, 0))) - for y in range(GRID_Y_MAX): - hLines.add(svg.line(start=(GRID_X_MIN*SCALE, SCALE+y*SCALE), end=(GRID_X_MAX*SCALE, SCALE+y*SCALE))) - for y in range(-GRID_Y_MIN): - hLines.add(svg.line(start=(GRID_X_MIN*SCALE, -SCALE-y*SCALE), end=(GRID_X_MAX*SCALE, -SCALE-y*SCALE))) - vLines = svg.add(svg.g(id='vline', stroke='black')) - vLines.add(svg.line(start=(0, GRID_Y_MIN*SCALE), end=(0, GRID_Y_MAX*SCALE))) - for x in range(GRID_X_MAX): - vLines.add(svg.line(start=(SCALE+x*SCALE, GRID_Y_MIN*SCALE), end=(SCALE+x*SCALE, GRID_Y_MAX*SCALE))) - for x in range(-GRID_X_MIN): - vLines.add(svg.line(start=(-SCALE-x*SCALE, GRID_Y_MIN*SCALE), end=(-SCALE-x*SCALE, GRID_Y_MAX*SCALE))) - - - # Draw legend: - legend = svg.g(id='legend', font_size=25) - stringGeometry = "Geometry: 1 Unit = 1.0m" - legendGeometry = svg.text(stringGeometry) - legend.add(legendGeometry) - legend.translate(tx=GRID_X_MIN*SCALE, ty=GRID_Y_MAX*SCALE + 30) # Move legend to buttom left corner - svg.add(legend) - - - #arrow = arrowMarker() # Init arrow marker - - rospy.loginfo("Initialization completed.\nWaiting for feedback message...") - - # -------------------- WAIT FOR CALLBACKS -------------------------- - while not rospy.is_shutdown(): - if feedbackMsg: - break # Leave loop after receiving all necessary TEB information (see callbacks) to finish drawing - rate.sleep() - # ------------------------------------------------------------------ - - if not feedbackMsg.trajectories: - rospy.loginfo("Received message does not contain trajectories. Shutting down...") - sys.exit() - - if len(feedbackMsg.trajectories[0].trajectory) < 2: - rospy.loginfo("Received message does not contain trajectories with at least two states (start and goal). Shutting down...") - sys.exit() - - # iterate trajectories - for index, traj in enumerate(feedbackMsg.trajectories): - - #color - traj_color = svgwrite.rgb(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255), 'RGB') - - # Iterate through TEB positions -> Draw Paths - points = [] - for point in traj.trajectory: - points.append( (point.pose.position.x*SCALE,-point.pose.position.y*SCALE) ) # y is negative in image coordinates - # svgwrite rotates clockwise! - - - if index == feedbackMsg.selected_trajectory_idx: # highlight currently selected teb - line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='round', \ - stroke_linejoin='round', opacity=1.0 ) ) - else: - line = svg.add( svg.polyline(points=points, fill='none', stroke=traj_color, stroke_width=10, stroke_linecap='butt', \ - stroke_linejoin='round', stroke_dasharray='10,3', opacity=1.0 ) ) - #marker_points = points[::7] - #markerline = svg.add( svg.polyline(points=marker_points, fill='none', stroke=traj_color, stroke_width=10, opacity=0.0 ) ) - #arrow = arrowMarker(traj_color) - #markerline.set_markers( (arrow, arrow, arrow) ) - #line.set_markers( (arrow, arrow, arrow) ) - #line['marker-start'] = arrow.get_funciri() - - - # Add Start and Goal Point - start_pose = feedbackMsg.trajectories[0].trajectory[0].pose - goal_pose = feedbackMsg.trajectories[0].trajectory[len(feedbackMsg.trajectories[0].trajectory)-1].pose - start_position = start_pose.position - goal_position = goal_pose.position - svg.add(svg.circle(center=(start_position.x*SCALE,-start_position.y*SCALE), r=10, stroke_width=1, stroke='blue', fill ='blue')) - svg.add(svg.text("Start", (start_position.x*SCALE-70, -start_position.y*SCALE+45), font_size=35)) # Add label - svg.add(svg.circle(center=(goal_position.x*SCALE,-goal_position.y*SCALE), r=10, stroke_width=1, stroke='red', fill ='red')) - svg.add(svg.text("Goal", (goal_position.x*SCALE-40, -goal_position.y*SCALE+45), font_size=35)) # Add label - - # draw start arrow - start_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='blue', opacity=1.0) - start_arrow.translate(start_position.x*SCALE,-start_position.y*SCALE) - start_arrow.rotate( quaternion2YawDegree(start_pose.orientation) ) - start_arrow.scale(3) - svg.add(start_arrow) - - # draw goal arrow - goal_arrow = svg.polyline([(0,-1),(6,-1),(5,-5),(15,0),(5,5),(6,1),(0,1)], fill='red', opacity=1.0) - goal_arrow.translate(goal_position.x*SCALE,-goal_position.y*SCALE) - goal_arrow.rotate( quaternion2YawDegree(goal_pose.orientation) ) - goal_arrow.scale(3) - svg.add(goal_arrow) - - # Draw obstacles - for obstacle in feedbackMsg.obstacles: - if len(obstacle.polygon.points) == 1: # point obstacle - point = obstacle.polygon.points[0] - svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=OBSTACLE_DIST, stroke_width=1, stroke='grey', fill ='grey', opacity=0.3)) - svg.add(svg.circle(center=(point.x*SCALE,-point.y*SCALE), r=15, stroke_width=1, stroke='black', fill ='black')) - svg.add(svg.text("Obstacle", (point.x*SCALE-70, -point.y*SCALE+45), font_size=35)) # Add label - if len(obstacle.polygon.points) == 2: # line obstacle - line_start = obstacle.polygon.points[0] - line_end = obstacle.polygon.points[1] - svg.add(svg.line(start=(line_start.x*SCALE,-line_start.y*SCALE), end=(line_end.x*SCALE,-line_end.y*SCALE), stroke='black', fill='gray', stroke_width=1, opacity=1.0)) - svg.add(svg.text("Obstacle", (line_start.x*SCALE-70, -line_start.y*SCALE+45), font_size=35)) # Add label - if len(obstacle.polygon.points) > 2: # polygon obstacle - vertices = [] - for point in obstacle.polygon.points: - vertices.append((point.x*SCALE, -point.y*SCALE)) - svg.add(svg.polygon(points=vertices, stroke='black', fill='gray', stroke_width=1, opacity=1.0)) - svg.add(svg.text("Obstacle", (obstacle.polygon.points[0].x*SCALE-70, -obstacle.polygon.points.y*SCALE+45), font_size=35)) # Add label - - - - # Save svg to file (svg_output.svg) and exit node - svg.save() - - rospy.loginfo("Drawing completed.") diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_dynamic_obstacle.py b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_dynamic_obstacle.py deleted file mode 100755 index 0cc16d4..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_dynamic_obstacle.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -# Author: franz.albers@tu-dortmund.de - -import rospy, math, tf -from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg -from geometry_msgs.msg import PolygonStamped, Point32, QuaternionStamped, Quaternion, TwistWithCovariance -from tf.transformations import quaternion_from_euler - - -def publish_obstacle_msg(): - pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) - #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) - rospy.init_node("test_obstacle_msg") - - y_0 = -3.0 - vel_x = 0.0 - vel_y = 0.3 - range_y = 6.0 - - obstacle_msg = ObstacleArrayMsg() - obstacle_msg.header.stamp = rospy.Time.now() - obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map - - # Add point obstacle - obstacle_msg.obstacles.append(ObstacleMsg()) - obstacle_msg.obstacles[0].id = 99 - obstacle_msg.obstacles[0].polygon.points = [Point32()] - obstacle_msg.obstacles[0].polygon.points[0].x = -1.5 - obstacle_msg.obstacles[0].polygon.points[0].y = 0 - obstacle_msg.obstacles[0].polygon.points[0].z = 0 - - yaw = math.atan2(vel_y, vel_x) - q = tf.transformations.quaternion_from_euler(0,0,yaw) - obstacle_msg.obstacles[0].orientation = Quaternion(*q) - - obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x - obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y - obstacle_msg.obstacles[0].velocities.twist.linear.z = 0 - obstacle_msg.obstacles[0].velocities.twist.angular.x = 0 - obstacle_msg.obstacles[0].velocities.twist.angular.y = 0 - obstacle_msg.obstacles[0].velocities.twist.angular.z = 0 - - r = rospy.Rate(10) # 10hz - t = 0.0 - while not rospy.is_shutdown(): - - # Vary y component of the point obstacle - if (vel_y >= 0): - obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - else: - obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y*t)%range_y - range_y - - t = t + 0.1 - - pub.publish(obstacle_msg) - - r.sleep() - - - -if __name__ == '__main__': - try: - publish_obstacle_msg() - except rospy.ROSInterruptException: - pass - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_test_obstacles.py b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_test_obstacles.py deleted file mode 100755 index 800803d..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_test_obstacles.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python - -# Author: christoph.roesmann@tu-dortmund.de - -import rospy, math -from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg -from geometry_msgs.msg import PolygonStamped, Point32 - - -def publish_obstacle_msg(): - pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) - #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) - rospy.init_node("test_obstacle_msg") - - - obstacle_msg = ObstacleArrayMsg() - obstacle_msg.header.stamp = rospy.Time.now() - obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map - - # Add point obstacle - obstacle_msg.obstacles.append(ObstacleMsg()) - obstacle_msg.obstacles[0].id = 0 - obstacle_msg.obstacles[0].polygon.points = [Point32()] - obstacle_msg.obstacles[0].polygon.points[0].x = 1.5 - obstacle_msg.obstacles[0].polygon.points[0].y = 0 - obstacle_msg.obstacles[0].polygon.points[0].z = 0 - - - # Add line obstacle - obstacle_msg.obstacles.append(ObstacleMsg()) - obstacle_msg.obstacles[1].id = 1 - line_start = Point32() - line_start.x = -2.5 - line_start.y = 0.5 - #line_start.y = -3 - line_end = Point32() - line_end.x = -2.5 - line_end.y = 2 - #line_end.y = -4 - obstacle_msg.obstacles[1].polygon.points = [line_start, line_end] - - # Add polygon obstacle - obstacle_msg.obstacles.append(ObstacleMsg()) - obstacle_msg.obstacles[1].id = 2 - v1 = Point32() - v1.x = -1 - v1.y = -1 - v2 = Point32() - v2.x = -0.5 - v2.y = -1.5 - v3 = Point32() - v3.x = 0 - v3.y = -1 - obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3] - - - r = rospy.Rate(10) # 10hz - t = 0.0 - while not rospy.is_shutdown(): - - # Vary y component of the point obstacle - obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t) - t = t + 0.1 - - pub.publish(obstacle_msg) - - r.sleep() - - - -if __name__ == '__main__': - try: - publish_obstacle_msg() - except rospy.ROSInterruptException: - pass - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_viapoints.py b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_viapoints.py deleted file mode 100755 index 7bd0c7e..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/publish_viapoints.py +++ /dev/null @@ -1,46 +0,0 @@ -#!/usr/bin/env python - -# Author: christoph.roesmann@tu-dortmund.de - -import rospy, math -from geometry_msgs.msg import PoseStamped -from nav_msgs.msg import Path - - -def publish_via_points_msg(): - pub = rospy.Publisher('/test_optim_node/via_points', Path, queue_size=1) - rospy.init_node("test_via_points_msg") - - - via_points_msg = Path() - via_points_msg.header.stamp = rospy.Time.now() - via_points_msg.header.frame_id = "odom" # CHANGE HERE: odom/map - - # Add via-points - point1 = PoseStamped() - point1.pose.position.x = 0.0; - point1.pose.position.y = 1.5; - - point2 = PoseStamped() - point2.pose.position.x = 2.0; - point2.pose.position.y = -0.5; - - - via_points_msg.poses = [point1, point2] - - r = rospy.Rate(5) # 10hz - t = 0.0 - while not rospy.is_shutdown(): - - pub.publish(via_points_msg) - - r.sleep() - - - -if __name__ == '__main__': - try: - publish_via_points_msg() - except rospy.ROSInterruptException: - pass - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/visualize_velocity_profile.py b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/visualize_velocity_profile.py deleted file mode 100755 index b9b6d78..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/scripts/visualize_velocity_profile.py +++ /dev/null @@ -1,76 +0,0 @@ -#!/usr/bin/env python - -# This small script subscribes to the FeedbackMsg message of teb_local_planner -# and plots the current velocity. -# publish_feedback must be turned on such that the planner publishes this information. -# Author: christoph.roesmann@tu-dortmund.de - -import rospy, math -from teb_local_planner.msg import FeedbackMsg, TrajectoryMsg, TrajectoryPointMsg -from geometry_msgs.msg import PolygonStamped, Point32 -import numpy as np -import matplotlib.pyplot as plotter - -def feedback_callback(data): - global trajectory - - if not data.trajectories: # empty - trajectory = [] - return - trajectory = data.trajectories[data.selected_trajectory_idx].trajectory - - -def plot_velocity_profile(fig, ax_v, ax_omega, t, v, omega): - ax_v.cla() - ax_v.grid() - ax_v.set_ylabel('Trans. velocity [m/s]') - ax_v.plot(t, v, '-bx') - ax_omega.cla() - ax_omega.grid() - ax_omega.set_ylabel('Rot. velocity [rad/s]') - ax_omega.set_xlabel('Time [s]') - ax_omega.plot(t, omega, '-bx') - fig.canvas.draw() - - - -def velocity_plotter(): - global trajectory - rospy.init_node("visualize_velocity_profile", anonymous=True) - - topic_name = "/test_optim_node/teb_feedback" - topic_name = rospy.get_param('~feedback_topic', topic_name) - rospy.Subscriber(topic_name, FeedbackMsg, feedback_callback, queue_size = 1) # define feedback topic here! - - rospy.loginfo("Visualizing velocity profile published on '%s'.",topic_name) - rospy.loginfo("Make sure to enable rosparam 'publish_feedback' in the teb_local_planner.") - - # two subplots sharing the same t axis - fig, (ax_v, ax_omega) = plotter.subplots(2, sharex=True) - plotter.ion() - plotter.show() - - - r = rospy.Rate(2) # define rate here - while not rospy.is_shutdown(): - - t = [] - v = [] - omega = [] - - for point in trajectory: - t.append(point.time_from_start.to_sec()) - v.append(point.velocity.linear.x) - omega.append(point.velocity.angular.z) - - plot_velocity_profile(fig, ax_v, ax_omega, np.asarray(t), np.asarray(v), np.asarray(omega)) - - r.sleep() - -if __name__ == '__main__': - try: - trajectory = [] - velocity_plotter() - except rospy.ROSInterruptException: - pass - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/graph_search.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/graph_search.cpp deleted file mode 100644 index f2d1218..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/graph_search.cpp +++ /dev/null @@ -1,346 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Authors: Christoph Rösmann, Franz Albers - *********************************************************************/ - -#include "teb_local_planner/graph_search.h" -#include "teb_local_planner/homotopy_class_planner.h" - -namespace teb_local_planner -{ - -void GraphSearchInterface::DepthFirst(HcGraph& g, std::vector& visited, const HcGraphVertexType& goal, double start_orientation, - double goal_orientation, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel) -{ - // see http://www.technical-recipes.com/2011/a-recursive-algorithm-to-find-all-paths-between-two-given-nodes/ for details on finding all simple paths - - if ((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes) - return; // We do not need to search for further possible alternative homotopy classes. - - HcGraphVertexType back = visited.back(); - - /// Examine adjacent nodes - HcGraphAdjecencyIterator it, end; - for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it) - { - if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() ) - continue; // already visited - - if ( *it == goal ) // goal reached - { - visited.push_back(*it); - - // Add new TEB, if this path belongs to a new homotopy class - hcp_->addAndInitNewTeb(visited.begin(), visited.end(), std::bind(getVector2dFromHcGraph, std::placeholders::_1, boost::cref(graph_)), - start_orientation, goal_orientation, start_velocity); - - visited.pop_back(); - break; - } - } - - /// Recursion for all adjacent vertices - for ( boost::tie(it,end) = boost::adjacent_vertices(back,g); it!=end; ++it) - { - if ( std::find(visited.begin(), visited.end(), *it)!=visited.end() || *it == goal) - continue; // already visited || goal reached - - - visited.push_back(*it); - - // recursion step - DepthFirst(g, visited, goal, start_orientation, goal_orientation, start_velocity, free_goal_vel); - - visited.pop_back(); - } -} - - - -void lrKeyPointGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel) -{ - // Clear existing graph and paths - clearGraph(); - if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes) - return; - // Direction-vector between start and goal and normal-vector: - Eigen::Vector2d diff = goal.position()-start.position(); - - if (diff.norm()goal_tolerance.xy_goal_tolerance) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached."); - if (hcp_->getTrajectoryContainer().empty()) - { - RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), - "HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors."); - hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel); - } - return; - } - - Eigen::Vector2d normal(-diff[1],diff[0]); // normal-vector - normal.normalize(); - normal = normal*dist_to_obst; // scale with obstacle_distance; - - // Insert Vertices - HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex - graph_[start_vtx].pos = start.position(); - diff.normalize(); - - // store nearest obstacle keypoints -> only used if limit_obstacle_heading is enabled - std::pair nearest_obstacle; // both vertices are stored - double min_dist = DBL_MAX; - - if (hcp_->obstacles()!=NULL) - { - for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst) - { - // check if obstacle is placed in front of start point - Eigen::Vector2d start2obst = (*it_obst)->getCentroid() - start.position(); - double dist = start2obst.norm(); - if (start2obst.dot(diff)/dist<0.1) - continue; - - // Add Keypoints - HcGraphVertexType u = boost::add_vertex(graph_); - graph_[u].pos = (*it_obst)->getCentroid() + normal; - HcGraphVertexType v = boost::add_vertex(graph_); - graph_[v].pos = (*it_obst)->getCentroid() - normal; - - // store nearest obstacle - if (obstacle_heading_threshold && distobstacles()!=NULL) - { - bool collision = false; - for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst) - { - if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, 0.5*dist_to_obst) ) - { - collision = true; - break; - } - } - if (collision) - continue; - } - - // Create Edge - boost::add_edge(*it_i,*it_j,graph_); - } - } - - - // Find all paths between start and goal! - std::vector visited; - visited.push_back(start_vtx); - DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel); -} - - -void ProbRoadmapGraph::createGraph(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, double obstacle_heading_threshold, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel) -{ - // Clear existing graph and paths - clearGraph(); - if((int)hcp_->getTrajectoryContainer().size() >= cfg_->hcp.max_number_classes) - return; - // Direction-vector between start and goal and normal-vector: - Eigen::Vector2d diff = goal.position()-start.position(); - double start_goal_dist = diff.norm(); - - if (start_goal_distgoal_tolerance.xy_goal_tolerance) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "HomotopyClassPlanner::createProbRoadmapGraph(): xy-goal-tolerance already reached."); - if (hcp_->getTrajectoryContainer().empty()) - { - RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), - "HomotopyClassPlanner::createProbRoadmapGraph(): Initializing a small straight line to just correct orientation errors."); - hcp_->addAndInitNewTeb(start, goal, start_velocity, free_goal_vel); - } - return; - } - Eigen::Vector2d normal(-diff.coeffRef(1),diff.coeffRef(0)); // normal-vector - normal.normalize(); - - // Now sample vertices between start, goal and a specified width between both sides - // Let's start with a square area between start and goal (maybe change it later to something like a circle or whatever) - - double area_width = cfg_->hcp.roadmap_graph_area_width; - - boost::random::uniform_real_distribution distribution_x(0, start_goal_dist * cfg_->hcp.roadmap_graph_area_length_scale); - boost::random::uniform_real_distribution distribution_y(0, area_width); - - double phi = atan2(diff.coeffRef(1),diff.coeffRef(0)); // rotate area by this angle - Eigen::Rotation2D rot_phi(phi); - - Eigen::Vector2d area_origin; - if (cfg_->hcp.roadmap_graph_area_length_scale != 1.0) - area_origin = start.position() + 0.5*(1.0-cfg_->hcp.roadmap_graph_area_length_scale)*start_goal_dist*diff.normalized() - 0.5*area_width*normal; // bottom left corner of the origin - else - area_origin = start.position() - 0.5*area_width*normal; // bottom left corner of the origin - - // Insert Vertices - HcGraphVertexType start_vtx = boost::add_vertex(graph_); // start vertex - graph_[start_vtx].pos = start.position(); - diff.normalize(); // normalize in place - - - // Start sampling - for (int i=0; i < cfg_->hcp.roadmap_graph_no_samples; ++i) - { - Eigen::Vector2d sample; -// bool coll_free; -// do // sample as long as a collision free sample is found -// { - // Sample coordinates - sample = area_origin + rot_phi*Eigen::Vector2d(distribution_x(rnd_generator_), distribution_y(rnd_generator_)); - - // Test for collision - // we do not care for collision checking here to improve efficiency, since we perform resampling repeatedly. - // occupied vertices are ignored in the edge insertion state since they always violate the edge-obstacle collision check. -// coll_free = true; -// for (ObstContainer::const_iterator it_obst = obstacles_->begin(); it_obst != obstacles_->end(); ++it_obst) -// { -// if ( (*it_obst)->checkCollision(sample, dist_to_obst)) // TODO really keep dist_to_obst here? -// { -// coll_free = false; -// break; -// } -// } -// -// } while (!coll_free && ros::ok()); - - // Add new vertex - HcGraphVertexType v = boost::add_vertex(graph_); - graph_[v].pos = sample; - } - - // Now add goal vertex - HcGraphVertexType goal_vtx = boost::add_vertex(graph_); // goal vertex - graph_[goal_vtx].pos = goal.position(); - - - // Insert Edges - HcGraphVertexIterator it_i, end_i, it_j, end_j; - for (boost::tie(it_i,end_i) = boost::vertices(graph_); it_i!=boost::prior(end_i); ++it_i) // ignore goal in this loop - { - for (boost::tie(it_j,end_j) = boost::vertices(graph_); it_j!=end_j; ++it_j) // check all forward connections - { - if (it_i==it_j) // same vertex found - continue; - - Eigen::Vector2d distij = graph_[*it_j].pos-graph_[*it_i].pos; - distij.normalize(); // normalize in place - - // Check if the direction is backwards: - if (distij.dot(diff)<=obstacle_heading_threshold) - continue; // diff is already normalized - - - // Collision Check - bool collision = false; - for (ObstContainer::const_iterator it_obst = hcp_->obstacles()->begin(); it_obst != hcp_->obstacles()->end(); ++it_obst) - { - if ( (*it_obst)->checkLineIntersection(graph_[*it_i].pos,graph_[*it_j].pos, dist_to_obst) ) - { - collision = true; - break; - } - } - if (collision) - continue; - - // Create Edge - boost::add_edge(*it_i,*it_j,graph_); - } - } - - /// Find all paths between start and goal! - std::vector visited; - visited.push_back(start_vtx); - DepthFirst(graph_,visited,goal_vtx, start.theta(), goal.theta(), start_velocity, free_goal_vel); -} - -} // end namespace diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/homotopy_class_planner.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/homotopy_class_planner.cpp deleted file mode 100644 index 6fbf995..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/homotopy_class_planner.cpp +++ /dev/null @@ -1,854 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/homotopy_class_planner.h" - -#include - -namespace teb_local_planner -{ - -HomotopyClassPlanner::HomotopyClassPlanner() : cfg_(NULL), obstacles_(NULL), via_points_(NULL), robot_model_(new PointRobotFootprint()), initial_plan_(NULL), initialized_(false) -{ -} - -HomotopyClassPlanner::HomotopyClassPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, - TebVisualizationPtr visual, const ViaPointContainer* via_points) : initial_plan_(NULL) -{ - initialize(node, cfg, obstacles, visual, via_points); -} - -HomotopyClassPlanner::~HomotopyClassPlanner() -{ -} - -void HomotopyClassPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, - TebVisualizationPtr visual, const ViaPointContainer* via_points) -{ - node_ = node; - cfg_ = &cfg; - obstacles_ = obstacles; - via_points_ = via_points; - - if (cfg_->hcp.simple_exploration) - graph_search_ = std::shared_ptr(new lrKeyPointGraph(*cfg_, this)); - else - graph_search_ = std::shared_ptr(new ProbRoadmapGraph(*cfg_, this)); - - std::random_device rd; - random_.seed(rd()); - - // This is needed to prevent different time sources error - last_eq_class_switching_time_ = rclcpp::Time(0, 0, node->get_clock()->get_clock_type()); - - initialized_ = true; - - setVisualization(visual); -} - -void HomotopyClassPlanner::setVisualization(const TebVisualizationPtr& visualization) -{ - visualization_ = visualization; -} - - - -bool HomotopyClassPlanner::plan(const std::vector& initial_plan, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel) -{ - TEB_ASSERT_MSG(initialized_, "Call initialize() first."); - - // store initial plan for further initializations (must be valid for the lifetime of this object or clearPlanner() is called!) - initial_plan_ = &initial_plan; - - PoseSE2 start(initial_plan.front().pose); - PoseSE2 goal(initial_plan.back().pose); - - return plan(start, goal, start_vel, free_goal_vel); -} - -// tf2 doesn't have tf::Pose -//bool HomotopyClassPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel) -//{ -// TEB_ASSERT_MSG(initialized_, "Call initialize() first."); -// PoseSE2 start_pose(start); -// PoseSE2 goal_pose(goal); -// return plan(start_pose, goal_pose, start_vel, free_goal_vel); -//} - -bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel) -{ - TEB_ASSERT_MSG(initialized_, "Call initialize() first."); - - // Update old TEBs with new start, goal and velocity - updateAllTEBs(&start, &goal, start_vel); - - // Init new TEBs based on newly explored homotopy classes - exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel, free_goal_vel); - // update via-points if activated - updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates); - // Optimize all trajectories in alternative homotopy classes - optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); - // Select which candidate (based on alternative homotopy classes) should be used - selectBestTeb(); - - initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature); - return true; -} - -bool HomotopyClassPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const -{ - TebOptimalPlannerConstPtr best_teb = bestTeb(); - if (!best_teb) - { - vx = 0; - vy = 0; - omega = 0; - return false; - } - - return best_teb->getVelocityCommand(vx, vy, omega, look_ahead_poses); -} - - - - -void HomotopyClassPlanner::visualize() -{ - if (visualization_) - { - // Visualize graph - if (cfg_->hcp.visualize_hc_graph && graph_search_) - visualization_->publishGraph(graph_search_->graph_); - - // Visualize active tebs as marker - visualization_->publishTebContainer(tebs_); - - // Visualize best teb and feedback message if desired - TebOptimalPlannerConstPtr best_teb = bestTeb(); - if (best_teb) - { - visualization_->publishLocalPlanAndPoses(best_teb->teb()); - - if (best_teb->teb().sizePoses() > 0) //TODO maybe store current pose (start) within plan method as class field. - visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *cfg_->robot_model); - - // feedback message - if (cfg_->trajectory.publish_feedback) - { - int best_idx = bestTebIdx(); - if (best_idx>=0) - visualization_->publishFeedbackMessage(tebs_, (unsigned int) best_idx, *obstacles_); - } - } - } - else RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before."); -} - - - -bool HomotopyClassPlanner::hasEquivalenceClass(const EquivalenceClassPtr& eq_class) const -{ - // iterate existing h-signatures and check if there is an existing H-Signature similar the candidate - for (const std::pair& eqrel : equivalence_classes_) - { - if (eq_class->isEqual(*eqrel.first)) - return true; // Found! Homotopy class already exists, therefore nothing added - } - return false; -} - -bool HomotopyClassPlanner::addEquivalenceClassIfNew(const EquivalenceClassPtr& eq_class, bool lock) -{ - if (!eq_class) - return false; - - if (!eq_class->isValid()) - { - RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner: Ignoring invalid H-signature"); - return false; - } - - if (hasEquivalenceClass(eq_class)) - { - // Allow up to configured number of Tebs that are in the same homotopy - // class as the current (best) Teb to avoid being stuck in a local minimum - if (!isInBestTebClass(eq_class) || numTebsInBestTebClass() >= cfg_->hcp.max_number_plans_in_current_class) - return false; - } - - // Homotopy class not found -> Add to class-list, return that the h-signature is new - equivalence_classes_.push_back(std::make_pair(eq_class,lock)); - return true; -} - - -void HomotopyClassPlanner::renewAndAnalyzeOldTebs(bool delete_detours) -{ - // clear old h-signatures (since they could be changed due to new obstacle positions. - equivalence_classes_.clear(); - - // Adding the equivalence class of the latest best_teb_ first - TebOptPlannerContainer::iterator it_best_teb = best_teb_ ? std::find(tebs_.begin(), tebs_.end(), best_teb_) : tebs_.end(); - bool has_best_teb = it_best_teb != tebs_.end(); - if (has_best_teb) - { - std::iter_swap(tebs_.begin(), it_best_teb); // Putting the last best teb at the beginning of the container - best_teb_eq_class_ = calculateEquivalenceClass(best_teb_->teb().poses().begin(), - best_teb_->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_, - best_teb_->teb().timediffs().begin(), best_teb_->teb().timediffs().end()); - addEquivalenceClassIfNew(best_teb_eq_class_); - } - // Collect h-signatures for all existing TEBs and store them together with the corresponding iterator / pointer: -// typedef std::list< std::pair > > TebCandidateType; -// TebCandidateType teb_candidates; - - // get new homotopy classes and delete multiple TEBs per homotopy class. Skips the best teb if available (added before). - TebOptPlannerContainer::iterator it_teb = has_best_teb ? std::next(tebs_.begin(), 1) : tebs_.begin(); - while(it_teb != tebs_.end()) - { - // calculate equivalence class for the current candidate - EquivalenceClassPtr equivalence_class = calculateEquivalenceClass(it_teb->get()->teb().poses().begin(), it_teb->get()->teb().poses().end(), getCplxFromVertexPosePtr , obstacles_, - it_teb->get()->teb().timediffs().begin(), it_teb->get()->teb().timediffs().end()); - -// teb_candidates.push_back(std::make_pair(it_teb,H)); - - // WORKAROUND until the commented code below works - // Here we do not compare cost values. Just first come first serve... - bool new_flag = addEquivalenceClassIfNew(equivalence_class); - if (!new_flag) - { - it_teb = tebs_.erase(it_teb); - continue; - } - - ++it_teb; - } - if(delete_detours) - deletePlansDetouringBackwards(cfg_->hcp.detours_orientation_tolerance, cfg_->hcp.length_start_orientation_vector); - - // Find multiple candidates and delete the one with higher cost - // TODO: this code needs to be adpated. Erasing tebs from the teb container_ could make iteratores stored in the candidate list invalid! -// TebCandidateType::reverse_iterator cand_i = teb_candidates.rbegin(); -// int test_idx = 0; -// while (cand_i != teb_candidates.rend()) -// { -// -// TebCandidateType::reverse_iterator cand_j = std::find_if(boost::next(cand_i),teb_candidates.rend(), boost::bind(compareH,_1,cand_i->second)); -// if (cand_j != teb_candidates.rend() && cand_j != cand_i) -// { -// TebOptimalPlannerPtr pt1 = *(cand_j->first); -// TebOptimalPlannerPtr pt2 = *(cand_i->first); -// assert(pt1); -// assert(pt2); -// if ( cand_j->first->get()->getCurrentCost().sum() > cand_i->first->get()->getCurrentCost().sum() ) -// { -// // found one that has higher cost, therefore erase cand_j -// tebs_.erase(cand_j->first); -// teb_candidates.erase(cand_j); -// } -// else // otherwise erase cand_i -// { -// tebs_.erase(cand_i->first); -// cand_i = teb_candidates.erase(cand_i); -// } -// } -// else -// { -// ROS_WARN_STREAM("increase cand_i"); -// ++cand_i; -// } -// } - - // now add the h-signatures to the internal lookup-table (but only if there is no existing duplicate) -// for (TebCandidateType::iterator cand=teb_candidates.begin(); cand!=teb_candidates.end(); ++cand) -// { -// bool new_flag = addNewHSignatureIfNew(cand->second, cfg_->hcp.h_signature_threshold); -// if (!new_flag) -// { -// // ROS_ERROR_STREAM("getAndFilterHomotopyClassesTEB() - This schould not be happen."); -// tebs_.erase(cand->first); -// } -// } - -} - -void HomotopyClassPlanner::updateReferenceTrajectoryViaPoints(bool all_trajectories) -{ - if ( (!all_trajectories && !initial_plan_) || !via_points_ || via_points_->empty() || cfg_->optim.weight_viapoint <= 0) - return; - - if(equivalence_classes_.size() < tebs_.size()) - { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner::updateReferenceTrajectoryWithViaPoints(): Number of h-signatures does not match number of trajectories."); - return; - } - - if (all_trajectories) - { - // enable via-points for all tebs - for (std::size_t i=0; i < equivalence_classes_.size(); ++i) - { - tebs_[i]->setViaPoints(via_points_); - } - } - else - { - // enable via-points for teb in the same hommotopy class as the initial_plan and deactivate it for all other ones - for (std::size_t i=0; i < equivalence_classes_.size(); ++i) - { - if(initial_plan_eq_class_->isEqual(*equivalence_classes_[i].first)) - tebs_[i]->setViaPoints(via_points_); - else - tebs_[i]->setViaPoints(NULL); - } - } -} - - -void HomotopyClassPlanner::exploreEquivalenceClassesAndInitTebs(const PoseSE2& start, const PoseSE2& goal, double dist_to_obst, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel) -{ - // first process old trajectories - renewAndAnalyzeOldTebs(cfg_->hcp.delete_detours_backwards); - randomlyDropTebs(); - - // inject initial plan if available and not yet captured - if (initial_plan_) - { - initial_plan_teb_ = addAndInitNewTeb(*initial_plan_, start_vel, free_goal_vel); - } - else - { - initial_plan_teb_.reset(); - initial_plan_teb_ = getInitialPlanTEB(); // this method searches for initial_plan_eq_class_ in the teb container (-> if !initial_plan_teb_) - } - - // now explore new homotopy classes and initialize tebs if new ones are found. The appropriate createGraph method is chosen via polymorphism. - graph_search_->createGraph(start,goal,dist_to_obst,cfg_->hcp.obstacle_heading_threshold, start_vel, free_goal_vel); -} - - -TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel) -{ - if(tebs_.size() >= cfg_->hcp.max_number_classes) - return TebOptimalPlannerPtr(); - TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_)); - - candidate->teb().initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); - - if (start_velocity) - candidate->setVelocityStart(*start_velocity); - - EquivalenceClassPtr H = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_, - candidate->teb().timediffs().begin(), candidate->teb().timediffs().end()); - - if (free_goal_vel) - candidate->setVelocityGoalFree(); - - if(addEquivalenceClassIfNew(H)) - { - tebs_.push_back(candidate); - return tebs_.back(); - } - - // If the candidate constitutes no new equivalence class, return a null pointer - return TebOptimalPlannerPtr(); -} - - -bool HomotopyClassPlanner::isInBestTebClass(const EquivalenceClassPtr& eq_class) const -{ - bool answer = false; - if (best_teb_eq_class_) - answer = best_teb_eq_class_->isEqual(*eq_class); - return answer; -} - -int HomotopyClassPlanner::numTebsInClass(const EquivalenceClassPtr& eq_class) const -{ - int count = 0; - for (const std::pair& eqrel : equivalence_classes_) - { - if (eq_class->isEqual(*eqrel.first)) - ++count; - } - return count; -} - -int HomotopyClassPlanner::numTebsInBestTebClass() const -{ - int count = 0; - if (best_teb_eq_class_) - count = numTebsInClass(best_teb_eq_class_); - return count; -} - -TebOptimalPlannerPtr HomotopyClassPlanner::addAndInitNewTeb(const std::vector& initial_plan, const geometry_msgs::msg::Twist* start_velocity, bool free_goal_vel) -{ - if(tebs_.size() >= cfg_->hcp.max_number_classes) - return TebOptimalPlannerPtr(); - TebOptimalPlannerPtr candidate = TebOptimalPlannerPtr( new TebOptimalPlanner(node_, *cfg_, obstacles_, visualization_)); - - candidate->teb().initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, - cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); - - if (start_velocity) - candidate->setVelocityStart(*start_velocity); - - if (free_goal_vel) - candidate->setVelocityGoalFree(); - - // store the h signature of the initial plan to enable searching a matching teb later. - initial_plan_eq_class_ = calculateEquivalenceClass(candidate->teb().poses().begin(), candidate->teb().poses().end(), getCplxFromVertexPosePtr, obstacles_, - candidate->teb().timediffs().begin(), candidate->teb().timediffs().end()); - - if(addEquivalenceClassIfNew(initial_plan_eq_class_, true)) // also prevent candidate from deletion - { - tebs_.push_back(candidate); - return tebs_.back(); - } - - // If the candidate constitutes no new equivalence class, return a null pointer - return TebOptimalPlannerPtr(); -} - -void HomotopyClassPlanner::updateAllTEBs(const PoseSE2* start, const PoseSE2* goal, const geometry_msgs::msg::Twist* start_velocity) -{ - // If new goal is too far away, clear all existing trajectories to let them reinitialize later. - // Since all Tebs are sharing the same fixed goal pose, just take the first candidate: - if (!tebs_.empty() - && ((goal->position() - tebs_.front()->teb().BackPose().position()).norm() >= cfg_->trajectory.force_reinit_new_goal_dist - || fabs(g2o::normalize_theta(goal->theta() - tebs_.front()->teb().BackPose().theta())) >= cfg_->trajectory.force_reinit_new_goal_angular)) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); - tebs_.clear(); - equivalence_classes_.clear(); - } - - // hot-start from previous solutions - for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) - { - it_teb->get()->teb().updateAndPruneTEB(*start, *goal); - if (start_velocity) - it_teb->get()->setVelocityStart(*start_velocity); - } -} - - -void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop) -{ - // optimize TEBs in parallel since they are independend of each other - if (cfg_->hcp.enable_multithreading) - { - std::vector teb_threads; - - for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) - { - auto functor = [&, it_teb]() { - it_teb->get()->optimizeTEB(iter_innerloop, iter_outerloop, - true, cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale, - cfg_->hcp.selection_alternative_time_cost); - }; - - teb_threads.emplace_back(functor); - } - - for(auto & thread : teb_threads) { - if(thread.joinable()) { - thread.join(); - } - } - } - else - { - for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) - { - it_teb->get()->optimizeTEB(iter_innerloop,iter_outerloop, true, cfg_->hcp.selection_obst_cost_scale, - cfg_->hcp.selection_viapoint_cost_scale, cfg_->hcp.selection_alternative_time_cost); // compute cost as well inside optimizeTEB (last argument = true) - } - } -} - -TebOptimalPlannerPtr HomotopyClassPlanner::getInitialPlanTEB() -{ - // first check stored teb object - if (initial_plan_teb_) - { - // check if the teb is still part of the teb container - if ( std::find(tebs_.begin(), tebs_.end(), initial_plan_teb_ ) != tebs_.end() ) - return initial_plan_teb_; - else - { - initial_plan_teb_.reset(); // reset pointer for next call - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "initial teb not found, trying to find a match according to the cached equivalence class"); - } - } - - // reset the locked state for equivalence classes // TODO: this might be adapted if not only the plan containing the initial plan is locked! - for (int i=0; iisValid()) - { - if (equivalence_classes_.size() == tebs_.size()) - { - for (int i=0; iisEqual(*initial_plan_eq_class_)) - { - equivalence_classes_[i].second = true; - return tebs_[i]; - } - } - } - else - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), - "HomotopyClassPlanner::getInitialPlanTEB(): number of equivalence classes (%lu) and number of trajectories (%lu) does not match.", equivalence_classes_.size(), tebs_.size()); - } - else - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "HomotopyClassPlanner::getInitialPlanTEB(): initial TEB not found in the set of available trajectories."); - - return TebOptimalPlannerPtr(); -} - -void HomotopyClassPlanner::randomlyDropTebs() -{ - if (cfg_->hcp.selection_dropping_probability == 0.0) - { - return; - } - // interate both vectors in parallel - auto it_eqrel = equivalence_classes_.begin(); - auto it_teb = tebs_.begin(); - while (it_teb != tebs_.end() && it_eqrel != equivalence_classes_.end()) - { - if (it_teb->get() != best_teb_.get() // Always preserve the "best" teb - && (random_() <= cfg_->hcp.selection_dropping_probability * random_.max())) - { - it_teb = tebs_.erase(it_teb); - it_eqrel = equivalence_classes_.erase(it_eqrel); - } - else - { - ++it_teb; - ++it_eqrel; - } - } -} - -TebOptimalPlannerPtr HomotopyClassPlanner::selectBestTeb() -{ - double min_cost = std::numeric_limits::max(); // maximum cost - double min_cost_last_best = std::numeric_limits::max(); - double min_cost_initial_plan_teb = std::numeric_limits::max(); - TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB(); - - // check if last best_teb is still a valid candidate - if (best_teb_ && std::find(tebs_.begin(), tebs_.end(), best_teb_) != tebs_.end()) - { - // get cost of this candidate - min_cost_last_best = best_teb_->getCurrentCost() * cfg_->hcp.selection_cost_hysteresis; // small hysteresis - last_best_teb_ = best_teb_; - } - else - { - last_best_teb_.reset(); - } - - if (initial_plan_teb) // the validity was already checked in getInitialPlanTEB() - { - // get cost of this candidate - min_cost_initial_plan_teb = initial_plan_teb->getCurrentCost() * cfg_->hcp.selection_prefer_initial_plan; // small hysteresis - } - - - best_teb_.reset(); // reset pointer - - for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) - { - // check if the related TEB leaves the local costmap region -// if (tebs_.size()>1 && !(*it_teb)->teb().isTrajectoryInsideRegion(20, -1, 1)) -// { -// ROS_INFO("HomotopyClassPlanner::selectBestTeb(): skipping trajectories that are not inside the local costmap"); -// continue; -// } - - double teb_cost; - - if (*it_teb == last_best_teb_) - teb_cost = min_cost_last_best; // skip already known cost value of the last best_teb - else if (*it_teb == initial_plan_teb) - teb_cost = min_cost_initial_plan_teb; - else - teb_cost = it_teb->get()->getCurrentCost(); - - if (teb_cost < min_cost) - { - // check if this candidate is currently not selected - best_teb_ = *it_teb; - min_cost = teb_cost; - } - } - - - // in case we haven't found any teb due to some previous checks, investigate list again -// if (!best_teb_ && !tebs_.empty()) -// { -// RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "all " << tebs_.size() << " tebs rejected previously"); -// if (tebs_.size()==1) -// best_teb_ = tebs_.front(); -// else // if multiple TEBs are available: -// { -// // try to use the one that relates to the initial plan -// TebOptimalPlannerPtr initial_plan_teb = getInitialPlanTEB(); -// if (initial_plan_teb) -// best_teb_ = initial_plan_teb; -// else -// { -// // now compute the cost for the rest (we haven't computed it before) -// for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) -// { -// double teb_cost = it_teb->get()->getCurrentCost(); -// if (teb_cost < min_cost) -// { -// // check if this candidate is currently not selected -// best_teb_ = *it_teb; -// min_cost = teb_cost; -// } -// } -// } -// } -// } - - // check if we are allowed to change - if (last_best_teb_ && best_teb_ != last_best_teb_) - { - rclcpp::Time now = node_->now(); - if ((now - last_eq_class_switching_time_).seconds() > cfg_->hcp.switching_blocking_period) - { - last_eq_class_switching_time_ = now; - } - else - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "HomotopyClassPlanner::selectBestTeb(): Switching equivalence classes blocked (check parameter switching_blocking_period."); - // block switching, so revert best_teb_ - best_teb_ = last_best_teb_; - } - - } - - - return best_teb_; -} - -int HomotopyClassPlanner::bestTebIdx() const -{ - if (tebs_.size() == 1) - return 0; - - if (!best_teb_) - return -1; - - int idx = 0; - for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb, ++idx) - { - if (*it_teb == best_teb_) - return idx; - } - return -1; -} - -bool HomotopyClassPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector& footprint_spec, - double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance) -{ - bool feasible = false; - while(rclcpp::ok() && !feasible && tebs_.size() > 0) - { - TebOptimalPlannerPtr best = findBestTeb(); - if (!best) - { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Couldn't retrieve the best plan"); - return false; - } - feasible = best->isTrajectoryFeasible(costmap_model, footprint_spec, inscribed_radius, circumscribed_radius, look_ahead_idx, feasibility_check_lookahead_distance); - if(!feasible) - { - removeTeb(best); - if(last_best_teb_ && (last_best_teb_ == best)) // Same plan as before. - return feasible; // Not failing could result in oscillations between trajectories. - } - } - return feasible; -} - -TebOptimalPlannerPtr HomotopyClassPlanner::findBestTeb() -{ - if(tebs_.empty()) - return TebOptimalPlannerPtr(); - if (!best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end()) - best_teb_ = selectBestTeb(); - return best_teb_; -} - -TebOptPlannerContainer::iterator HomotopyClassPlanner::removeTeb(TebOptimalPlannerPtr& teb) -{ - TebOptPlannerContainer::iterator return_iterator = tebs_.end(); - if(equivalence_classes_.size() != tebs_.size()) - { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "removeTeb: size of eq classes != size of tebs"); - return return_iterator; - } - auto it_eq_classes = equivalence_classes_.begin(); - for(auto it = tebs_.begin(); it != tebs_.end(); ++it) - { - if(*it == teb) - { - return_iterator = tebs_.erase(it); - equivalence_classes_.erase(it_eq_classes); - break; - } - ++it_eq_classes; - } - return return_iterator; -} - -void HomotopyClassPlanner::setPreferredTurningDir(RotType dir) -{ - // set preferred turning dir for all TEBs - for (TebOptPlannerContainer::const_iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) - { - (*it_teb)->setPreferredTurningDir(dir); - } -} - -bool HomotopyClassPlanner::hasDiverged() const -{ - // Early return if there is no best trajectory initialized - if (!best_teb_) - return false; - - return best_teb_->hasDiverged(); -} - -void HomotopyClassPlanner::computeCurrentCost(std::vector& cost, double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) -{ - for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb) - { - it_teb->get()->computeCurrentCost(cost, obst_cost_scale, viapoint_cost_scale, alternative_time_cost); - } -} - -void HomotopyClassPlanner::deletePlansDetouringBackwards(const double orient_threshold, - const double len_orientation_vector) -{ - if (tebs_.size() < 2 || !best_teb_ || std::find(tebs_.begin(), tebs_.end(), best_teb_) == tebs_.end() || - best_teb_->teb().sizePoses() < 2) - { - return; // A moving direction wasn't chosen yet - } - double current_movement_orientation; - double best_plan_duration = std::max(best_teb_->teb().getSumOfAllTimeDiffs(), 1.0); - if(!computeStartOrientation(best_teb_, len_orientation_vector, current_movement_orientation)) - return; // The plan is shorter than len_orientation_vector - for(auto it_teb = tebs_.begin(); it_teb != tebs_.end();) - { - if(*it_teb == best_teb_) // The current plan should not be considered a detour - { - ++it_teb; - continue; - } - if((*it_teb)->teb().sizePoses() < 2) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Discarding a plan with less than 2 poses"); - it_teb = removeTeb(*it_teb); - continue; - } - double plan_orientation; - if(!computeStartOrientation(*it_teb, len_orientation_vector, plan_orientation)) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Failed to compute the start orientation for one of the tebs, likely close to the target"); - it_teb = removeTeb(*it_teb); - continue; - } - if(fabs(g2o::normalize_theta(plan_orientation - current_movement_orientation)) > orient_threshold) - { - it_teb = removeTeb(*it_teb); // Plan detouring backwards - continue; - } - if(!it_teb->get()->isOptimized()) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Removing a teb because it's not optimized"); - it_teb = removeTeb(*it_teb); // Deletes tebs that cannot be optimized (last optim call failed) - continue; - } - if(it_teb->get()->teb().getSumOfAllTimeDiffs() / best_plan_duration > cfg_->hcp.max_ratio_detours_duration_best_duration) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Removing a teb because it's duration is much longer than that of the best teb"); - it_teb = removeTeb(*it_teb); - continue; - } - ++it_teb; - } -} - -bool HomotopyClassPlanner::computeStartOrientation(const TebOptimalPlannerPtr plan, const double len_orientation_vector, - double& orientation) -{ - VertexPose start_pose = plan->teb().Pose(0); - bool second_pose_found = false; - Eigen::Vector2d start_vector; - for(auto& pose : plan->teb().poses()) - { - start_vector = start_pose.position() - pose->position(); - if(start_vector.norm() > len_orientation_vector) - { - second_pose_found = true; - break; - } - } - if(!second_pose_found) // The current plan is too short to make assumptions on the start orientation - return false; - orientation = std::atan2(start_vector[1], start_vector[0]); - return true; -} - - -} // end namespace diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/obstacles.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/obstacles.cpp deleted file mode 100644 index 95253e3..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/obstacles.cpp +++ /dev/null @@ -1,216 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/obstacles.h" -// #include "teb_local_planner/misc.h" -#include -#include - -namespace teb_local_planner -{ - - -void PolygonObstacle::fixPolygonClosure() -{ - if (vertices_.size()<2) - return; - - if (vertices_.front().isApprox(vertices_.back())) - vertices_.pop_back(); -} - -void PolygonObstacle::calcCentroid() -{ - if (vertices_.empty()) - { - centroid_.setConstant(NAN); - RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), - "PolygonObstacle::calcCentroid(): number of vertices is empty. the resulting centroid is a vector of NANs."); - return; - } - - // if polygon is a point - if (noVertices()==1) - { - centroid_ = vertices_.front(); - return; - } - - // if polygon is a line: - if (noVertices()==2) - { - centroid_ = 0.5*(vertices_.front() + vertices_.back()); - return; - } - - // otherwise: - - centroid_.setZero(); - - // calculate centroid (see wikipedia http://de.wikipedia.org/wiki/Geometrischer_Schwerpunkt#Polygon) - double A = 0; // A = 0.5 * sum_0_n-1 (x_i * y_{i+1} - x_{i+1} * y_i) - for (int i=0; i < noVertices()-1; ++i) - { - A += vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1); - } - A += vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1); - A *= 0.5; - - if (A!=0) - { - for (int i=0; i < noVertices()-1; ++i) - { - double aux = (vertices_.at(i).coeffRef(0) * vertices_.at(i+1).coeffRef(1) - vertices_.at(i+1).coeffRef(0) * vertices_.at(i).coeffRef(1)); - centroid_ += ( vertices_.at(i) + vertices_.at(i+1) )*aux; - } - double aux = (vertices_.at(noVertices()-1).coeffRef(0) * vertices_.at(0).coeffRef(1) - vertices_.at(0).coeffRef(0) * vertices_.at(noVertices()-1).coeffRef(1)); - centroid_ += ( vertices_.at(noVertices()-1) + vertices_.at(0) )*aux; - centroid_ /= (6*A); - } - else // A == 0 -> all points are placed on a 'perfect' line - { - // seach for the two outer points of the line with the maximum distance inbetween - int i_cand = 0; - int j_cand = 0; - double max_dist = 0; - for (int i=0; i< noVertices(); ++i) - { - for (int j=i+1; j< noVertices(); ++j) // start with j=i+1 - { - double dist = (vertices_[j] - vertices_[i]).norm(); - if (dist > max_dist) - { - max_dist = dist; - i_cand = i; - j_cand = j; - } - } - } - // calc centroid of that line - centroid_ = 0.5*(vertices_[i_cand] + vertices_[j_cand]); - } -} - - - -Eigen::Vector2d PolygonObstacle::getClosestPoint(const Eigen::Vector2d& position) const -{ - // the polygon is a point - if (noVertices() == 1) - { - return vertices_.front(); - } - - if (noVertices() > 1) - { - - Eigen::Vector2d new_pt = closest_point_on_line_segment_2d(position, vertices_.at(0), vertices_.at(1)); - - if (noVertices() > 2) // real polygon and not a line - { - double dist = (new_pt-position).norm(); - Eigen::Vector2d closest_pt = new_pt; - - // check each polygon edge - for (int i=1; i -#include - -#include "teb_local_planner/optimal_planner.h" - -#include -// g2o custom edges and vertices for the TEB planner -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace teb_local_planner -{ - -// ============== Implementation =================== - -TebOptimalPlanner::TebOptimalPlanner() : cfg_(nullptr), obstacles_(NULL), via_points_(NULL), cost_(HUGE_VAL), prefer_rotdir_(RotType::none), - initialized_(false), optimized_(false) -{ -} - -TebOptimalPlanner::TebOptimalPlanner(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, TebVisualizationPtr visual, const ViaPointContainer* via_points) -{ - initialize(node, cfg, obstacles, visual, via_points); -} - -TebOptimalPlanner::~TebOptimalPlanner() -{ - clearGraph(); - // free dynamically allocated memory - //if (optimizer_) - // g2o::Factory::destroy(); - //g2o::OptimizationAlgorithmFactory::destroy(); - //g2o::HyperGraphActionLibrary::destroy(); -} - -void TebOptimalPlanner::initialize(nav2_util::LifecycleNode::SharedPtr node, const TebConfig& cfg, ObstContainer* obstacles, TebVisualizationPtr visual, const ViaPointContainer* via_points) -{ - node_ = node; - // init optimizer (set solver and block ordering settings) - optimizer_ = initOptimizer(); - - cfg_ = &cfg; - obstacles_ = obstacles; - via_points_ = via_points; - cost_ = HUGE_VAL; - prefer_rotdir_ = RotType::none; - - vel_start_.first = true; - vel_start_.second.linear.x = 0; - vel_start_.second.linear.y = 0; - vel_start_.second.angular.z = 0; - - vel_goal_.first = true; - vel_goal_.second.linear.x = 0; - vel_goal_.second.linear.y = 0; - vel_goal_.second.angular.z = 0; - initialized_ = true; - - setVisualization(visual); -} - -void TebOptimalPlanner::setVisualization(const TebVisualizationPtr& visualization) -{ - visualization_ = visualization; -} - -void TebOptimalPlanner::visualize() -{ - if (!visualization_) - return; - - visualization_->publishLocalPlanAndPoses(teb_); - - if (teb_.sizePoses() > 0) - visualization_->publishRobotFootprintModel(teb_.Pose(0), *cfg_->robot_model); - - if (cfg_->trajectory.publish_feedback) - visualization_->publishFeedbackMessage(*this, *obstacles_); - -} - -/* - * registers custom vertices and edges in g2o framework - */ -void TebOptimalPlanner::registerG2OTypes() -{ - g2o::Factory* factory = g2o::Factory::instance(); - factory->registerType("VERTEX_POSE", std::make_shared>()); - factory->registerType("VERTEX_TIMEDIFF", std::make_shared>()); - factory->registerType("EDGE_TIME_OPTIMAL", std::make_shared>()); - factory->registerType("EDGE_SHORTEST_PATH", std::make_shared>()); - factory->registerType("EDGE_VELOCITY", std::make_shared>()); - factory->registerType("EDGE_VELOCITY_HOLONOMIC", std::make_shared>()); - factory->registerType("EDGE_ACCELERATION", std::make_shared>()); - factory->registerType("EDGE_ACCELERATION_START", std::make_shared>()); - factory->registerType("EDGE_ACCELERATION_GOAL", std::make_shared>()); - factory->registerType("EDGE_ACCELERATION_HOLONOMIC", std::make_shared>()); - factory->registerType("EDGE_ACCELERATION_HOLONOMIC_START", std::make_shared>()); - factory->registerType("EDGE_ACCELERATION_HOLONOMIC_GOAL", std::make_shared>()); - factory->registerType("EDGE_KINEMATICS_DIFF_DRIVE", std::make_shared>()); - factory->registerType("EDGE_KINEMATICS_CARLIKE", std::make_shared>()); - factory->registerType("EDGE_OBSTACLE", std::make_shared>()); - factory->registerType("EDGE_INFLATED_OBSTACLE", std::make_shared>()); - factory->registerType("EDGE_DYNAMIC_OBSTACLE", std::make_shared>()); - factory->registerType("EDGE_VIA_POINT", std::make_shared>()); - factory->registerType("EDGE_PREFER_ROTDIR", std::make_shared>()); - return; -} - -/* - * initialize g2o optimizer. Set solver settings here. - * Return: pointer to new SparseOptimizer Object. - */ -std::shared_ptr TebOptimalPlanner::initOptimizer() -{ - // Call register_g2o_types once, even for multiple TebOptimalPlanner instances (thread-safe) - static std::once_flag flag; - std::call_once(flag, [this](){this->registerG2OTypes();}); - - // allocating the optimizer - std::shared_ptr optimizer = std::make_shared(); - auto linearSolver = std::make_unique(); // see typedef in optimization.h - linearSolver->setBlockOrdering(true); - auto blockSolver = std::make_unique(std::move(linearSolver)); - g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver)); - - optimizer->setAlgorithm(solver); - - optimizer->initMultiThreading(); // required for >Eigen 3.1 - - return optimizer; -} - - -bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards, - double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) -{ - if (cfg_->optim.optimization_activate==false) - return false; - - bool success = false; - optimized_ = false; - - double weight_multiplier = 1.0; - - // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles - // (which leads to better results in terms of x-y-t homotopy planning). - // however, we have not tested this mode intensively yet, so we keep - // the legacy fast mode as default until we finish our tests. - bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; - - for(int i=0; itrajectory.teb_autosize) - { - //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples); - teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode); - - } - - success = buildGraph(weight_multiplier); - if (!success) - { - clearGraph(); - return false; - } - success = optimizeGraph(iterations_innerloop, false); - if (!success) - { - clearGraph(); - return false; - } - optimized_ = true; - - if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration - computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); - - clearGraph(); - - weight_multiplier *= cfg_->optim.weight_adapt_factor; - } - - return true; -} - -void TebOptimalPlanner::setVelocityStart(const geometry_msgs::msg::Twist& vel_start) -{ - vel_start_.first = true; - vel_start_.second.linear.x = vel_start.linear.x; - vel_start_.second.linear.y = vel_start.linear.y; - vel_start_.second.angular.z = vel_start.angular.z; -} - -void TebOptimalPlanner::setVelocityGoal(const geometry_msgs::msg::Twist& vel_goal) -{ - vel_goal_.first = true; - vel_goal_.second = vel_goal; -} - -bool TebOptimalPlanner::plan(const std::vector& initial_plan, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel) -{ - TEB_ASSERT_MSG(initialized_, "Call initialize() first."); - if (!teb_.isInit()) - { - teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, - cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); - } - else // warm start - { - PoseSE2 start_(initial_plan.front().pose); - PoseSE2 goal_(initial_plan.back().pose); - if (teb_.sizePoses()>0 - && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist - && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start! - teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB - else // goal too far away -> reinit - { - RCLCPP_DEBUG(node_->get_logger(), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); - teb_.clearTimedElasticBand(); - teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, - cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); - } - } - if (start_vel) - setVelocityStart(*start_vel); - if (free_goal_vel) - setVelocityGoalFree(); - else - vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified) - - // now optimize - return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); -} - - -//bool TebOptimalPlanner::plan(const tf::Pose& start, const tf::Pose& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel) -//{ -// PoseSE2 start_(start); -// PoseSE2 goal_(goal); -// return plan(start_, goal_, start_vel); -//} - -bool TebOptimalPlanner::plan(const PoseSE2& start, const PoseSE2& goal, const geometry_msgs::msg::Twist* start_vel, bool free_goal_vel) -{ - TEB_ASSERT_MSG(initialized_, "Call initialize() first."); - if (!teb_.isInit()) - { - // init trajectory - teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); // 0 intermediate samples, but dt=1 -> autoResize will add more samples before calling first optimization - } - else // warm start - { - if (teb_.sizePoses() > 0 - && (goal.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist - && fabs(g2o::normalize_theta(goal.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start! - teb_.updateAndPruneTEB(start, goal, cfg_->trajectory.min_samples); - else // goal too far away -> reinit - { - RCLCPP_DEBUG(node_->get_logger(), "New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); - teb_.clearTimedElasticBand(); - teb_.initTrajectoryToGoal(start, goal, 0, cfg_->robot.max_vel_x, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); - } - } - if (start_vel) - setVelocityStart(*start_vel); - if (free_goal_vel) - setVelocityGoalFree(); - else - vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified) - - // now optimize - return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations, true); -} - - -bool TebOptimalPlanner::buildGraph(double weight_multiplier) -{ - if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) - { - RCLCPP_WARN(node_->get_logger(), "Cannot build graph, because it is not empty. Call graphClear()!"); - return false; - } - - optimizer_->setComputeBatchStatistics(cfg_->recovery.divergence_detection_enable); - - // add TEB vertices - AddTEBVertices(); - - // add Edges (local cost functions) - if (cfg_->obstacles.legacy_obstacle_association) - AddEdgesObstaclesLegacy(weight_multiplier); - else - AddEdgesObstacles(weight_multiplier); - - if (cfg_->obstacles.include_dynamic_obstacles) - AddEdgesDynamicObstacles(); - - AddEdgesViaPoints(); - - AddEdgesVelocity(); - - AddEdgesAcceleration(); - - AddEdgesTimeOptimal(); - - AddEdgesShortestPath(); - - if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0) - AddEdgesKinematicsDiffDrive(); // we have a differential drive robot - else - AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below. - - AddEdgesPreferRotDir(); - - if (cfg_->optim.weight_velocity_obstacle_ratio > 0) - AddEdgesVelocityObstacleRatio(); - - return true; -} - -bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after) -{ - if (cfg_->robot.max_vel_x<0.01) - { - RCLCPP_WARN(node_->get_logger(), "optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted..."); - if (clear_after) clearGraph(); - return false; - } - - if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples) - { - RCLCPP_WARN(node_->get_logger(), "optimizeGraph(): TEB is empty or has too less elements. Skipping optimization."); - if (clear_after) clearGraph(); - return false; - } - - optimizer_->setVerbose(cfg_->optim.optimization_verbose); - optimizer_->initializeOptimization(); - - int iter = optimizer_->optimize(no_iterations); - - // Save Hessian for visualization - // g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast (optimizer_->solver()); - // lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt"); - - if(!iter) - { - RCLCPP_ERROR(node_->get_logger(), "optimizeGraph(): Optimization failed! iter=%i", iter); - return false; - } - - if (clear_after) clearGraph(); - - return true; -} - -void TebOptimalPlanner::clearGraph() -{ - // clear optimizer states - if (optimizer_) - { - // we will delete all edges but keep the vertices. - // before doing so, we will delete the link from the vertices to the edges. - auto& vertices = optimizer_->vertices(); - for(auto& v : vertices) - v.second->edges().clear(); - - optimizer_->vertices().clear(); // necessary, because optimizer->clear deletes pointer-targets (therefore it deletes TEB states!) - optimizer_->clear(); - } -} - - - -void TebOptimalPlanner::AddTEBVertices() -{ - // add vertices to graph - RCLCPP_DEBUG_EXPRESSION(node_->get_logger(), cfg_->optim.optimization_verbose, "Adding TEB vertices ..."); - unsigned int id_counter = 0; // used for vertices ids - obstacles_per_vertex_.resize(teb_.sizePoses()); - auto iter_obstacle = obstacles_per_vertex_.begin(); - for (int i=0; isetId(id_counter++); - optimizer_->addVertex(teb_.PoseVertex(i)); - if (teb_.sizeTimeDiffs()!=0 && isetId(id_counter++); - optimizer_->addVertex(teb_.TimeDiffVertex(i)); - } - iter_obstacle->clear(); - (iter_obstacle++)->reserve(obstacles_->size()); - } -} - - -void TebOptimalPlanner::AddEdgesObstacles(double weight_multiplier) -{ - if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr ) - return; // if weight equals zero skip adding edges! - - - bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist; - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_obstacle * weight_multiplier); - - Eigen::Matrix information_inflated; - information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier; - information_inflated(1,1) = cfg_->optim.weight_inflation; - information_inflated(0,1) = information_inflated(1,0) = 0; - - auto iter_obstacle = obstacles_per_vertex_.begin(); - - auto create_edge = [inflated, &information, &information_inflated, this] (int index, const Obstacle* obstacle) { - if (inflated) - { - EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); - dist_bandpt_obst->setInformation(information_inflated); - dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obstacle); - optimizer_->addEdge(dist_bandpt_obst); - } - else - { - EdgeObstacle* dist_bandpt_obst = new EdgeObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); - dist_bandpt_obst->setInformation(information); - dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obstacle); - optimizer_->addEdge(dist_bandpt_obst); - }; - }; - - // iterate all teb points, skipping the last and, if the EdgeVelocityObstacleRatio edges should not be created, the first one too - const int first_vertex = cfg_->optim.weight_velocity_obstacle_ratio == 0 ? 1 : 0; - for (int i = first_vertex; i < teb_.sizePoses() - 1; ++i) - { - double left_min_dist = std::numeric_limits::max(); - double right_min_dist = std::numeric_limits::max(); - ObstaclePtr left_obstacle; - ObstaclePtr right_obstacle; - - const Eigen::Vector2d pose_orient = teb_.Pose(i).orientationUnitVec(); - - // iterate obstacles - for (const ObstaclePtr& obst : *obstacles_) - { - // we handle dynamic obstacles differently below - if(cfg_->obstacles.include_dynamic_obstacles && obst->isDynamic()) - continue; - - // calculate distance to robot model - double dist = cfg_->robot_model->calculateDistance(teb_.Pose(i), obst.get()); - - // force considering obstacle if really close to the current pose - if (dist < cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_force_inclusion_factor) - { - iter_obstacle->push_back(obst); - continue; - } - // cut-off distance - if (dist > cfg_->obstacles.min_obstacle_dist*cfg_->obstacles.obstacle_association_cutoff_factor) - continue; - - // determine side (left or right) and assign obstacle if closer than the previous one - if (cross2d(pose_orient, obst->getCentroid()) > 0) // left - { - if (dist < left_min_dist) - { - left_min_dist = dist; - left_obstacle = obst; - } - } - else - { - if (dist < right_min_dist) - { - right_min_dist = dist; - right_obstacle = obst; - } - } - } - - if (left_obstacle) - iter_obstacle->push_back(left_obstacle); - if (right_obstacle) - iter_obstacle->push_back(right_obstacle); - - // continue here to ignore obstacles for the first pose, but use them later to create the EdgeVelocityObstacleRatio edges - if (i == 0) - { - ++iter_obstacle; - continue; - } - - // create obstacle edges - for (const ObstaclePtr obst : *iter_obstacle) - create_edge(i, obst.get()); - ++iter_obstacle; - } -} - - -void TebOptimalPlanner::AddEdgesObstaclesLegacy(double weight_multiplier) -{ - if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==nullptr) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_obstacle * weight_multiplier); - - Eigen::Matrix information_inflated; - information_inflated(0,0) = cfg_->optim.weight_obstacle * weight_multiplier; - information_inflated(1,1) = cfg_->optim.weight_inflation; - information_inflated(0,1) = information_inflated(1,0) = 0; - - bool inflated = cfg_->obstacles.inflation_dist > cfg_->obstacles.min_obstacle_dist; - - for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst) - { - if (cfg_->obstacles.include_dynamic_obstacles && (*obst)->isDynamic()) // we handle dynamic obstacles differently below - continue; - - int index; - - if (cfg_->obstacles.obstacle_poses_affected >= teb_.sizePoses()) - index = teb_.sizePoses() / 2; - else - index = teb_.findClosestTrajectoryPose(*(obst->get())); - - - // check if obstacle is outside index-range between start and goal - if ( (index <= 1) || (index > teb_.sizePoses()-2) ) // start and goal are fixed and findNearestBandpoint finds first or last conf if intersection point is outside the range - continue; - - if (inflated) - { - EdgeInflatedObstacle* dist_bandpt_obst = new EdgeInflatedObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); - dist_bandpt_obst->setInformation(information_inflated); - dist_bandpt_obst->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst); - } - else - { - EdgeObstacle* dist_bandpt_obst = new EdgeObstacle; - dist_bandpt_obst->setVertex(0,teb_.PoseVertex(index)); - dist_bandpt_obst->setInformation(information); - dist_bandpt_obst->setParameters(*cfg_, cfg_->robot_model.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst); - } - - for (int neighbourIdx=0; neighbourIdx < floor(cfg_->obstacles.obstacle_poses_affected/2); neighbourIdx++) - { - if (index+neighbourIdx < teb_.sizePoses()) - { - if (inflated) - { - EdgeInflatedObstacle* dist_bandpt_obst_n_r = new EdgeInflatedObstacle; - dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx)); - dist_bandpt_obst_n_r->setInformation(information_inflated); - dist_bandpt_obst_n_r->setParameters(*cfg_, cfg_->robot_model.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_r); - } - else - { - EdgeObstacle* dist_bandpt_obst_n_r = new EdgeObstacle; - dist_bandpt_obst_n_r->setVertex(0,teb_.PoseVertex(index+neighbourIdx)); - dist_bandpt_obst_n_r->setInformation(information); - dist_bandpt_obst_n_r->setParameters(*cfg_, robot_model_.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_r); - } - } - if ( index - neighbourIdx >= 0) // needs to be casted to int to allow negative values - { - if (inflated) - { - EdgeInflatedObstacle* dist_bandpt_obst_n_l = new EdgeInflatedObstacle; - dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx)); - dist_bandpt_obst_n_l->setInformation(information_inflated); - dist_bandpt_obst_n_l->setParameters(*cfg_, cfg_->robot_model.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_l); - } - else - { - EdgeObstacle* dist_bandpt_obst_n_l = new EdgeObstacle; - dist_bandpt_obst_n_l->setVertex(0,teb_.PoseVertex(index-neighbourIdx)); - dist_bandpt_obst_n_l->setInformation(information); - dist_bandpt_obst_n_l->setParameters(*cfg_, cfg_->robot_model.get(), obst->get()); - optimizer_->addEdge(dist_bandpt_obst_n_l); - } - } - } - - } -} - - -void TebOptimalPlanner::AddEdgesDynamicObstacles(double weight_multiplier) -{ - if (cfg_->optim.weight_obstacle==0 || weight_multiplier==0 || obstacles_==NULL ) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information(0,0) = cfg_->optim.weight_dynamic_obstacle * weight_multiplier; - information(1,1) = cfg_->optim.weight_dynamic_obstacle_inflation; - information(0,1) = information(1,0) = 0; - - for (ObstContainer::const_iterator obst = obstacles_->begin(); obst != obstacles_->end(); ++obst) - { - if (!(*obst)->isDynamic()) - continue; - - // Skip first and last pose, as they are fixed - double time = teb_.TimeDiff(0); - for (int i=1; i < teb_.sizePoses() - 1; ++i) - { - EdgeDynamicObstacle* dynobst_edge = new EdgeDynamicObstacle(time); - dynobst_edge->setVertex(0,teb_.PoseVertex(i)); - dynobst_edge->setInformation(information); - dynobst_edge->setParameters(*cfg_, cfg_->robot_model.get(), obst->get()); - optimizer_->addEdge(dynobst_edge); - time += teb_.TimeDiff(i); // we do not need to check the time diff bounds, since we iterate to "< sizePoses()-1". - } - } -} - -void TebOptimalPlanner::AddEdgesViaPoints() -{ - if (cfg_->optim.weight_viapoint==0 || via_points_==NULL || via_points_->empty() ) - return; // if weight equals zero skip adding edges! - - int start_pose_idx = 0; - - int n = teb_.sizePoses(); - if (n<3) // we do not have any degrees of freedom for reaching via-points - return; - - for (ViaPointContainer::const_iterator vp_it = via_points_->begin(); vp_it != via_points_->end(); ++vp_it) - { - - int index = teb_.findClosestTrajectoryPose(*vp_it, NULL, start_pose_idx); - if (cfg_->trajectory.via_points_ordered) - start_pose_idx = index+2; // skip a point to have a DOF inbetween for further via-points - - // check if point conicides with goal or is located behind it - if ( index > n-2 ) - index = n-2; // set to a pose before the goal, since we can move it away! - // check if point coincides with start or is located before it - if ( index < 1) - { - if (cfg_->trajectory.via_points_ordered) - { - index = 1; // try to connect the via point with the second (and non-fixed) pose. It is likely that autoresize adds new poses inbetween later. - } - else - { - RCLCPP_DEBUG(node_->get_logger(), "TebOptimalPlanner::AddEdgesViaPoints(): skipping a via-point that is close or behind the current robot pose."); - continue; // skip via points really close or behind the current robot pose - } - } - Eigen::Matrix information; - information.fill(cfg_->optim.weight_viapoint); - - EdgeViaPoint* edge_viapoint = new EdgeViaPoint; - edge_viapoint->setVertex(0,teb_.PoseVertex(index)); - edge_viapoint->setInformation(information); - edge_viapoint->setParameters(*cfg_, &(*vp_it)); - optimizer_->addEdge(edge_viapoint); - } -} - -void TebOptimalPlanner::AddEdgesVelocity() -{ - if (cfg_->robot.max_vel_y == 0) // non-holonomic robot - { - if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_theta==0) - return; // if weight equals zero skip adding edges! - - int n = teb_.sizePoses(); - Eigen::Matrix information; - information(0,0) = cfg_->optim.weight_max_vel_x; - information(1,1) = cfg_->optim.weight_max_vel_theta; - information(0,1) = 0.0; - information(1,0) = 0.0; - - for (int i=0; i < n - 1; ++i) - { - EdgeVelocity* velocity_edge = new EdgeVelocity; - velocity_edge->setVertex(0,teb_.PoseVertex(i)); - velocity_edge->setVertex(1,teb_.PoseVertex(i+1)); - velocity_edge->setVertex(2,teb_.TimeDiffVertex(i)); - velocity_edge->setInformation(information); - velocity_edge->setTebConfig(*cfg_); - optimizer_->addEdge(velocity_edge); - } - } - else // holonomic-robot - { - if ( cfg_->optim.weight_max_vel_x==0 && cfg_->optim.weight_max_vel_y==0 && cfg_->optim.weight_max_vel_theta==0) - return; // if weight equals zero skip adding edges! - - int n = teb_.sizePoses(); - Eigen::Matrix information; - information.fill(0); - information(0,0) = cfg_->optim.weight_max_vel_x; - information(1,1) = cfg_->optim.weight_max_vel_y; - information(2,2) = cfg_->optim.weight_max_vel_theta; - - for (int i=0; i < n - 1; ++i) - { - EdgeVelocityHolonomic* velocity_edge = new EdgeVelocityHolonomic; - velocity_edge->setVertex(0,teb_.PoseVertex(i)); - velocity_edge->setVertex(1,teb_.PoseVertex(i+1)); - velocity_edge->setVertex(2,teb_.TimeDiffVertex(i)); - velocity_edge->setInformation(information); - velocity_edge->setTebConfig(*cfg_); - optimizer_->addEdge(velocity_edge); - } - - } -} - -void TebOptimalPlanner::AddEdgesAcceleration() -{ - if (cfg_->optim.weight_acc_lim_x==0 && cfg_->optim.weight_acc_lim_theta==0) - return; // if weight equals zero skip adding edges! - - int n = teb_.sizePoses(); - - if (cfg_->robot.max_vel_y == 0 || cfg_->robot.acc_lim_y == 0) // non-holonomic robot - { - Eigen::Matrix information; - information.fill(0); - information(0,0) = cfg_->optim.weight_acc_lim_x; - information(1,1) = cfg_->optim.weight_acc_lim_theta; - - // check if an initial velocity should be taken into accound - if (vel_start_.first) - { - EdgeAccelerationStart* acceleration_edge = new EdgeAccelerationStart; - acceleration_edge->setVertex(0,teb_.PoseVertex(0)); - acceleration_edge->setVertex(1,teb_.PoseVertex(1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0)); - acceleration_edge->setInitialVelocity(vel_start_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - - // now add the usual acceleration edge for each tuple of three teb poses - for (int i=0; i < n - 2; ++i) - { - EdgeAcceleration* acceleration_edge = new EdgeAcceleration; - acceleration_edge->setVertex(0,teb_.PoseVertex(i)); - acceleration_edge->setVertex(1,teb_.PoseVertex(i+1)); - acceleration_edge->setVertex(2,teb_.PoseVertex(i+2)); - acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i)); - acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - - // check if a goal velocity should be taken into accound - if (vel_goal_.first) - { - EdgeAccelerationGoal* acceleration_edge = new EdgeAccelerationGoal; - acceleration_edge->setVertex(0,teb_.PoseVertex(n-2)); - acceleration_edge->setVertex(1,teb_.PoseVertex(n-1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); - acceleration_edge->setGoalVelocity(vel_goal_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - } - else // holonomic robot - { - Eigen::Matrix information; - information.fill(0); - information(0,0) = cfg_->optim.weight_acc_lim_x; - information(1,1) = cfg_->optim.weight_acc_lim_y; - information(2,2) = cfg_->optim.weight_acc_lim_theta; - - // check if an initial velocity should be taken into accound - if (vel_start_.first) - { - EdgeAccelerationHolonomicStart* acceleration_edge = new EdgeAccelerationHolonomicStart; - acceleration_edge->setVertex(0,teb_.PoseVertex(0)); - acceleration_edge->setVertex(1,teb_.PoseVertex(1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex(0)); - acceleration_edge->setInitialVelocity(vel_start_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - - // now add the usual acceleration edge for each tuple of three teb poses - for (int i=0; i < n - 2; ++i) - { - EdgeAccelerationHolonomic* acceleration_edge = new EdgeAccelerationHolonomic; - acceleration_edge->setVertex(0,teb_.PoseVertex(i)); - acceleration_edge->setVertex(1,teb_.PoseVertex(i+1)); - acceleration_edge->setVertex(2,teb_.PoseVertex(i+2)); - acceleration_edge->setVertex(3,teb_.TimeDiffVertex(i)); - acceleration_edge->setVertex(4,teb_.TimeDiffVertex(i+1)); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - - // check if a goal velocity should be taken into accound - if (vel_goal_.first) - { - EdgeAccelerationHolonomicGoal* acceleration_edge = new EdgeAccelerationHolonomicGoal; - acceleration_edge->setVertex(0,teb_.PoseVertex(n-2)); - acceleration_edge->setVertex(1,teb_.PoseVertex(n-1)); - acceleration_edge->setVertex(2,teb_.TimeDiffVertex( teb_.sizeTimeDiffs()-1 )); - acceleration_edge->setGoalVelocity(vel_goal_.second); - acceleration_edge->setInformation(information); - acceleration_edge->setTebConfig(*cfg_); - optimizer_->addEdge(acceleration_edge); - } - } -} - - - -void TebOptimalPlanner::AddEdgesTimeOptimal() -{ - if (cfg_->optim.weight_optimaltime==0) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_optimaltime); - - for (int i=0; i < teb_.sizeTimeDiffs(); ++i) - { - EdgeTimeOptimal* timeoptimal_edge = new EdgeTimeOptimal; - timeoptimal_edge->setVertex(0,teb_.TimeDiffVertex(i)); - timeoptimal_edge->setInformation(information); - timeoptimal_edge->setTebConfig(*cfg_); - optimizer_->addEdge(timeoptimal_edge); - } -} - -void TebOptimalPlanner::AddEdgesShortestPath() -{ - if (cfg_->optim.weight_shortest_path==0) - return; // if weight equals zero skip adding edges! - - Eigen::Matrix information; - information.fill(cfg_->optim.weight_shortest_path); - - for (int i=0; i < teb_.sizePoses()-1; ++i) - { - EdgeShortestPath* shortest_path_edge = new EdgeShortestPath; - shortest_path_edge->setVertex(0,teb_.PoseVertex(i)); - shortest_path_edge->setVertex(1,teb_.PoseVertex(i+1)); - shortest_path_edge->setInformation(information); - shortest_path_edge->setTebConfig(*cfg_); - optimizer_->addEdge(shortest_path_edge); - } -} - - - -void TebOptimalPlanner::AddEdgesKinematicsDiffDrive() -{ - if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_forward_drive==0) - return; // if weight equals zero skip adding edges! - - // create edge for satisfiying kinematic constraints - Eigen::Matrix information_kinematics; - information_kinematics.fill(0.0); - information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh; - information_kinematics(1, 1) = cfg_->optim.weight_kinematics_forward_drive; - - for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only - { - EdgeKinematicsDiffDrive* kinematics_edge = new EdgeKinematicsDiffDrive; - kinematics_edge->setVertex(0,teb_.PoseVertex(i)); - kinematics_edge->setVertex(1,teb_.PoseVertex(i+1)); - kinematics_edge->setInformation(information_kinematics); - kinematics_edge->setTebConfig(*cfg_); - optimizer_->addEdge(kinematics_edge); - } -} - -void TebOptimalPlanner::AddEdgesKinematicsCarlike() -{ - if (cfg_->optim.weight_kinematics_nh==0 && cfg_->optim.weight_kinematics_turning_radius==0) - return; // if weight equals zero skip adding edges! - - // create edge for satisfiying kinematic constraints - Eigen::Matrix information_kinematics; - information_kinematics.fill(0.0); - information_kinematics(0, 0) = cfg_->optim.weight_kinematics_nh; - information_kinematics(1, 1) = cfg_->optim.weight_kinematics_turning_radius; - - for (int i=0; i < teb_.sizePoses()-1; i++) // ignore twiced start only - { - EdgeKinematicsCarlike* kinematics_edge = new EdgeKinematicsCarlike; - kinematics_edge->setVertex(0,teb_.PoseVertex(i)); - kinematics_edge->setVertex(1,teb_.PoseVertex(i+1)); - kinematics_edge->setInformation(information_kinematics); - kinematics_edge->setTebConfig(*cfg_); - optimizer_->addEdge(kinematics_edge); - } -} - - -void TebOptimalPlanner::AddEdgesPreferRotDir() -{ - //TODO(roesmann): Note, these edges can result in odd predictions, in particular - // we can observe a substantional mismatch between open- and closed-loop planning - // leading to a poor control performance. - // At the moment, we keep these functionality for oscillation recovery: - // Activating the edge for a short time period might not be crucial and - // could move the robot to a new oscillation-free state. - // This needs to be analyzed in more detail! - if (prefer_rotdir_ == RotType::none || cfg_->optim.weight_prefer_rotdir==0) - return; // if weight equals zero skip adding edges! - - if (prefer_rotdir_ != RotType::right && prefer_rotdir_ != RotType::left) - { - RCLCPP_WARN(node_->get_logger(), "TebOptimalPlanner::AddEdgesPreferRotDir(): unsupported RotType selected. Skipping edge creation."); - return; - } - - // create edge for satisfiying kinematic constraints - Eigen::Matrix information_rotdir; - information_rotdir.fill(cfg_->optim.weight_prefer_rotdir); - - for (int i=0; i < teb_.sizePoses()-1 && i < 3; ++i) // currently: apply to first 3 rotations - { - EdgePreferRotDir* rotdir_edge = new EdgePreferRotDir; - rotdir_edge->setVertex(0,teb_.PoseVertex(i)); - rotdir_edge->setVertex(1,teb_.PoseVertex(i+1)); - rotdir_edge->setInformation(information_rotdir); - - if (prefer_rotdir_ == RotType::left) - rotdir_edge->preferLeft(); - else if (prefer_rotdir_ == RotType::right) - rotdir_edge->preferRight(); - - optimizer_->addEdge(rotdir_edge); - } -} - -void TebOptimalPlanner::AddEdgesVelocityObstacleRatio() -{ - Eigen::Matrix information; - information(0,0) = cfg_->optim.weight_velocity_obstacle_ratio; - information(1,1) = cfg_->optim.weight_velocity_obstacle_ratio; - information(0,1) = information(1,0) = 0; - - auto iter_obstacle = obstacles_per_vertex_.begin(); - - for (int index = 0; index < teb_.sizePoses() - 1; ++index) - { - for (const ObstaclePtr obstacle : (*iter_obstacle++)) - { - EdgeVelocityObstacleRatio* edge = new EdgeVelocityObstacleRatio; - edge->setVertex(0,teb_.PoseVertex(index)); - edge->setVertex(1,teb_.PoseVertex(index + 1)); - edge->setVertex(2,teb_.TimeDiffVertex(index)); - edge->setInformation(information); - edge->setParameters(*cfg_, cfg_->robot_model.get(), obstacle.get()); - optimizer_->addEdge(edge); - } - } -} - -bool TebOptimalPlanner::hasDiverged() const -{ - // Early returns if divergence detection is not active - if (!cfg_->recovery.divergence_detection_enable) - return false; - - auto stats_vector = optimizer_->batchStatistics(); - - // No statistics yet - if (stats_vector.empty()) - return false; - - // Grab the statistics of the final iteration - const auto last_iter_stats = stats_vector.back(); - - return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared; -} - -void TebOptimalPlanner::computeCurrentCost(double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) -{ - // check if graph is empty/exist -> important if function is called between buildGraph and optimizeGraph/clearGraph - bool graph_exist_flag(false); - if (optimizer_->edges().empty() && optimizer_->vertices().empty()) - { - // here the graph is build again, for time efficiency make sure to call this function - // between buildGraph and Optimize (deleted), but it depends on the application - buildGraph(); - optimizer_->initializeOptimization(); - } - else - { - graph_exist_flag = true; - } - - optimizer_->computeInitialGuess(); - - cost_ = 0; - - if (alternative_time_cost) - { - cost_ += teb_.getSumOfAllTimeDiffs(); - // TEST we use SumOfAllTimeDiffs() here, because edge cost depends on number of samples, which is not always the same for similar TEBs, - // since we are using an AutoResize Function with hysteresis. - } - - // now we need pointers to all edges -> calculate error for each edge-type - // since we aren't storing edge pointers, we need to check every edge - for (std::vector::const_iterator it = optimizer_->activeEdges().begin(); it!= optimizer_->activeEdges().end(); it++) - { - double cur_cost = (*it)->chi2(); - - if (dynamic_cast(*it) != nullptr - || dynamic_cast(*it) != nullptr - || dynamic_cast(*it) != nullptr) - { - cur_cost *= obst_cost_scale; - } - else if (dynamic_cast(*it) != nullptr) - { - cur_cost *= viapoint_cost_scale; - } - else if (dynamic_cast(*it) != nullptr && alternative_time_cost) - { - continue; // skip these edges if alternative_time_cost is active - } - cost_ += cur_cost; - } - - // delete temporary created graph - if (!graph_exist_flag) - clearGraph(); -} - - -void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const -{ - if (dt == 0) - { - vx = 0; - vy = 0; - omega = 0; - return; - } - - Eigen::Vector2d deltaS = pose2.position() - pose1.position(); - - if (cfg_->robot.max_vel_y == 0) // nonholonomic robot - { - Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) ); - // translational velocity - double dir = deltaS.dot(conf1dir); - vx = (double) g2o::sign(dir) * deltaS.norm()/dt; - vy = 0; - } - else // holonomic robot - { - // transform pose 2 into the current robot frame (pose1) - // for velocities only the rotation of the direction vector is necessary. - // (map->pose1-frame: inverse 2d rotation matrix) - double cos_theta1 = std::cos(pose1.theta()); - double sin_theta1 = std::sin(pose1.theta()); - double p1_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y(); - double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y(); - vx = p1_dx / dt; - vy = p1_dy / dt; - } - - // rotational velocity - double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta()); - omega = orientdiff/dt; -} - -bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const -{ - if (teb_.sizePoses()<2) - { - RCLCPP_ERROR(node_->get_logger(), "TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init and optimize/plan the trajectory fist."); - vx = 0; - vy = 0; - omega = 0; - return false; - } - look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1)); - double dt = 0.0; - for(int counter = 0; counter < look_ahead_poses; ++counter) - { - dt += teb_.TimeDiff(counter); - if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory? - { - look_ahead_poses = counter + 1; - break; - } - } - if (dt<=0) - { - RCLCPP_ERROR(node_->get_logger(), "TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!"); - vx = 0; - vy = 0; - omega = 0; - return false; - } - - // Get velocity from the first two configurations - extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega); - return true; -} - -void TebOptimalPlanner::getVelocityProfile(std::vector& velocity_profile) const -{ - int n = teb_.sizePoses(); - velocity_profile.resize( n+1 ); - - // start velocity - velocity_profile.front().linear.z = 0; - velocity_profile.front().angular.x = velocity_profile.front().angular.y = 0; - velocity_profile.front().linear.x = vel_start_.second.linear.x; - velocity_profile.front().linear.y = vel_start_.second.linear.y; - velocity_profile.front().angular.z = vel_start_.second.angular.z; - - for (int i=1; i& trajectory) const -{ - int n = teb_.sizePoses(); - - trajectory.resize(n); - - if (n == 0) - return; - - double curr_time = 0; - - // start - teb_msgs::msg::TrajectoryPointMsg& start = trajectory.front(); - teb_.Pose(0).toPoseMsg(start.pose); - start.velocity.linear.z = 0; - start.velocity.angular.x = start.velocity.angular.y = 0; - start.velocity.linear.x = vel_start_.second.linear.x; - start.velocity.linear.y = vel_start_.second.linear.y; - start.velocity.angular.z = vel_start_.second.angular.z; - start.time_from_start = durationFromSec(curr_time); - - curr_time += teb_.TimeDiff(0); - - // intermediate points - for (int i=1; i < n-1; ++i) - { - teb_msgs::msg::TrajectoryPointMsg& point = trajectory[i]; - teb_.Pose(i).toPoseMsg(point.pose); - point.velocity.linear.z = 0; - point.velocity.angular.x = point.velocity.angular.y = 0; - double vel1_x, vel1_y, vel2_x, vel2_y, omega1, omega2; - extractVelocity(teb_.Pose(i-1), teb_.Pose(i), teb_.TimeDiff(i-1), vel1_x, vel1_y, omega1); - extractVelocity(teb_.Pose(i), teb_.Pose(i+1), teb_.TimeDiff(i), vel2_x, vel2_y, omega2); - point.velocity.linear.x = 0.5*(vel1_x+vel2_x); - point.velocity.linear.y = 0.5*(vel1_y+vel2_y); - point.velocity.angular.z = 0.5*(omega1+omega2); - point.time_from_start = durationFromSec(curr_time); - - curr_time += teb_.TimeDiff(i); - } - - // goal - teb_msgs::msg::TrajectoryPointMsg& goal = trajectory.back(); - teb_.BackPose().toPoseMsg(goal.pose); - goal.velocity.linear.z = 0; - goal.velocity.angular.x = goal.velocity.angular.y = 0; - goal.velocity.linear.x = vel_goal_.second.linear.x; - goal.velocity.linear.y = vel_goal_.second.linear.y; - goal.velocity.angular.z = vel_goal_.second.angular.z; - goal.time_from_start = durationFromSec(curr_time); -} - - -bool TebOptimalPlanner::isTrajectoryFeasible(dwb_critics::ObstacleFootprintCritic* costmap_model, const std::vector& footprint_spec, - double inscribed_radius, double circumscribed_radius, int look_ahead_idx, double feasibility_check_lookahead_distance) -{ - if (look_ahead_idx < 0 || look_ahead_idx >= teb().sizePoses()) - look_ahead_idx = teb().sizePoses() - 1; - - if (feasibility_check_lookahead_distance > 0){ - for (int i=1; i < teb().sizePoses(); ++i){ - double pose_distance=std::hypot(teb().Pose(i).x()-teb().Pose(0).x(), teb().Pose(i).y()-teb().Pose(0).y()); - if(pose_distance > feasibility_check_lookahead_distance){ - look_ahead_idx = i - 1; - break; - } - } - } - - geometry_msgs::msg::Pose2D pose2d; - for (int i=0; i <= look_ahead_idx; ++i) - { - teb().Pose(i).toPoseMsg(pose2d); - if (!isPoseValid(pose2d, costmap_model, footprint_spec)){ - if (visualization_) - { - visualization_->publishInfeasibleRobotPose(teb().Pose(i), *cfg_->robot_model); - } - return false; - } - // Checks if the distance between two poses is higher than the robot radius or the orientation diff is bigger than the specified threshold - // and interpolates in that case. - // (if obstacles are pushing two consecutive poses away, the center between two consecutive poses might coincide with the obstacle ;-)! - if (i cfg_->trajectory.min_resolution_collision_check_angular || delta_dist.norm() > inscribed_radius) - { - int n_additional_samples = std::max(std::ceil(fabs(delta_rot) / cfg_->trajectory.min_resolution_collision_check_angular), - std::ceil(delta_dist.norm() / inscribed_radius)) - 1; - PoseSE2 intermediate_pose = teb().Pose(i); - for(int step = 0; step < n_additional_samples; ++step) - { - intermediate_pose.position() = intermediate_pose.position() + delta_dist / (n_additional_samples + 1.0); - intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + - delta_rot / (n_additional_samples + 1.0)); - intermediate_pose.toPoseMsg(pose2d); - - if (!isPoseValid(pose2d, costmap_model, footprint_spec)){ - if (visualization_) - { - visualization_->publishInfeasibleRobotPose(intermediate_pose, *cfg_->robot_model); - } - return false; - } - } - } - } - } - return true; -} - -bool TebOptimalPlanner::isPoseValid(geometry_msgs::msg::Pose2D pose2d, dwb_critics::ObstacleFootprintCritic* costmap_model, - const std::vector& footprint_spec) -{ - try { - if ( costmap_model->scorePose(pose2d, dwb_critics::getOrientedFootprint(pose2d, footprint_spec)) < 0 ) { - return false; - } - } catch (...) { - return false; - } - return true; -} - -} // namespace teb_local_planner diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/recovery_behaviors.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/recovery_behaviors.cpp deleted file mode 100644 index eaeca56..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/recovery_behaviors.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/recovery_behaviors.h" -#include -#include -#include -#include -#include - -namespace teb_local_planner -{ - -// ============== FailureDetector Implementation =================== - -void FailureDetector::update(const geometry_msgs::msg::Twist& twist, double v_max, double v_backwards_max, double omega_max, double v_eps, double omega_eps) -{ - if (buffer_.capacity() == 0) - return; - - VelMeasurement measurement; - measurement.v = twist.linear.x; // just consider linear velocity in x-direction in the robot frame for now - measurement.omega = twist.angular.z; - - if (measurement.v > 0 && v_max>0) - measurement.v /= v_max; - else if (measurement.v < 0 && v_backwards_max > 0) - measurement.v /= v_backwards_max; - - if (omega_max > 0) - measurement.omega /= omega_max; - - buffer_.push_back(measurement); - - // immediately compute new state - detect(v_eps, omega_eps); -} - -void FailureDetector::clear() -{ - buffer_.clear(); - oscillating_ = false; -} - -bool FailureDetector::isOscillating() const -{ - return oscillating_; -} - -bool FailureDetector::detect(double v_eps, double omega_eps) -{ - oscillating_ = false; - - if (buffer_.size() < buffer_.capacity()/2) // we start detecting only as soon as we have the buffer filled at least half - return false; - - double n = (double)buffer_.size(); - - // compute mean for v and omega - double v_mean=0; - double omega_mean=0; - int omega_zero_crossings = 0; - for (int i=0; i < n; ++i) - { - v_mean += buffer_[i].v; - omega_mean += buffer_[i].omega; - if ( i>0 && g2o::sign(buffer_[i].omega) != g2o::sign(buffer_[i-1].omega) ) - ++omega_zero_crossings; - } - v_mean /= n; - omega_mean /= n; - - if (std::abs(v_mean) < v_eps && std::abs(omega_mean) < omega_eps && omega_zero_crossings>1 ) - { - oscillating_ = true; - } -// ROS_INFO_STREAM("v: " << std::abs(v_mean) << ", omega: " << std::abs(omega_mean) << ", zero crossings: " << omega_zero_crossings); - return oscillating_; -} - - - -} // namespace teb_local_planner diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_config.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_config.cpp deleted file mode 100644 index 8123efd..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_config.cpp +++ /dev/null @@ -1,886 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/teb_config.h" - -using nav2_util::declare_parameter_if_not_declared; - -namespace teb_local_planner -{ - -void TebConfig::declareParameters(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) { - node_name = name; - - declare_parameter_if_not_declared(nh, name + "." + "odom_topic", rclcpp::ParameterValue(odom_topic)); - - // Trajectory - declare_parameter_if_not_declared(nh, name + "." + "teb_autosize", rclcpp::ParameterValue(trajectory.teb_autosize)); - declare_parameter_if_not_declared(nh, name + "." + "dt_ref", rclcpp::ParameterValue(trajectory.dt_ref)); - declare_parameter_if_not_declared(nh, name + "." + "dt_hysteresis", rclcpp::ParameterValue(trajectory.dt_hysteresis)); - declare_parameter_if_not_declared(nh, name + "." + "min_samples", rclcpp::ParameterValue(trajectory.min_samples)); - declare_parameter_if_not_declared(nh, name + "." + "max_samples", rclcpp::ParameterValue(trajectory.max_samples)); - declare_parameter_if_not_declared(nh, name + "." + "global_plan_overwrite_orientation", rclcpp::ParameterValue(trajectory.global_plan_overwrite_orientation)); - declare_parameter_if_not_declared(nh, name + "." + "allow_init_with_backwards_motion", rclcpp::ParameterValue(trajectory.allow_init_with_backwards_motion)); - declare_parameter_if_not_declared(nh, name + "." + "global_plan_viapoint_sep", rclcpp::ParameterValue(trajectory.global_plan_viapoint_sep)); - declare_parameter_if_not_declared(nh, name + "." + "via_points_ordered", rclcpp::ParameterValue(trajectory.via_points_ordered)); - declare_parameter_if_not_declared(nh, name + "." + "max_global_plan_lookahead_dist", rclcpp::ParameterValue(trajectory.max_global_plan_lookahead_dist)); - declare_parameter_if_not_declared(nh, name + "." + "global_plan_prune_distance", rclcpp::ParameterValue(trajectory.global_plan_prune_distance)); - declare_parameter_if_not_declared(nh, name + "." + "exact_arc_length", rclcpp::ParameterValue(trajectory.exact_arc_length)); - declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_dist", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_dist)); - declare_parameter_if_not_declared(nh, name + "." + "force_reinit_new_goal_angular", rclcpp::ParameterValue(trajectory.force_reinit_new_goal_angular)); - declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_no_poses", rclcpp::ParameterValue(trajectory.feasibility_check_no_poses)); - declare_parameter_if_not_declared(nh, name + "." + "publish_feedback", rclcpp::ParameterValue(trajectory.publish_feedback)); - declare_parameter_if_not_declared(nh, name + "." + "min_resolution_collision_check_angular", rclcpp::ParameterValue(trajectory.min_resolution_collision_check_angular)); - declare_parameter_if_not_declared(nh, name + "." + "control_look_ahead_poses", rclcpp::ParameterValue(trajectory.control_look_ahead_poses)); - declare_parameter_if_not_declared(nh, name + "." + "feasibility_check_lookahead_distance", rclcpp::ParameterValue(trajectory.feasibility_check_lookahead_distance)); - - // Robot - declare_parameter_if_not_declared(nh, name + "." + "max_vel_x", rclcpp::ParameterValue(robot.max_vel_x)); - declare_parameter_if_not_declared(nh, name + "." + "max_vel_x_backwards", rclcpp::ParameterValue(robot.max_vel_x_backwards)); - declare_parameter_if_not_declared(nh, name + "." + "max_vel_y", rclcpp::ParameterValue(robot.max_vel_y)); - declare_parameter_if_not_declared(nh, name + "." + "max_vel_theta", rclcpp::ParameterValue(robot.max_vel_theta)); - declare_parameter_if_not_declared(nh, name + "." + "acc_lim_x", rclcpp::ParameterValue(robot.acc_lim_x)); - declare_parameter_if_not_declared(nh, name + "." + "acc_lim_y", rclcpp::ParameterValue(robot.acc_lim_y)); - declare_parameter_if_not_declared(nh, name + "." + "acc_lim_theta", rclcpp::ParameterValue(robot.acc_lim_theta)); - declare_parameter_if_not_declared(nh, name + "." + "min_turning_radius", rclcpp::ParameterValue(robot.min_turning_radius)); - declare_parameter_if_not_declared(nh, name + "." + "wheelbase", rclcpp::ParameterValue(robot.wheelbase)); - declare_parameter_if_not_declared(nh, name + "." + "cmd_angle_instead_rotvel", rclcpp::ParameterValue(robot.cmd_angle_instead_rotvel)); - declare_parameter_if_not_declared(nh, name + "." + "is_footprint_dynamic", rclcpp::ParameterValue(robot.is_footprint_dynamic)); - - // GoalTolerance - declare_parameter_if_not_declared(nh, name + "." + "free_goal_vel", rclcpp::ParameterValue(goal_tolerance.free_goal_vel)); - - // Obstacles - declare_parameter_if_not_declared(nh, name + "." + "min_obstacle_dist", rclcpp::ParameterValue(obstacles.min_obstacle_dist)); - declare_parameter_if_not_declared(nh, name + "." + "inflation_dist", rclcpp::ParameterValue(obstacles.inflation_dist)); - declare_parameter_if_not_declared(nh, name + "." + "dynamic_obstacle_inflation_dist", rclcpp::ParameterValue(obstacles.dynamic_obstacle_inflation_dist)); - declare_parameter_if_not_declared(nh, name + "." + "include_dynamic_obstacles", rclcpp::ParameterValue(obstacles.include_dynamic_obstacles)); - declare_parameter_if_not_declared(nh, name + "." + "include_costmap_obstacles", rclcpp::ParameterValue(obstacles.include_costmap_obstacles)); - declare_parameter_if_not_declared(nh, name + "." + "costmap_obstacles_behind_robot_dist", rclcpp::ParameterValue(obstacles.costmap_obstacles_behind_robot_dist)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_poses_affected", rclcpp::ParameterValue(obstacles.obstacle_poses_affected)); - declare_parameter_if_not_declared(nh, name + "." + "legacy_obstacle_association", rclcpp::ParameterValue(obstacles.legacy_obstacle_association)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_association_force_inclusion_factor", rclcpp::ParameterValue(obstacles.obstacle_association_force_inclusion_factor)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_association_cutoff_factor", rclcpp::ParameterValue(obstacles.obstacle_association_cutoff_factor)); - declare_parameter_if_not_declared(nh, name + "." + "costmap_converter_plugin", rclcpp::ParameterValue(obstacles.costmap_converter_plugin)); - declare_parameter_if_not_declared(nh, name + "." + "costmap_converter_spin_thread", rclcpp::ParameterValue(obstacles.costmap_converter_spin_thread)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_ratio_max_vel", rclcpp::ParameterValue(obstacles.obstacle_proximity_ratio_max_vel)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_lower_bound", rclcpp::ParameterValue(obstacles.obstacle_proximity_lower_bound)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_proximity_upper_bound", rclcpp::ParameterValue(obstacles.obstacle_proximity_upper_bound)); - - // Optimization - declare_parameter_if_not_declared(nh, name + "." + "no_inner_iterations", rclcpp::ParameterValue(optim.no_inner_iterations)); - declare_parameter_if_not_declared(nh, name + "." + "no_outer_iterations", rclcpp::ParameterValue(optim.no_outer_iterations)); - declare_parameter_if_not_declared(nh, name + "." + "optimization_activate", rclcpp::ParameterValue(optim.optimization_activate)); - declare_parameter_if_not_declared(nh, name + "." + "optimization_verbose", rclcpp::ParameterValue(optim.optimization_verbose)); - declare_parameter_if_not_declared(nh, name + "." + "penalty_epsilon", rclcpp::ParameterValue(optim.penalty_epsilon)); - declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_x", rclcpp::ParameterValue(optim.weight_max_vel_x)); - declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_y", rclcpp::ParameterValue(optim.weight_max_vel_y)); - declare_parameter_if_not_declared(nh, name + "." + "weight_max_vel_theta", rclcpp::ParameterValue(optim.weight_max_vel_theta)); - declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_x", rclcpp::ParameterValue(optim.weight_acc_lim_x)); - declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_y", rclcpp::ParameterValue(optim.weight_acc_lim_y)); - declare_parameter_if_not_declared(nh, name + "." + "weight_acc_lim_theta", rclcpp::ParameterValue(optim.weight_acc_lim_theta)); - declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_nh", rclcpp::ParameterValue(optim.weight_kinematics_nh)); - declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_forward_drive", rclcpp::ParameterValue(optim.weight_kinematics_forward_drive)); - declare_parameter_if_not_declared(nh, name + "." + "weight_kinematics_turning_radius", rclcpp::ParameterValue(optim.weight_kinematics_turning_radius)); - declare_parameter_if_not_declared(nh, name + "." + "weight_optimaltime", rclcpp::ParameterValue(optim.weight_optimaltime)); - declare_parameter_if_not_declared(nh, name + "." + "weight_shortest_path", rclcpp::ParameterValue(optim.weight_shortest_path)); - declare_parameter_if_not_declared(nh, name + "." + "weight_obstacle", rclcpp::ParameterValue(optim.weight_obstacle)); - declare_parameter_if_not_declared(nh, name + "." + "weight_inflation", rclcpp::ParameterValue(optim.weight_inflation)); - declare_parameter_if_not_declared(nh, name + "." + "weight_dynamic_obstacle", rclcpp::ParameterValue(optim.weight_dynamic_obstacle)); - declare_parameter_if_not_declared(nh, name + "." + "weight_dynamic_obstacle_inflation", rclcpp::ParameterValue(optim.weight_dynamic_obstacle_inflation)); - declare_parameter_if_not_declared(nh, name + "." + "weight_viapoint", rclcpp::ParameterValue(optim.weight_viapoint)); - declare_parameter_if_not_declared(nh, name + "." + "weight_prefer_rotdir", rclcpp::ParameterValue(optim.weight_prefer_rotdir)); - declare_parameter_if_not_declared(nh, name + "." + "weight_adapt_factor", rclcpp::ParameterValue(optim.weight_adapt_factor)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_cost_exponent", rclcpp::ParameterValue(optim.obstacle_cost_exponent)); - declare_parameter_if_not_declared(nh, name + "." + "weight_velocity_obstacle_ratio", rclcpp::ParameterValue(optim.weight_velocity_obstacle_ratio)); - - // Homotopy Class Planner - declare_parameter_if_not_declared(nh, name + "." + "enable_homotopy_class_planning", rclcpp::ParameterValue(hcp.enable_homotopy_class_planning)); - declare_parameter_if_not_declared(nh, name + "." + "enable_multithreading", rclcpp::ParameterValue(hcp.enable_multithreading)); - declare_parameter_if_not_declared(nh, name + "." + "simple_exploration", rclcpp::ParameterValue(hcp.simple_exploration)); - declare_parameter_if_not_declared(nh, name + "." + "max_number_classes", rclcpp::ParameterValue(hcp.max_number_classes)); - declare_parameter_if_not_declared(nh, name + "." + "selection_obst_cost_scale", rclcpp::ParameterValue(hcp.selection_obst_cost_scale)); - declare_parameter_if_not_declared(nh, name + "." + "selection_prefer_initial_plan", rclcpp::ParameterValue(hcp.selection_prefer_initial_plan)); - declare_parameter_if_not_declared(nh, name + "." + "selection_viapoint_cost_scale", rclcpp::ParameterValue(hcp.selection_viapoint_cost_scale)); - declare_parameter_if_not_declared(nh, name + "." + "selection_cost_hysteresis", rclcpp::ParameterValue(hcp.selection_cost_hysteresis)); - declare_parameter_if_not_declared(nh, name + "." + "selection_alternative_time_cost", rclcpp::ParameterValue(hcp.selection_alternative_time_cost)); - declare_parameter_if_not_declared(nh, name + "." + "switching_blocking_period", rclcpp::ParameterValue(hcp.switching_blocking_period)); - declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_samples", rclcpp::ParameterValue(hcp.roadmap_graph_no_samples)); - declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_area_width", rclcpp::ParameterValue(hcp.roadmap_graph_area_width)); - declare_parameter_if_not_declared(nh, name + "." + "roadmap_graph_area_length_scale", rclcpp::ParameterValue(hcp.roadmap_graph_area_length_scale)); - declare_parameter_if_not_declared(nh, name + "." + "h_signature_prescaler", rclcpp::ParameterValue(hcp.h_signature_prescaler)); - declare_parameter_if_not_declared(nh, name + "." + "h_signature_threshold", rclcpp::ParameterValue(hcp.h_signature_threshold)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_keypoint_offset", rclcpp::ParameterValue(hcp.obstacle_keypoint_offset)); - declare_parameter_if_not_declared(nh, name + "." + "obstacle_heading_threshold", rclcpp::ParameterValue(hcp.obstacle_heading_threshold)); - declare_parameter_if_not_declared(nh, name + "." + "viapoints_all_candidates", rclcpp::ParameterValue(hcp.viapoints_all_candidates)); - declare_parameter_if_not_declared(nh, name + "." + "visualize_hc_graph", rclcpp::ParameterValue(hcp.visualize_hc_graph)); - declare_parameter_if_not_declared(nh, name + "." + "visualize_with_time_as_z_axis_scale", rclcpp::ParameterValue(hcp.visualize_with_time_as_z_axis_scale)); - declare_parameter_if_not_declared(nh, name + "." + "delete_detours_backwards", rclcpp::ParameterValue(hcp.delete_detours_backwards)); - declare_parameter_if_not_declared(nh, name + "." + "detours_orientation_tolerance", rclcpp::ParameterValue(hcp.detours_orientation_tolerance)); - declare_parameter_if_not_declared(nh, name + "." + "length_start_orientation_vector", rclcpp::ParameterValue(hcp.length_start_orientation_vector)); - declare_parameter_if_not_declared(nh, name + "." + "max_ratio_detours_duration_best_duration", rclcpp::ParameterValue(hcp.max_ratio_detours_duration_best_duration)); - declare_parameter_if_not_declared(nh, name + "." + "selection_dropping_probability", rclcpp::ParameterValue(hcp.selection_dropping_probability)); - - // Recovery - declare_parameter_if_not_declared(nh, name + "." + "shrink_horizon_backup", rclcpp::ParameterValue(recovery.shrink_horizon_backup)); - declare_parameter_if_not_declared(nh, name + "." + "shrink_horizon_min_duration", rclcpp::ParameterValue(recovery.shrink_horizon_min_duration)); - declare_parameter_if_not_declared(nh, name + "." + "oscillation_recovery", rclcpp::ParameterValue(recovery.oscillation_recovery)); - declare_parameter_if_not_declared(nh, name + "." + "oscillation_v_eps", rclcpp::ParameterValue(recovery.oscillation_v_eps)); - declare_parameter_if_not_declared(nh, name + "." + "oscillation_omega_eps", rclcpp::ParameterValue(recovery.oscillation_omega_eps)); - declare_parameter_if_not_declared(nh, name + "." + "oscillation_recovery_min_duration", rclcpp::ParameterValue(recovery.oscillation_recovery_min_duration)); - declare_parameter_if_not_declared(nh, name + "." + "oscillation_filter_duration", rclcpp::ParameterValue(recovery.oscillation_filter_duration)); - declare_parameter_if_not_declared(nh, name + "." + "divergence_detection_enable", rclcpp::ParameterValue(recovery.divergence_detection_enable)); - declare_parameter_if_not_declared(nh, name + "." + "divergence_detection_max_chi_squared", rclcpp::ParameterValue(recovery.divergence_detection_max_chi_squared)); - - // footprint model - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.type", rclcpp::ParameterType::PARAMETER_STRING); -} - -void TebConfig::loadRosParamFromNodeHandle(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) -{ - nh->get_parameter_or(name + "." + "odom_topic", odom_topic, odom_topic); - - // Trajectory - nh->get_parameter_or(name + "." + "teb_autosize", trajectory.teb_autosize, trajectory.teb_autosize); - nh->get_parameter_or(name + "." + "dt_ref", trajectory.dt_ref, trajectory.dt_ref); - nh->get_parameter_or(name + "." + "dt_hysteresis", trajectory.dt_hysteresis, trajectory.dt_hysteresis); - nh->get_parameter_or(name + "." + "min_samples", trajectory.min_samples, trajectory.min_samples); - nh->get_parameter_or(name + "." + "max_samples", trajectory.max_samples, trajectory.max_samples); - nh->get_parameter_or(name + "." + "global_plan_overwrite_orientation", trajectory.global_plan_overwrite_orientation, trajectory.global_plan_overwrite_orientation); - nh->get_parameter_or(name + "." + "allow_init_with_backwards_motion", trajectory.allow_init_with_backwards_motion, trajectory.allow_init_with_backwards_motion); - nh->get_parameter_or(name + "." + "global_plan_viapoint_sep", trajectory.global_plan_viapoint_sep, trajectory.global_plan_viapoint_sep); - nh->get_parameter_or(name + "." + "via_points_ordered", trajectory.via_points_ordered, trajectory.via_points_ordered); - nh->get_parameter_or(name + "." + "max_global_plan_lookahead_dist", trajectory.max_global_plan_lookahead_dist, trajectory.max_global_plan_lookahead_dist); - nh->get_parameter_or(name + "." + "global_plan_prune_distance", trajectory.global_plan_prune_distance, trajectory.global_plan_prune_distance); - nh->get_parameter_or(name + "." + "exact_arc_length", trajectory.exact_arc_length, trajectory.exact_arc_length); - nh->get_parameter_or(name + "." + "force_reinit_new_goal_dist", trajectory.force_reinit_new_goal_dist, trajectory.force_reinit_new_goal_dist); - nh->get_parameter_or(name + "." + "force_reinit_new_goal_angular", trajectory.force_reinit_new_goal_angular, trajectory.force_reinit_new_goal_angular); - nh->get_parameter_or(name + "." + "feasibility_check_no_poses", trajectory.feasibility_check_no_poses, trajectory.feasibility_check_no_poses); - nh->get_parameter_or(name + "." + "publish_feedback", trajectory.publish_feedback, trajectory.publish_feedback); - nh->get_parameter_or(name + "." + "min_resolution_collision_check_angular", trajectory.min_resolution_collision_check_angular, trajectory.min_resolution_collision_check_angular); - nh->get_parameter_or(name + "." + "control_look_ahead_poses", trajectory.control_look_ahead_poses, trajectory.control_look_ahead_poses); - nh->get_parameter_or(name + "." + "feasibility_check_lookahead_distance", trajectory.feasibility_check_lookahead_distance, trajectory.feasibility_check_lookahead_distance); - - // Robot - nh->get_parameter_or(name + "." + "max_vel_x", robot.max_vel_x, robot.max_vel_x); - nh->get_parameter_or(name + "." + "max_vel_x_backwards", robot.max_vel_x_backwards, robot.max_vel_x_backwards); - nh->get_parameter_or(name + "." + "max_vel_y", robot.max_vel_y, robot.max_vel_y); - nh->get_parameter_or(name + "." + "max_vel_theta", robot.max_vel_theta, robot.max_vel_theta); - nh->get_parameter_or(name + "." + "acc_lim_x", robot.acc_lim_x, robot.acc_lim_x); - nh->get_parameter_or(name + "." + "acc_lim_y", robot.acc_lim_y, robot.acc_lim_y); - nh->get_parameter_or(name + "." + "acc_lim_theta", robot.acc_lim_theta, robot.acc_lim_theta); - nh->get_parameter_or(name + "." + "min_turning_radius", robot.min_turning_radius, robot.min_turning_radius); - nh->get_parameter_or(name + "." + "wheelbase", robot.wheelbase, robot.wheelbase); - nh->get_parameter_or(name + "." + "cmd_angle_instead_rotvel", robot.cmd_angle_instead_rotvel, robot.cmd_angle_instead_rotvel); - nh->get_parameter_or(name + "." + "is_footprint_dynamic", robot.is_footprint_dynamic, robot.is_footprint_dynamic); - - // GoalTolerance - nh->get_parameter_or(name + "." + "free_goal_vel", goal_tolerance.free_goal_vel, goal_tolerance.free_goal_vel); - - // Obstacles - nh->get_parameter_or(name + "." + "min_obstacle_dist", obstacles.min_obstacle_dist, obstacles.min_obstacle_dist); - nh->get_parameter_or(name + "." + "inflation_dist", obstacles.inflation_dist, obstacles.inflation_dist); - nh->get_parameter_or(name + "." + "dynamic_obstacle_inflation_dist", obstacles.dynamic_obstacle_inflation_dist, obstacles.dynamic_obstacle_inflation_dist); - nh->get_parameter_or(name + "." + "include_dynamic_obstacles", obstacles.include_dynamic_obstacles, obstacles.include_dynamic_obstacles); - nh->get_parameter_or(name + "." + "include_costmap_obstacles", obstacles.include_costmap_obstacles, obstacles.include_costmap_obstacles); - nh->get_parameter_or(name + "." + "costmap_obstacles_behind_robot_dist", obstacles.costmap_obstacles_behind_robot_dist, obstacles.costmap_obstacles_behind_robot_dist); - nh->get_parameter_or(name + "." + "obstacle_poses_affected", obstacles.obstacle_poses_affected, obstacles.obstacle_poses_affected); - nh->get_parameter_or(name + "." + "legacy_obstacle_association", obstacles.legacy_obstacle_association, obstacles.legacy_obstacle_association); - nh->get_parameter_or(name + "." + "obstacle_association_force_inclusion_factor", obstacles.obstacle_association_force_inclusion_factor, obstacles.obstacle_association_force_inclusion_factor); - nh->get_parameter_or(name + "." + "obstacle_association_cutoff_factor", obstacles.obstacle_association_cutoff_factor, obstacles.obstacle_association_cutoff_factor); - nh->get_parameter_or(name + "." + "costmap_converter_plugin", obstacles.costmap_converter_plugin, obstacles.costmap_converter_plugin); - nh->get_parameter_or(name + "." + "costmap_converter_spin_thread", obstacles.costmap_converter_spin_thread, obstacles.costmap_converter_spin_thread); - nh->get_parameter_or(name + "." + "obstacle_proximity_ratio_max_vel", obstacles.obstacle_proximity_ratio_max_vel, obstacles.obstacle_proximity_ratio_max_vel); - nh->get_parameter_or(name + "." + "obstacle_proximity_lower_bound", obstacles.obstacle_proximity_lower_bound, obstacles.obstacle_proximity_lower_bound); - nh->get_parameter_or(name + "." + "obstacle_proximity_upper_bound", obstacles.obstacle_proximity_upper_bound, obstacles.obstacle_proximity_upper_bound); - - // Optimization - nh->get_parameter_or(name + "." + "no_inner_iterations", optim.no_inner_iterations, optim.no_inner_iterations); - nh->get_parameter_or(name + "." + "no_outer_iterations", optim.no_outer_iterations, optim.no_outer_iterations); - nh->get_parameter_or(name + "." + "optimization_activate", optim.optimization_activate, optim.optimization_activate); - nh->get_parameter_or(name + "." + "optimization_verbose", optim.optimization_verbose, optim.optimization_verbose); - nh->get_parameter_or(name + "." + "penalty_epsilon", optim.penalty_epsilon, optim.penalty_epsilon); - nh->get_parameter_or(name + "." + "weight_max_vel_x", optim.weight_max_vel_x, optim.weight_max_vel_x); - nh->get_parameter_or(name + "." + "weight_max_vel_y", optim.weight_max_vel_y, optim.weight_max_vel_y); - nh->get_parameter_or(name + "." + "weight_max_vel_theta", optim.weight_max_vel_theta, optim.weight_max_vel_theta); - nh->get_parameter_or(name + "." + "weight_acc_lim_x", optim.weight_acc_lim_x, optim.weight_acc_lim_x); - nh->get_parameter_or(name + "." + "weight_acc_lim_y", optim.weight_acc_lim_y, optim.weight_acc_lim_y); - nh->get_parameter_or(name + "." + "weight_acc_lim_theta", optim.weight_acc_lim_theta, optim.weight_acc_lim_theta); - nh->get_parameter_or(name + "." + "weight_kinematics_nh", optim.weight_kinematics_nh, optim.weight_kinematics_nh); - nh->get_parameter_or(name + "." + "weight_kinematics_forward_drive", optim.weight_kinematics_forward_drive, optim.weight_kinematics_forward_drive); - nh->get_parameter_or(name + "." + "weight_kinematics_turning_radius", optim.weight_kinematics_turning_radius, optim.weight_kinematics_turning_radius); - nh->get_parameter_or(name + "." + "weight_optimaltime", optim.weight_optimaltime, optim.weight_optimaltime); - nh->get_parameter_or(name + "." + "weight_shortest_path", optim.weight_shortest_path, optim.weight_shortest_path); - nh->get_parameter_or(name + "." + "weight_obstacle", optim.weight_obstacle, optim.weight_obstacle); - nh->get_parameter_or(name + "." + "weight_inflation", optim.weight_inflation, optim.weight_inflation); - nh->get_parameter_or(name + "." + "weight_dynamic_obstacle", optim.weight_dynamic_obstacle, optim.weight_dynamic_obstacle); - nh->get_parameter_or(name + "." + "weight_dynamic_obstacle_inflation", optim.weight_dynamic_obstacle_inflation, optim.weight_dynamic_obstacle_inflation); - nh->get_parameter_or(name + "." + "weight_viapoint", optim.weight_viapoint, optim.weight_viapoint); - nh->get_parameter_or(name + "." + "weight_prefer_rotdir", optim.weight_prefer_rotdir, optim.weight_prefer_rotdir); - nh->get_parameter_or(name + "." + "weight_adapt_factor", optim.weight_adapt_factor, optim.weight_adapt_factor); - nh->get_parameter_or(name + "." + "obstacle_cost_exponent", optim.obstacle_cost_exponent, optim.obstacle_cost_exponent); - nh->get_parameter_or(name + "." + "weight_velocity_obstacle_ratio", optim.weight_velocity_obstacle_ratio, optim.weight_velocity_obstacle_ratio); - - // Homotopy Class Planner - nh->get_parameter_or(name + "." + "enable_homotopy_class_planning", hcp.enable_homotopy_class_planning, hcp.enable_homotopy_class_planning); - nh->get_parameter_or(name + "." + "enable_multithreading", hcp.enable_multithreading, hcp.enable_multithreading); - nh->get_parameter_or(name + "." + "simple_exploration", hcp.simple_exploration, hcp.simple_exploration); - nh->get_parameter_or(name + "." + "max_number_classes", hcp.max_number_classes, hcp.max_number_classes); - nh->get_parameter_or(name + "." + "selection_obst_cost_scale", hcp.selection_obst_cost_scale, hcp.selection_obst_cost_scale); - nh->get_parameter_or(name + "." + "selection_prefer_initial_plan", hcp.selection_prefer_initial_plan, hcp.selection_prefer_initial_plan); - nh->get_parameter_or(name + "." + "selection_viapoint_cost_scale", hcp.selection_viapoint_cost_scale, hcp.selection_viapoint_cost_scale); - nh->get_parameter_or(name + "." + "selection_cost_hysteresis", hcp.selection_cost_hysteresis, hcp.selection_cost_hysteresis); - nh->get_parameter_or(name + "." + "selection_alternative_time_cost", hcp.selection_alternative_time_cost, hcp.selection_alternative_time_cost); - nh->get_parameter_or(name + "." + "switching_blocking_period", hcp.switching_blocking_period, hcp.switching_blocking_period); - nh->get_parameter_or(name + "." + "roadmap_graph_samples", hcp.roadmap_graph_no_samples, hcp.roadmap_graph_no_samples); - nh->get_parameter_or(name + "." + "roadmap_graph_area_width", hcp.roadmap_graph_area_width, hcp.roadmap_graph_area_width); - nh->get_parameter_or(name + "." + "roadmap_graph_area_length_scale", hcp.roadmap_graph_area_length_scale, hcp.roadmap_graph_area_length_scale); - nh->get_parameter_or(name + "." + "h_signature_prescaler", hcp.h_signature_prescaler, hcp.h_signature_prescaler); - nh->get_parameter_or(name + "." + "h_signature_threshold", hcp.h_signature_threshold, hcp.h_signature_threshold); - nh->get_parameter_or(name + "." + "obstacle_keypoint_offset", hcp.obstacle_keypoint_offset, hcp.obstacle_keypoint_offset); - nh->get_parameter_or(name + "." + "obstacle_heading_threshold", hcp.obstacle_heading_threshold, hcp.obstacle_heading_threshold); - nh->get_parameter_or(name + "." + "viapoints_all_candidates", hcp.viapoints_all_candidates, hcp.viapoints_all_candidates); - nh->get_parameter_or(name + "." + "visualize_hc_graph", hcp.visualize_hc_graph, hcp.visualize_hc_graph); - nh->get_parameter_or(name + "." + "visualize_with_time_as_z_axis_scale", hcp.visualize_with_time_as_z_axis_scale, hcp.visualize_with_time_as_z_axis_scale); - nh->get_parameter_or(name + "." + "delete_detours_backwards", hcp.delete_detours_backwards, hcp.delete_detours_backwards); - nh->get_parameter_or(name + "." + "detours_orientation_tolerance", hcp.detours_orientation_tolerance, hcp.detours_orientation_tolerance); - nh->get_parameter_or(name + "." + "length_start_orientation_vector", hcp.length_start_orientation_vector, hcp.length_start_orientation_vector); - nh->get_parameter_or(name + "." + "max_ratio_detours_duration_best_duration", hcp.max_ratio_detours_duration_best_duration, hcp.max_ratio_detours_duration_best_duration); - nh->get_parameter_or(name + "." + "selection_dropping_probability", hcp.selection_dropping_probability, hcp.selection_dropping_probability); - - // Recovery - nh->get_parameter_or(name + "." + "shrink_horizon_backup", recovery.shrink_horizon_backup, recovery.shrink_horizon_backup); - nh->get_parameter_or(name + "." + "shrink_horizon_min_duration", recovery.shrink_horizon_min_duration, recovery.shrink_horizon_min_duration); - nh->get_parameter_or(name + "." + "oscillation_recovery", recovery.oscillation_recovery, recovery.oscillation_recovery); - nh->get_parameter_or(name + "." + "oscillation_v_eps", recovery.oscillation_v_eps, recovery.oscillation_v_eps); - nh->get_parameter_or(name + "." + "oscillation_omega_eps", recovery.oscillation_omega_eps, recovery.oscillation_omega_eps); - nh->get_parameter_or(name + "." + "oscillation_recovery_min_duration", recovery.oscillation_recovery_min_duration, recovery.oscillation_recovery_min_duration); - nh->get_parameter_or(name + "." + "oscillation_filter_duration", recovery.oscillation_filter_duration, recovery.oscillation_filter_duration); - nh->get_parameter_or(name + "." + "divergence_detection_enable", recovery.divergence_detection_enable, recovery.divergence_detection_enable); - nh->get_parameter_or(name + "." + "divergence_detection_max_chi_squared", recovery.divergence_detection_max_chi_squared, recovery.divergence_detection_max_chi_squared); - - // footprint model - if (!nh->get_parameter(name + "." + "footprint_model.type", model_name)) - { - RCLCPP_INFO(logger_, "No robot footprint model specified for trajectory optimization. Using point-shaped model."); - robot_model = std::make_shared(); - } - - // point - else if (model_name.compare("point") == 0) - { - RCLCPP_INFO(logger_, "Footprint model 'point' loaded for trajectory optimization."); - robot_model = std::make_shared(); - } - - // circular - else if (model_name.compare("circular") == 0) - { - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.radius", rclcpp::ParameterType::PARAMETER_DOUBLE); - // get radius - double radius; - if (!nh->get_parameter(name + "." + "footprint_model.radius", radius)) - { - RCLCPP_ERROR(logger_, - "Footprint model 'circular' cannot be loaded for trajectory optimization, since param '%s.footprint_model.radius' does not exist. Using point-model instead.", - nh->get_namespace()); - - robot_model = std::make_shared(); - } - RCLCPP_INFO(logger_, "Footprint model 'circular' (radius: %fm) loaded for trajectory optimization.", radius); - robot_model = std::make_shared(radius); - } - - - // line - else if (model_name.compare("line") == 0) - { - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.line_start", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.line_end", rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); - std::vector line_start, line_end; - // check parameters - if (!nh->get_parameter(name + "." + "footprint_model.line_start", line_start) || !nh->get_parameter(name + "." + "footprint_model.line_end", line_end)) - { - RCLCPP_ERROR(logger_, - "Footprint model 'line' cannot be loaded for trajectory optimization, since param '%s.footprint_model.line_start' and/or '.../line_end' do not exist. Using point-model instead.", - nh->get_namespace()); - robot_model = std::make_shared(); - } - if (line_start.size() != 2 || line_end.size() != 2) - { - RCLCPP_ERROR(logger_, "Footprint model 'line' cannot be loaded for trajectory optimization, since param '%s.footprint_model.line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead.", - nh->get_namespace()); - robot_model = std::make_shared(); - } - - RCLCPP_INFO(logger_, - "Footprint model 'line' (line_start: [%f,%f]m, line_end: [%f,%f]m) loaded for trajectory optimization.", - line_start[0], line_start[1], line_end[0], line_end[1]); - - robot_model = std::make_shared(Eigen::Map(line_start.data()), Eigen::Map(line_end.data())); - } - - // two circles - else if (model_name.compare("two_circles") == 0) - { - rclcpp::Parameter dummy; - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.front_offset", rclcpp::ParameterType::PARAMETER_DOUBLE); - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.front_radius", rclcpp::ParameterType::PARAMETER_DOUBLE); - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.rear_offset", rclcpp::ParameterType::PARAMETER_DOUBLE); - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.rear_radius", rclcpp::ParameterType::PARAMETER_DOUBLE); - // check parameters - if (!nh->get_parameter(name + "." + "footprint_model.front_offset", dummy) || !nh->get_parameter(name + "." + "footprint_model.front_radius", dummy) - || !nh->get_parameter(name + "." + "footprint_model.rear_offset", dummy) || !nh->get_parameter(name + "." + "footprint_model.rear_radius", dummy)) - { - RCLCPP_ERROR(logger_, - "Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '%s.footprint_model.front_offset', '.../front_radius', '.../rear_offset' and '.../rear_radius' do not exist. Using point-model instead.", - nh->get_namespace()); - robot_model = std::make_shared(); - } - double front_offset, front_radius, rear_offset, rear_radius; - nh->get_parameter(name + "." + "footprint_model.front_offset", front_offset); - nh->get_parameter(name + "." + "footprint_model.front_radius", front_radius); - nh->get_parameter(name + "." + "footprint_model.rear_offset", rear_offset); - nh->get_parameter(name + "." + "footprint_model.rear_radius", rear_radius); - RCLCPP_INFO(logger_, - "Footprint model 'two_circles' (front_offset: %fm, front_radius: %fm, rear_offset: %fm, rear_radius: %fm) loaded for trajectory optimization.", - front_offset, front_radius, rear_offset, rear_radius); - - robot_model = std::make_shared(front_offset, front_radius, rear_offset, rear_radius); - } - - // polygon - else if (model_name.compare("polygon") == 0) - { - declare_parameter_if_not_declared(nh, name + "." + "footprint_model.vertices", rclcpp::ParameterType::PARAMETER_STRING); - // check parameters - std::string footprint_string; - if (!nh->get_parameter(name + "." + "footprint_model.vertices", footprint_string) ) - { - RCLCPP_ERROR(logger_, - "Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '%s.footprint_model.vertices' does not exist. Using point-model instead.", - nh->get_namespace()); - - robot_model = std::make_shared(); - } - - std::vector footprint; - // get vertices - if (nav2_costmap_2d::makeFootprintFromString(footprint_string, footprint)) - { - Point2dContainer polygon; - for(const auto &pt : footprint) { - polygon.push_back(Eigen::Vector2d(pt.x, pt.y)); - } - RCLCPP_INFO(logger_, "Footprint model 'polygon' loaded for trajectory optimization."); - robot_model = std::make_shared(polygon); - } - else - { - RCLCPP_ERROR(logger_, - "Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '%s.footprint_model.vertices' does not define an array of coordinates. Using point-model instead.", - nh->get_namespace()); - robot_model = std::make_shared(); - } - - } - // otherwise - else - { - RCLCPP_WARN(logger_, "Unknown robot footprint model specified with parameter '%s.footprint_model.type'. Using point model instead.", - nh->get_namespace()); - robot_model = std::make_shared(); - } - - - checkParameters(); - checkDeprecated(nh, name); -} - -rcl_interfaces::msg::SetParametersResult - TebConfig::dynamicParametersCallback(std::vector parameters) -{ - auto result = rcl_interfaces::msg::SetParametersResult(); - std::lock_guard l(config_mutex_); - - bool reload_footprint = false; - - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) { - // Trajectory - if (name == node_name + ".teb_autosize") { - trajectory.teb_autosize = parameter.as_double(); - } else if (name == node_name + ".dt_ref") { - trajectory.dt_ref = parameter.as_double(); - } else if (name == node_name + ".dt_hysteresis") { - trajectory.dt_hysteresis = parameter.as_double(); - } else if (name == node_name + ".global_plan_viapoint_sep") { - trajectory.global_plan_viapoint_sep = parameter.as_double(); - } else if (name == node_name + ".max_global_plan_lookahead_dist") { - trajectory.max_global_plan_lookahead_dist = parameter.as_double(); - } else if (name == node_name + ".global_plan_prune_distance") { - trajectory.global_plan_prune_distance = parameter.as_double(); - } else if (name == node_name + ".force_reinit_new_goal_dist") { - trajectory.force_reinit_new_goal_dist = parameter.as_double(); - } else if (name == node_name + ".force_reinit_new_goal_angular") { - trajectory.force_reinit_new_goal_angular = parameter.as_double(); - } else if (name == node_name + ".min_resolution_collision_check_angular") { - trajectory.min_resolution_collision_check_angular = parameter.as_double(); - } else if (name == node_name + ".feasibility_check_lookahead_distance") { - trajectory.feasibility_check_lookahead_distance = parameter.as_double(); - } - // Robot - else if (name == node_name + ".max_vel_x") { - robot.max_vel_x = parameter.as_double(); - robot.base_max_vel_x = parameter.as_double(); - } else if (name == node_name + ".max_vel_x_backwards") { - robot.max_vel_x_backwards = parameter.as_double(); - robot.base_max_vel_x_backwards = parameter.as_double(); - } else if (name == node_name + ".max_vel_y") { - robot.max_vel_y = parameter.as_double(); - robot.base_max_vel_y = parameter.as_double(); - } else if (name == node_name + ".max_vel_theta") { - robot.max_vel_theta = parameter.as_double(); - robot.base_max_vel_theta = parameter.as_double(); - } else if (name == node_name + ".acc_lim_x") { - robot.acc_lim_x = parameter.as_double(); - } else if (name == node_name + ".acc_lim_y") { - robot.acc_lim_y = parameter.as_double(); - } else if (name == node_name + ".acc_lim_theta") { - robot.acc_lim_theta = parameter.as_double(); - } else if (name == node_name + ".min_turning_radius") { - robot.min_turning_radius = parameter.as_double(); - } else if (name == node_name + ".wheelbase") { - robot.wheelbase = parameter.as_double(); - } - // GoalTolerance - // Obstacles - else if (name == node_name + ".min_obstacle_dist") { - obstacles.min_obstacle_dist = parameter.as_double(); - } else if (name == node_name + ".inflation_dist") { - obstacles.inflation_dist = parameter.as_double(); - } else if (name == node_name + ".dynamic_obstacle_inflation_dist") { - obstacles.dynamic_obstacle_inflation_dist = parameter.as_double(); - } else if (name == node_name + ".costmap_obstacles_behind_robot_dist") { - obstacles.costmap_obstacles_behind_robot_dist = parameter.as_double(); - } else if (name == node_name + ".obstacle_association_force_inclusion_factor") { - obstacles.obstacle_association_force_inclusion_factor = parameter.as_double(); - } else if (name == node_name + ".obstacle_association_cutoff_factor") { - obstacles.obstacle_association_cutoff_factor = parameter.as_double(); - } else if (name == node_name + ".obstacle_proximity_ratio_max_vel") { - obstacles.obstacle_proximity_ratio_max_vel = parameter.as_double(); - } else if (name == node_name + ".obstacle_proximity_lower_bound") { - obstacles.obstacle_proximity_lower_bound = parameter.as_double(); - } else if (name == node_name + ".obstacle_proximity_upper_bound") { - obstacles.obstacle_proximity_upper_bound = parameter.as_double(); - } - // Optimization - else if (name == node_name + ".penalty_epsilon") { - optim.penalty_epsilon = parameter.as_double(); - } else if (name == node_name + ".weight_max_vel_x") { - optim.weight_max_vel_x = parameter.as_double(); - } else if (name == node_name + ".weight_max_vel_y") { - optim.weight_max_vel_y = parameter.as_double(); - } else if (name == node_name + ".weight_max_vel_theta") { - optim.weight_max_vel_theta = parameter.as_double(); - } else if (name == node_name + ".weight_acc_lim_x") { - optim.weight_acc_lim_x = parameter.as_double(); - } else if (name == node_name + ".weight_acc_lim_y") { - optim.weight_acc_lim_y = parameter.as_double(); - } else if (name == node_name + ".weight_acc_lim_theta") { - optim.weight_acc_lim_theta = parameter.as_double(); - } else if (name == node_name + ".weight_kinematics_nh") { - optim.weight_kinematics_nh = parameter.as_double(); - } else if (name == node_name + ".weight_kinematics_forward_drive") { - optim.weight_kinematics_forward_drive = parameter.as_double(); - } else if (name == node_name + ".weight_kinematics_turning_radius") { - optim.weight_kinematics_turning_radius = parameter.as_double(); - } else if (name == node_name + ".weight_optimaltime") { - optim.weight_optimaltime = parameter.as_double(); - } else if (name == node_name + ".weight_shortest_path") { - optim.weight_shortest_path = parameter.as_double(); - } else if (name == node_name + ".weight_obstacle") { - optim.weight_obstacle = parameter.as_double(); - } else if (name == node_name + ".weight_inflation") { - optim.weight_inflation = parameter.as_double(); - } else if (name == node_name + ".weight_dynamic_obstacle") { - optim.weight_dynamic_obstacle = parameter.as_double(); - } else if (name == node_name + ".weight_dynamic_obstacle_inflation") { - optim.weight_dynamic_obstacle_inflation = parameter.as_double(); - } else if (name == node_name + ".weight_viapoint") { - optim.weight_viapoint = parameter.as_double(); - } else if (name == node_name + ".weight_prefer_rotdir") { - optim.weight_prefer_rotdir = parameter.as_double(); - } else if (name == node_name + ".weight_adapt_factor") { - optim.weight_adapt_factor = parameter.as_double(); - } else if (name == node_name + ".obstacle_cost_exponent") { - optim.obstacle_cost_exponent = parameter.as_double(); - } - // Homotopy Class Planner - else if (name == node_name + ".selection_cost_hysteresis") { - hcp.selection_cost_hysteresis = parameter.as_double(); - } else if (name == node_name + ".selection_prefer_initial_plan") { - hcp.selection_prefer_initial_plan = parameter.as_double(); - } else if (name == node_name + ".selection_obst_cost_scale") { - hcp.selection_obst_cost_scale = parameter.as_double(); - } else if (name == node_name + ".selection_viapoint_cost_scale") { - hcp.selection_viapoint_cost_scale = parameter.as_double(); - } else if (name == node_name + ".switching_blocking_period") { - hcp.switching_blocking_period = parameter.as_double(); - } else if (name == node_name + ".roadmap_graph_area_width") { - hcp.roadmap_graph_area_width = parameter.as_double(); - } else if (name == node_name + ".roadmap_graph_area_length_scale") { - hcp.roadmap_graph_area_length_scale = parameter.as_double(); - } else if (name == node_name + ".h_signature_prescaler") { - hcp.h_signature_prescaler = parameter.as_double(); - } else if (name == node_name + ".h_signature_threshold") { - hcp.h_signature_threshold = parameter.as_double(); - } else if (name == node_name + ".obstacle_keypoint_offset") { - hcp.obstacle_keypoint_offset = parameter.as_double(); - } else if (name == node_name + ".obstacle_heading_threshold") { - hcp.obstacle_heading_threshold = parameter.as_double(); - } else if (name == node_name + ".visualize_with_time_as_z_axis_scale") { - hcp.visualize_with_time_as_z_axis_scale = parameter.as_double(); - } else if (name == node_name + ".detours_orientation_tolerance") { - hcp.detours_orientation_tolerance = parameter.as_double(); - } else if (name == node_name + ".length_start_orientation_vector") { - hcp.length_start_orientation_vector = parameter.as_double(); - } else if (name == node_name + ".max_ratio_detours_duration_best_duration") { - hcp.max_ratio_detours_duration_best_duration = parameter.as_double(); - } else if (name == node_name + ".selection_dropping_probability") { - hcp.selection_dropping_probability = parameter.as_double(); - } - // Recovery - else if (name == node_name + ".shrink_horizon_min_duration") { - recovery.shrink_horizon_min_duration = parameter.as_double(); - } else if (name == node_name + ".oscillation_v_eps") { - recovery.oscillation_v_eps = parameter.as_double(); - } else if (name == node_name + ".oscillation_omega_eps") { - recovery.oscillation_omega_eps = parameter.as_double(); - } else if (name == node_name + ".oscillation_recovery_min_duration") { - recovery.oscillation_recovery_min_duration = parameter.as_double(); - } else if (name == node_name + ".oscillation_filter_duration") { - recovery.oscillation_filter_duration = parameter.as_double(); - } else if (name == node_name + ".divergence_detection_max_chi_squared") { - recovery.divergence_detection_max_chi_squared = parameter.as_double(); - } - // Footprint model - else if (name == node_name + ".footprint_model.radius") { - reload_footprint = true; - radius = parameter.as_double(); - } else if (name == node_name + ".footprint_model.front_offset") { - reload_footprint = true; - front_offset = parameter.as_double(); - } else if (name == node_name + ".footprint_model.front_radius") { - reload_footprint = true; - front_radius = parameter.as_double(); - } else if (name == node_name + ".footprint_model.rear_offset") { - reload_footprint = true; - rear_offset = parameter.as_double(); - } else if (name == node_name + ".footprint_model.rear_radius") { - reload_footprint = true; - rear_radius = parameter.as_double(); - } - } - - else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) { - if (name == node_name + ".footprint_model.line_start") { - reload_footprint = true; - line_start = parameter.as_double_array(); - } else if (name == node_name + ".footprint_model.line_end") { - reload_footprint = true; - line_end = parameter.as_double_array(); - } - } - - else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER) { - // Trajectory - if (name == node_name + ".min_samples") { - trajectory.min_samples = parameter.as_int(); - } else if (name == node_name + ".max_samples") { - trajectory.max_samples = parameter.as_int(); - } else if (name == node_name + ".feasibility_check_no_poses") { - trajectory.feasibility_check_no_poses = parameter.as_int(); - } else if (name == node_name + ".control_look_ahead_poses") { - trajectory.control_look_ahead_poses = parameter.as_int(); - } - // Robot - // GoalTolerance - // Obstacles - else if (name == node_name + ".obstacle_poses_affected") { - obstacles.obstacle_poses_affected = parameter.as_int(); - } else if (name == node_name + ".costmap_converter_rate") { - obstacles.costmap_converter_rate = parameter.as_int(); - } - // Optimization - else if (name == node_name + ".no_inner_iterations") { - optim.no_inner_iterations = parameter.as_int(); - } else if (name == node_name + ".no_outer_iterations") { - optim.no_outer_iterations = parameter.as_int(); - } - // Homotopy Class Planner - else if (name == node_name + ".max_number_classes") { - hcp.max_number_classes = parameter.as_int(); - } else if (name == node_name + ".roadmap_graph_no_samples") { - hcp.roadmap_graph_no_samples = parameter.as_int(); - } - // Recovery - } - - else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { - // Trajectory - if (name == node_name + ".global_plan_overwrite_orientation") { - trajectory.global_plan_overwrite_orientation = parameter.as_bool(); - } else if (name == node_name + ".allow_init_with_backwards_motion") { - trajectory.allow_init_with_backwards_motion = parameter.as_bool(); - } else if (name == node_name + ".via_points_ordered") { - trajectory.via_points_ordered = parameter.as_bool(); - } else if (name == node_name + ".exact_arc_length") { - trajectory.exact_arc_length = parameter.as_bool(); - } else if (name == node_name + ".publish_feedback") { - trajectory.publish_feedback = parameter.as_bool(); - } - // Robot - else if (name == node_name + ".cmd_angle_instead_rotvel") { - robot.cmd_angle_instead_rotvel = parameter.as_bool(); - } else if (name == node_name + ".is_footprint_dynamic") { - robot.is_footprint_dynamic = parameter.as_bool(); - } - // GoalTolerance - else if (name == node_name + ".free_goal_vel") { - goal_tolerance.free_goal_vel = parameter.as_bool(); - } - // Obstacles - else if (name == node_name + ".include_dynamic_obstacles") { - obstacles.include_dynamic_obstacles = parameter.as_bool(); - } else if (name == node_name + ".include_costmap_obstacles") { - obstacles.include_costmap_obstacles = parameter.as_bool(); - } else if (name == node_name + ".legacy_obstacle_association") { - obstacles.legacy_obstacle_association = parameter.as_bool(); - } else if (name == node_name + ".costmap_converter_spin_thread") { - obstacles.costmap_converter_spin_thread = parameter.as_bool(); - } - // Optimization - else if (name == node_name + ".optimization_activate") { - optim.optimization_activate = parameter.as_bool(); - } else if (name == node_name + ".optimization_verbose") { - optim.optimization_verbose = parameter.as_bool(); - } - // Homotopy Class Planner - else if (name == node_name + ".enable_homotopy_class_planning") { - hcp.enable_homotopy_class_planning = parameter.as_bool(); - } else if (name == node_name + ".enable_multithreading") { - hcp.enable_multithreading = parameter.as_bool(); - } else if (name == node_name + ".simple_exploration") { - hcp.simple_exploration = parameter.as_bool(); - } else if (name == node_name + ".selection_alternative_time_cost") { - hcp.selection_alternative_time_cost = parameter.as_bool(); - } else if (name == node_name + ".viapoints_all_candidates") { - hcp.viapoints_all_candidates = parameter.as_bool(); - } else if (name == node_name + ".visualize_hc_graph") { - hcp.visualize_hc_graph = parameter.as_bool(); - } else if (name == node_name + ".delete_detours_backwards") { - hcp.delete_detours_backwards = parameter.as_bool(); - } - // Recovery - else if (name == node_name + ".shrink_horizon_backup") { - recovery.shrink_horizon_backup = parameter.as_bool(); - } else if (name == node_name + ".oscillation_recovery") { - recovery.oscillation_recovery = parameter.as_bool(); - } else if (name == node_name + ".divergence_detection_enable") { - recovery.divergence_detection_enable = parameter.as_bool(); - } - } - - else if (type == rcl_interfaces::msg::ParameterType::PARAMETER_STRING) { - // Trajectory - // Robot - // GoalTolerance - // Obstacles - if (name == node_name + ".costmap_converter_plugin") { - obstacles.costmap_converter_plugin = parameter.as_string(); - } - // Optimization - // Homotopy Class Planner - // Recovery - // Footprint model - else if (name == node_name + ".footprint_model.type") { - RCLCPP_WARN(logger_, "Changing footprint model type is not allowed at runtime"); - } else if (name == node_name + ".footprint_model.vertices") { - reload_footprint = true; - footprint_string = parameter.as_string(); - } - } - } - checkParameters(); - - if (reload_footprint) { - if (model_name.compare("circular") == 0) - { - RCLCPP_INFO(logger_, "Footprint model 'circular' (radius: %fm) reloaded for trajectory optimization.", radius); - robot_model = std::make_shared(radius); - } - else if (model_name.compare("line") == 0) { - if (line_start.size() != 2 || line_end.size() != 2) - { - RCLCPP_ERROR(logger_, "Footprint model 'line' cannot be reloaded for trajectory optimization"); - } else { - RCLCPP_INFO(logger_, - "Footprint model 'line' (line_start: [%f,%f]m, line_end: [%f,%f]m) reloaded for trajectory optimization.", - line_start[0], line_start[1], line_end[0], line_end[1]); - - robot_model = std::make_shared(Eigen::Map(line_start.data()), Eigen::Map(line_end.data())); - } - } - else if (model_name.compare("two_circles") == 0) { - RCLCPP_INFO(logger_, - "Footprint model 'two_circles' (front_offset: %fm, front_radius: %fm, rear_offset: %fm, rear_radius: %fm) loaded for trajectory optimization.", - front_offset, front_radius, rear_offset, rear_radius); - - robot_model = std::make_shared(front_offset, front_radius, rear_offset, rear_radius); - } - else if (model_name.compare("polygon") == 0) - { - std::vector footprint; - // get vertices - if (nav2_costmap_2d::makeFootprintFromString(footprint_string, footprint)) - { - Point2dContainer polygon; - for(const auto &pt : footprint) { - polygon.push_back(Eigen::Vector2d(pt.x, pt.y)); - } - RCLCPP_INFO(logger_, "Footprint model 'polygon' reloaded for trajectory optimization."); - robot_model = std::make_shared(polygon); - } - else - { - RCLCPP_ERROR(logger_, - "Footprint model 'polygon' cannot be reloaded for trajectory optimization, since param 'footprint_model.vertices' does not define an array of coordinates."); - } - } - } - result.successful = true; - return result; -} - - -void TebConfig::checkParameters() const -{ - //rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")}; - // positive backward velocity? - if (robot.max_vel_x_backwards <= 0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving."); - - // bounds smaller than penalty epsilon - if (robot.max_vel_x <= optim.penalty_epsilon) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.max_vel_x_backwards <= optim.penalty_epsilon) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_x_backwards <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.max_vel_theta <= optim.penalty_epsilon) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: max_vel_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.acc_lim_x <= optim.penalty_epsilon) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: acc_lim_x <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - if (robot.acc_lim_theta <= optim.penalty_epsilon) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: acc_lim_theta <= penalty_epsilon. The resulting bound is negative. Undefined behavior... Change at least one of them!"); - - // dt_ref and dt_hyst - if (trajectory.dt_ref <= trajectory.dt_hysteresis) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: dt_ref <= dt_hysteresis. The hysteresis is not allowed to be greater or equal!. Undefined behavior... Change at least one of them!"); - - // min number of samples - if (trajectory.min_samples <3) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter min_samples is smaller than 3! Sorry, I haven't enough degrees of freedom to plan a trajectory for you. Please increase ..."); - - // costmap obstacle behind robot - if (obstacles.costmap_obstacles_behind_robot_dist < 0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter 'costmap_obstacles_behind_robot_dist' should be positive or zero."); - - // hcp: obstacle heading threshold - if (hcp.obstacle_keypoint_offset>=1 || hcp.obstacle_keypoint_offset<=0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter obstacle_heading_threshold must be in the interval ]0,1[. 0=0deg opening angle, 1=90deg opening angle."); - - // carlike - if (robot.cmd_angle_instead_rotvel && robot.wheelbase==0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but wheelbase is set to zero: undesired behavior."); - - if (robot.cmd_angle_instead_rotvel && robot.min_turning_radius==0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter cmd_angle_instead_rotvel is non-zero but min_turning_radius is set to zero: undesired behavior. You are mixing a carlike and a diffdrive robot"); - - // positive weight_adapt_factor - if (optim.weight_adapt_factor < 1.0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter weight_adapt_factor shoud be >= 1.0"); - - if (recovery.oscillation_filter_duration < 0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter oscillation_filter_duration must be >= 0"); - - if (optim.weight_optimaltime <= 0) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: parameter weight_optimaltime shoud be > 0 (even if weight_shortest_path is in use)"); -} - -void TebConfig::checkDeprecated(const nav2_util::LifecycleNode::SharedPtr nh, const std::string name) const -{ - rclcpp::Parameter dummy; - - if (nh->get_parameter(name + "." + "line_obstacle_poses_affected", dummy) || nh->get_parameter(name + "." + "polygon_obstacle_poses_affected", dummy)) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'line_obstacle_poses_affected' and 'polygon_obstacle_poses_affected' are deprecated. They share now the common parameter 'obstacle_poses_affected'."); - - if (nh->get_parameter(name + "." + "weight_point_obstacle", dummy) || nh->get_parameter(name + "." + "weight_line_obstacle", dummy) || nh->get_parameter(name + "." + "weight_poly_obstacle", dummy)) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'weight_point_obstacle', 'weight_line_obstacle' and 'weight_poly_obstacle' are deprecated. They are replaced by the single param 'weight_obstacle'."); - - if (nh->get_parameter(name + "." + "costmap_obstacles_front_only", dummy)) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'costmap_obstacles_front_only' is deprecated. It is replaced by 'costmap_obstacles_behind_robot_dist' to define the actual area taken into account."); - - if (nh->get_parameter(name + "." + "costmap_emergency_stop_dist", dummy)) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'costmap_emergency_stop_dist' is deprecated. You can safely remove it from your parameter config."); - - if (nh->get_parameter(name + "." + "alternative_time_cost", dummy)) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'alternative_time_cost' is deprecated. It has been replaced by 'selection_alternative_time_cost'."); - - if (nh->get_parameter(name + "." + "global_plan_via_point_sep", dummy)) - RCLCPP_WARN(logger_, "TebLocalPlannerROS() Param Warning: 'global_plan_via_point_sep' is deprecated. It has been replaced by 'global_plan_viapoint_sep' due to consistency reasons."); -} - - -} // namespace teb_local_planner diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp deleted file mode 100644 index 6060d8b..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/teb_local_planner_ros.cpp +++ /dev/null @@ -1,1122 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/teb_local_planner_ros.h" - -//#include -#include - -#include - -// pluginlib macros -#include - -#include "g2o/core/sparse_optimizer.h" -#include "g2o/core/block_solver.h" -#include "g2o/core/factory.h" -#include "g2o/core/optimization_algorithm_gauss_newton.h" -#include "g2o/core/optimization_algorithm_levenberg.h" -#include "g2o/solvers/csparse/linear_solver_csparse.h" -#include "g2o/solvers/cholmod/linear_solver_cholmod.h" - -#include -#include -#include - -#include -#include - -using nav2_util::declare_parameter_if_not_declared; - -namespace teb_local_planner -{ - - -TebLocalPlannerROS::TebLocalPlannerROS() - : costmap_ros_(nullptr), tf_(nullptr), cfg_(new TebConfig()), costmap_model_(nullptr), intra_proc_node_(nullptr), - costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), - custom_via_points_active_(false), no_infeasible_plans_(0), - last_preferred_rotdir_(RotType::none), initialized_(false) -{ -} - - -TebLocalPlannerROS::~TebLocalPlannerROS() -{ -} - -void TebLocalPlannerROS::initialize(nav2_util::LifecycleNode::SharedPtr node) -{ - // check if the plugin is already initialized - if(!initialized_) - { - // declare parameters (ros2-dashing) - intra_proc_node_.reset( - new rclcpp::Node("costmap_converter", node->get_namespace(), - rclcpp::NodeOptions())); - cfg_->declareParameters(node, name_); - - // get parameters of TebConfig via the nodehandle and override the default config - cfg_->loadRosParamFromNodeHandle(node, name_); - - // reserve some memory for obstacles - obstacles_.reserve(500); - - // create the planner instance - if (cfg_->hcp.enable_homotopy_class_planning) - { - planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(node, *cfg_.get(), &obstacles_, visualization_, &via_points_)); - RCLCPP_INFO(logger_, "Parallel planning in distinctive topologies enabled."); - } - else - { - planner_ = PlannerInterfacePtr(new TebOptimalPlanner(node, *cfg_.get(), &obstacles_, visualization_, &via_points_)); - RCLCPP_INFO(logger_, "Parallel planning in distinctive topologies disabled."); - } - - // init other variables - costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase. - - costmap_model_ = std::make_shared(); - std::string costmap_model_name("costmap_model"); - costmap_model_->initialize(node, costmap_model_name, name_, costmap_ros_); - - cfg_->map_frame = costmap_ros_->getGlobalFrameID(); // TODO - - //Initialize a costmap to polygon converter - if (!cfg_->obstacles.costmap_converter_plugin.empty()) - { - try - { - costmap_converter_ = costmap_converter_loader_.createSharedInstance(cfg_->obstacles.costmap_converter_plugin); - std::string converter_name = costmap_converter_loader_.getName(cfg_->obstacles.costmap_converter_plugin); - RCLCPP_INFO(logger_, "library path : %s", costmap_converter_loader_.getClassLibraryPath(cfg_->obstacles.costmap_converter_plugin).c_str()); - // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace - boost::replace_all(converter_name, "::", "/"); - - costmap_converter_->setOdomTopic(cfg_->odom_topic); - costmap_converter_->initialize(intra_proc_node_); - costmap_converter_->setCostmap2D(costmap_); - const auto rate = std::make_shared((double)cfg_->obstacles.costmap_converter_rate); - costmap_converter_->startWorker(rate, costmap_, cfg_->obstacles.costmap_converter_spin_thread); - RCLCPP_INFO(logger_, "Costmap conversion plugin %s loaded.", cfg_->obstacles.costmap_converter_plugin.c_str()); - } - catch(pluginlib::PluginlibException& ex) - { - RCLCPP_INFO(logger_, - "The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what()); - costmap_converter_.reset(); - } - } - else { - RCLCPP_INFO(logger_, "No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); - } - - - // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. - footprint_spec_ = costmap_ros_->getRobotFootprint(); - nav2_costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); - - // Add callback for dynamic parameters - dyn_params_handler = node->add_on_set_parameters_callback( - std::bind(&TebConfig::dynamicParametersCallback, std::ref(cfg_), std::placeholders::_1)); - - // validate optimization footprint and costmap footprint - validateFootprints(cfg_->robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_->obstacles.min_obstacle_dist); - - // setup callback for custom obstacles - custom_obst_sub_ = node->create_subscription( - "obstacles", - rclcpp::SystemDefaultsQoS(), - std::bind(&TebLocalPlannerROS::customObstacleCB, this, std::placeholders::_1)); - - // setup callback for custom via-points - via_points_sub_ = node->create_subscription( - "via_points", - rclcpp::SystemDefaultsQoS(), - std::bind(&TebLocalPlannerROS::customViaPointsCB, this, std::placeholders::_1)); - - // initialize failure detector - //rclcpp::Node::SharedPtr nh_move_base("~"); - double controller_frequency = 5; - node->get_parameter("controller_frequency", controller_frequency); - failure_detector_.setBufferLength(std::round(cfg_->recovery.oscillation_filter_duration*controller_frequency)); - - // set initialized flag - initialized_ = true; - - // This should be called since to prevent different time sources exception - time_last_infeasible_plan_ = clock_->now(); - time_last_oscillation_ = clock_->now(); - RCLCPP_DEBUG(logger_, "teb_local_planner plugin initialized."); - } - else - { - RCLCPP_INFO(logger_, "teb_local_planner has already been initialized, doing nothing."); - } -} - -void TebLocalPlannerROS::configure( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, - std::shared_ptr tf, - std::shared_ptr costmap_ros) { - nh_ = parent; - - auto node = nh_.lock(); - logger_ = node->get_logger(); - clock_ = node->get_clock(); - - costmap_ros_ = costmap_ros; - tf_ = tf; - name_ = name; - - initialize(node); - visualization_ = std::make_shared(node, *cfg_); - visualization_->on_configure(); - planner_->setVisualization(visualization_); - - return; -} - -void TebLocalPlannerROS::setPlan(const nav_msgs::msg::Path & orig_global_plan) -{ - // check if plugin is initialized - if(!initialized_) - { - RCLCPP_ERROR(logger_, "teb_local_planner has not been initialized, please call initialize() before using this planner"); - return; - } - - // store the global plan - global_plan_.clear(); - global_plan_.reserve(orig_global_plan.poses.size()); - for(const auto &in_pose :orig_global_plan.poses) { - geometry_msgs::msg::PoseStamped out_pose; - out_pose.pose = in_pose.pose; - out_pose.header = orig_global_plan.header; - global_plan_.push_back(out_pose); - } - - // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. - // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. - - return; -} - - -geometry_msgs::msg::TwistStamped TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose, - const geometry_msgs::msg::Twist &velocity, nav2_core::GoalChecker *goal_checker) -{ - // check if plugin initialized - if(!initialized_) - { - throw nav2_core::PlannerException( - std::string("teb_local_planner has not been initialized, please call initialize() before using this planner") - ); - } - geometry_msgs::msg::TwistStamped cmd_vel; - - cmd_vel.header.stamp = clock_->now(); - cmd_vel.header.frame_id = costmap_ros_->getBaseFrameID(); - cmd_vel.twist.linear.x = 0; - cmd_vel.twist.linear.y = 0; - cmd_vel.twist.angular.z = 0; - - // Update for the current goal checker's state - geometry_msgs::msg::Pose pose_tolerance; - geometry_msgs::msg::Twist vel_tolerance; - if (!goal_checker->getTolerances(pose_tolerance, vel_tolerance)) { - RCLCPP_WARN(logger_, "Unable to retrieve goal checker's tolerances!"); - } else { - cfg_->goal_tolerance.xy_goal_tolerance = pose_tolerance.position.x; - } - - // Get robot pose - robot_pose_ = PoseSE2(pose.pose); - geometry_msgs::msg::PoseStamped robot_pose; - robot_pose.header = pose.header; - robot_pose_.toPoseMsg(robot_pose.pose); - - // Get robot velocity - robot_vel_ = velocity; - - // prune global plan to cut off parts of the past (spatially before the robot) - pruneGlobalPlan(robot_pose, global_plan_, cfg_->trajectory.global_plan_prune_distance); - - // Transform global plan to the frame of interest (w.r.t. the local costmap) - std::vector transformed_plan; - int goal_idx; - geometry_msgs::msg::TransformStamped tf_plan_to_global; - if (!transformGlobalPlan(global_plan_, robot_pose, *costmap_, cfg_->map_frame, cfg_->trajectory.max_global_plan_lookahead_dist, - transformed_plan, &goal_idx, &tf_plan_to_global)) - { - throw nav2_core::PlannerException( - std::string("Could not transform the global plan to the frame of the controller") - ); - } - - // update via-points container - if (!custom_via_points_active_) - updateViaPointsContainer(transformed_plan, cfg_->trajectory.global_plan_viapoint_sep); - - // check if we should enter any backup mode and apply settings - configureBackupModes(transformed_plan, goal_idx); - - // Return false if the transformed global plan is empty - if (transformed_plan.empty()) - { - throw nav2_core::PlannerException( - std::string("Transformed plan is empty. Cannot determine a local plan.") - ); - } - - // Get current goal point (last point of the transformed plan) - const geometry_msgs::msg::PoseStamped &goal_point = transformed_plan.back(); - robot_goal_.x() = goal_point.pose.position.x; - robot_goal_.y() = goal_point.pose.position.y; - if (cfg_->trajectory.global_plan_overwrite_orientation) - { - robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, goal_point, goal_idx, tf_plan_to_global); - // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization) - //transformed_plan.back().pose.orientation = tf::createQuaternionMsgFromYaw(robot_goal_.theta()); - tf2::Quaternion q; - q.setRPY(0, 0, robot_goal_.theta()); - transformed_plan.back().pose.orientation = tf2::toMsg(q); - } - else - { - robot_goal_.theta() = tf2::getYaw(goal_point.pose.orientation); - } - - // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory) - if (transformed_plan.size()==1) // plan only contains the goal - { - transformed_plan.insert(transformed_plan.begin(), geometry_msgs::msg::PoseStamped()); // insert start (not yet initialized) - } - //tf::poseTFToMsg(robot_pose, transformed_plan.front().pose); // update start; - transformed_plan.front().pose = robot_pose.pose; - - // clear currently existing obstacles - obstacles_.clear(); - - // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin - if (costmap_converter_) - updateObstacleContainerWithCostmapConverter(); - else - updateObstacleContainerWithCostmap(); - - // also consider custom obstacles (must be called after other updates, since the container is not cleared) - updateObstacleContainerWithCustomObstacles(); - - - // Do not allow config changes during the following optimization step - std::lock_guard cfg_lock(cfg_->configMutex()); - - // Now perform the actual planning -// bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_->goal_tolerance.free_goal_vel); // straight line init - bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_->goal_tolerance.free_goal_vel); - if (!success) - { - planner_->clearPlanner(); // force reinitialization for next time - - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = clock_->now(); - last_cmd_ = cmd_vel.twist; - - throw nav2_core::PlannerException( - std::string("teb_local_planner was not able to obtain a local plan for the current setting.") - ); - } - - // Check for divergence - if (planner_->hasDiverged()) - { - cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; - - // Reset everything to start again with the initialization of new trajectories. - planner_->clearPlanner(); - RCLCPP_WARN_THROTTLE(logger_, *(clock_), 1, "TebLocalPlannerROS: the trajectory has diverged. Resetting planner..."); - - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = clock_->now(); - last_cmd_ = cmd_vel.twist; - throw nav2_core::PlannerException( - std::string("TebLocalPlannerROS: velocity command invalid (hasDiverged). Resetting planner...") - ); - } - - // Check feasibility (but within the first few states only) - if(cfg_->robot.is_footprint_dynamic) - { - // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. - std::vector updated_footprint_spec_ = costmap_ros_->getRobotFootprint(); - if (updated_footprint_spec_ != footprint_spec_) { - updated_footprint_spec_ = footprint_spec_; - nav2_costmap_2d::calculateMinAndMaxDistances(updated_footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); - } - } - - bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_->trajectory.feasibility_check_no_poses, cfg_->trajectory.feasibility_check_lookahead_distance); - if (!feasible) - { - cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; - - // now we reset everything to start again with the initialization of new trajectories. - planner_->clearPlanner(); - - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = clock_->now(); - last_cmd_ = cmd_vel.twist; - - throw nav2_core::PlannerException( - std::string("TebLocalPlannerROS: trajectory is not feasible. Resetting planner...") - ); - } - - // Get the velocity command for this sampling interval - if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->trajectory.control_look_ahead_poses)) - { - planner_->clearPlanner(); - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = clock_->now(); - last_cmd_ = cmd_vel.twist; - - throw nav2_core::PlannerException( - std::string("TebLocalPlannerROS: velocity command invalid. Resetting planner...") - ); - } - - // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints). - saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_->robot.max_vel_x, cfg_->robot.max_vel_y, - cfg_->robot.max_vel_theta, cfg_->robot.max_vel_x_backwards); - - // convert rot-vel to steering angle if desired (carlike robot). - // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint - // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself. - if (cfg_->robot.cmd_angle_instead_rotvel) - { - cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z, cfg_->robot.wheelbase, 0.95*cfg_->robot.min_turning_radius); - if (!std::isfinite(cmd_vel.twist.angular.z)) - { - cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; - last_cmd_ = cmd_vel.twist; - planner_->clearPlanner(); - - ++no_infeasible_plans_; // increase number of infeasible solutions in a row - time_last_infeasible_plan_ = clock_->now(); - - throw nav2_core::PlannerException( - std::string("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner...") - ); - } - } - - // a feasible solution should be found, reset counter - no_infeasible_plans_ = 0; - - // store last command (for recovery analysis etc.) - last_cmd_ = cmd_vel.twist; - - // Now visualize everything - planner_->visualize(); - visualization_->publishObstacles(obstacles_); - visualization_->publishViaPoints(via_points_); - visualization_->publishGlobalPlan(global_plan_); - - return cmd_vel; -} - -void TebLocalPlannerROS::updateObstacleContainerWithCostmap() -{ - // Add costmap obstacles if desired - if (cfg_->obstacles.include_costmap_obstacles) - { - std::lock_guard lock(*costmap_->getMutex()); - - Eigen::Vector2d robot_orient = robot_pose_.orientationUnitVec(); - - for (unsigned int i=0; igetSizeInCellsX()-1; ++i) - { - for (unsigned int j=0; jgetSizeInCellsY()-1; ++j) - { - if (costmap_->getCost(i,j) == nav2_costmap_2d::LETHAL_OBSTACLE) - { - Eigen::Vector2d obs; - costmap_->mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1)); - - // check if obstacle is interesting (e.g. not far behind the robot) - Eigen::Vector2d obs_dir = obs-robot_pose_.position(); - if ( obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > cfg_->obstacles.costmap_obstacles_behind_robot_dist ) - continue; - - obstacles_.push_back(ObstaclePtr(new PointObstacle(obs))); - } - } - } - } -} - -void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter() -{ - if (!costmap_converter_) - return; - - //Get obstacles from costmap converter - costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles(); - if (!obstacles) - return; - - for (std::size_t i=0; iobstacles.size(); ++i) - { - const costmap_converter_msgs::msg::ObstacleMsg* obstacle = &obstacles->obstacles.at(i); - const geometry_msgs::msg::Polygon* polygon = &obstacle->polygon; - - if (polygon->points.size()==1 && obstacle->radius > 0) // Circle - { - obstacles_.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius))); - } - else if (polygon->points.size()==1) // Point - { - obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y))); - } - else if (polygon->points.size()==2) // Line - { - obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y, - polygon->points[1].x, polygon->points[1].y ))); - } - else if (polygon->points.size()>2) // Real polygon - { - PolygonObstacle* polyobst = new PolygonObstacle; - for (std::size_t j=0; jpoints.size(); ++j) - { - polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y); - } - polyobst->finalizePolygon(); - obstacles_.push_back(ObstaclePtr(polyobst)); - } - - // Set velocity, if obstacle is moving - if(!obstacles_.empty()) - obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation); - } -} - - -void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() -{ - // Add custom obstacles obtained via message - std::lock_guard l(custom_obst_mutex_); - - if (!custom_obstacle_msg_.obstacles.empty()) - { - // We only use the global header to specify the obstacle coordinate system instead of individual ones - Eigen::Affine3d obstacle_to_map_eig; - try - { - geometry_msgs::msg::TransformStamped obstacle_to_map = tf_->lookupTransform( - cfg_->map_frame, tf2::timeFromSec(0), - custom_obstacle_msg_.header.frame_id, tf2::timeFromSec(0), - custom_obstacle_msg_.header.frame_id, tf2::durationFromSec(0.5)); - obstacle_to_map_eig = tf2::transformToEigen(obstacle_to_map); - //tf2::fromMsg(obstacle_to_map.transform, obstacle_to_map_eig); - } - catch (tf2::TransformException ex) - { - RCLCPP_ERROR(logger_, "%s",ex.what()); - obstacle_to_map_eig.setIdentity(); - } - - for (size_t i=0; i 0 ) // circle - { - Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x, - custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y, - custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z ); - obstacles_.push_back(ObstaclePtr(new CircularObstacle( (obstacle_to_map_eig * pos).head(2), custom_obstacle_msg_.obstacles.at(i).radius))); - } - else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 ) // point - { - Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x, - custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y, - custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z ); - obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) ))); - } - else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 2 ) // line - { - Eigen::Vector3d line_start( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x, - custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y, - custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z ); - Eigen::Vector3d line_end( custom_obstacle_msg_.obstacles.at(i).polygon.points.back().x, - custom_obstacle_msg_.obstacles.at(i).polygon.points.back().y, - custom_obstacle_msg_.obstacles.at(i).polygon.points.back().z ); - obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2), - (obstacle_to_map_eig * line_end).head(2) ))); - } - else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.empty()) - { - RCLCPP_INFO(logger_, "Invalid custom obstacle received. List of polygon vertices is empty. Skipping..."); - continue; - } - else // polygon - { - PolygonObstacle* polyobst = new PolygonObstacle; - for (size_t j=0; jpushBackVertex( (obstacle_to_map_eig * pos).head(2) ); - } - polyobst->finalizePolygon(); - obstacles_.push_back(ObstaclePtr(polyobst)); - } - - // Set velocity, if obstacle is moving - if(!obstacles_.empty()) - obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation); - } - } -} - -void TebLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) -{ - via_points_.clear(); - - if (min_separation<=0) - return; - - std::size_t prev_idx = 0; - for (std::size_t i=1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m] - { - // check separation to the previous via-point inserted - if (distance_points2d( transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position ) < min_separation) - continue; - - // add via-point - via_points_.push_back( Eigen::Vector2d( transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y ) ); - prev_idx = i; - } - -} - -//Eigen::Vector2d TebLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel) -//{ -// Eigen::Vector2d vel; -// vel.coeffRef(0) = std::sqrt( tf_vel.getOrigin().getX() * tf_vel.getOrigin().getX() + tf_vel.getOrigin().getY() * tf_vel.getOrigin().getY() ); -// vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation()); -// return vel; -//} - - -bool TebLocalPlannerROS::pruneGlobalPlan(const geometry_msgs::msg::PoseStamped& global_pose, std::vector& global_plan, double dist_behind_robot) -{ - if (global_plan.empty()) - return true; - - try - { - // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times) - //geometry_msgs::msg::TransformStamped global_to_plan_transform = tf_->lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, tf2::timeFromSec(0)); - geometry_msgs::msg::PoseStamped robot = tf_->transform( - global_pose, - global_plan.front().header.frame_id); - - //robot.setData( global_to_plan_transform * global_pose ); - - double dist_thresh_sq = dist_behind_robot*dist_behind_robot; - - // iterate plan until a pose close the robot is found - std::vector::iterator it = global_plan.begin(); - std::vector::iterator erase_end = it; - while (it != global_plan.end()) - { - double dx = robot.pose.position.x - it->pose.position.x; - double dy = robot.pose.position.y - it->pose.position.y; - double dist_sq = dx * dx + dy * dy; - if (dist_sq < dist_thresh_sq) - { - erase_end = it; - break; - } - ++it; - } - if (erase_end == global_plan.end()) - return false; - - if (erase_end != global_plan.begin()) - global_plan.erase(global_plan.begin(), erase_end); - } - catch (const tf2::TransformException& ex) - { - RCLCPP_DEBUG(logger_, "Cannot prune path since no transform is available: %s\n", ex.what()); - return false; - } - return true; -} - - -bool TebLocalPlannerROS::transformGlobalPlan(const std::vector& global_plan, - const geometry_msgs::msg::PoseStamped& global_pose, const nav2_costmap_2d::Costmap2D& costmap, const std::string& global_frame, double max_plan_length, - std::vector& transformed_plan, int* current_goal_idx, geometry_msgs::msg::TransformStamped* tf_plan_to_global) const -{ - // this method is a slightly modified version of base_local_planner/goal_functions.h - - const geometry_msgs::msg::PoseStamped& plan_pose = global_plan[0]; - - transformed_plan.clear(); - - try - { - if (global_plan.empty()) - { - RCLCPP_ERROR(logger_, "Received plan with zero length"); - *current_goal_idx = 0; - return false; - } - - // get plan_to_global_transform from plan frame to global_frame - geometry_msgs::msg::TransformStamped plan_to_global_transform = tf_->lookupTransform( - global_frame, tf2_ros::fromMsg(plan_pose.header.stamp), - plan_pose.header.frame_id, tf2::timeFromSec(0), - plan_pose.header.frame_id, tf2::durationFromSec(0.5)); - -// tf_->waitForTransform(global_frame, ros::Time::now(), -// plan_pose.header.frame_id, plan_pose.header.stamp, -// plan_pose.header.frame_id, ros::Duration(0.5)); -// tf_->lookupTransform(global_frame, ros::Time(), -// plan_pose.header.frame_id, plan_pose.header.stamp, -// plan_pose.header.frame_id, plan_to_global_transform); - - //let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose = tf_->transform(global_pose, plan_pose.header.frame_id); - - //we'll discard points on the plan that are outside the local costmap - double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, - costmap.getSizeInCellsY() * costmap.getResolution() / 2.0); - dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are - // located on the border of the local costmap - - - int i = 0; - double sq_dist_threshold = dist_threshold * dist_threshold; - double sq_dist = 1e10; - - //we need to loop to a point on the plan that is within a certain distance of the robot - bool robot_reached = false; - for(int j=0; j < (int)global_plan.size(); ++j) - { - double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x; - double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y; - double new_sq_dist = x_diff * x_diff + y_diff * y_diff; - if (new_sq_dist > sq_dist_threshold) - break; // force stop if we have reached the costmap border - - if (robot_reached && new_sq_dist > sq_dist) - break; - - if (new_sq_dist < sq_dist) // find closest distance - { - sq_dist = new_sq_dist; - i = j; - if (sq_dist < 0.05) // 2.5 cm to the robot; take the immediate local minima; if it's not the global - robot_reached = true; // minima, probably means that there's a loop in the path, and so we prefer this - } - } - - geometry_msgs::msg::PoseStamped newer_pose; - - double plan_length = 0; // check cumulative Euclidean distance along the plan - - //now we'll transform until points are outside of our distance threshold - while(i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length<=0 || plan_length <= max_plan_length)) - { - //const geometry_msgs::msg::PoseStamped& pose = global_plan[i]; - //tf::poseStampedMsgToTF(pose, tf_pose); - //tf_pose.setData(plan_to_global_transform * tf_pose); - tf2::doTransform(global_plan[i], newer_pose, plan_to_global_transform); - -// tf_pose.stamp_ = plan_to_global_transform.stamp_; -// tf_pose.frame_id_ = global_frame; -// tf::poseStampedTFToMsg(tf_pose, newer_pose); - - transformed_plan.push_back(newer_pose); - - double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x; - double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y; - sq_dist = x_diff * x_diff + y_diff * y_diff; - - // caclulate distance to previous pose - if (i>0 && max_plan_length>0) - plan_length += distance_points2d(global_plan[i-1].pose.position, global_plan[i].pose.position); - - ++i; - } - - // if we are really close to the goal (>0) - // the resulting transformed plan can be empty. In that case we explicitly inject the global goal. - if (transformed_plan.empty()) - { -// tf::poseStampedMsgToTF(global_plan.back(), tf_pose); -// tf_pose.setData(plan_to_global_transform * tf_pose); -// tf_pose.stamp_ = plan_to_global_transform.stamp_; -// tf_pose.frame_id_ = global_frame; -// tf::poseStampedTFToMsg(tf_pose, newer_pose); - tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform); - - transformed_plan.push_back(newer_pose); - - // Return the index of the current goal point (inside the distance threshold) - if (current_goal_idx) *current_goal_idx = int(global_plan.size())-1; - } - else - { - // Return the index of the current goal point (inside the distance threshold) - if (current_goal_idx) *current_goal_idx = i-1; // subtract 1, since i was increased once before leaving the loop - } - - // Return the transformation from the global plan to the global planning frame if desired - if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform; - } - catch(tf2::LookupException& ex) - { - RCLCPP_ERROR(logger_, "No Transform available Error: %s\n", ex.what()); - return false; - } - catch(tf2::ConnectivityException& ex) - { - RCLCPP_ERROR(logger_, "Connectivity Error: %s\n", ex.what()); - return false; - } - catch(tf2::ExtrapolationException& ex) - { - RCLCPP_ERROR(logger_, "Extrapolation Error: %s\n", ex.what()); - if (global_plan.size() > 0) - RCLCPP_ERROR(logger_, "Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str()); - - return false; - } - - return true; -} - - - - -double TebLocalPlannerROS::estimateLocalGoalOrientation(const std::vector& global_plan, const geometry_msgs::msg::PoseStamped& local_goal, - int current_goal_idx, const geometry_msgs::msg::TransformStamped& tf_plan_to_global, int moving_average_length) const -{ - int n = (int)global_plan.size(); - - // check if we are near the global goal already - if (current_goal_idx > n-moving_average_length-2) - { - if (current_goal_idx >= n-1) // we've exactly reached the goal - { - return tf2::getYaw(local_goal.pose.orientation); - } - else - { -// tf::Quaternion global_orientation; -// tf::quaternionMsgToTF(global_plan.back().pose.orientation, global_orientation); -// return tf2::getYaw(tf_plan_to_global.getRotation() * global_orientation ); - - tf2::Quaternion global_orientation, tf_plan_to_global_orientation; - tf2::fromMsg(global_plan.back().pose.orientation, global_orientation); - tf2::fromMsg(tf_plan_to_global.transform.rotation, tf_plan_to_global_orientation); - - return tf2::getYaw(tf_plan_to_global_orientation * global_orientation); - } - } - - // reduce number of poses taken into account if the desired number of poses is not available - moving_average_length = std::min(moving_average_length, n-current_goal_idx-1 ); // maybe redundant, since we have checked the vicinity of the goal before - - std::vector candidates; - geometry_msgs::msg::PoseStamped tf_pose_k = local_goal; - geometry_msgs::msg::PoseStamped tf_pose_kp1; - - int range_end = current_goal_idx + moving_average_length; - for (int i = current_goal_idx; i < range_end; ++i) - { - // Transform pose of the global plan to the planning frame - const geometry_msgs::msg::PoseStamped& pose = global_plan.at(i+1); - tf2::doTransform(global_plan.at(i+1), tf_pose_kp1, tf_plan_to_global); - - // calculate yaw angle - candidates.push_back( std::atan2(tf_pose_kp1.pose.position.y - tf_pose_k.pose.position.y, - tf_pose_kp1.pose.position.x - tf_pose_k.pose.position.x ) ); - - if (i max_vel_x) - ratio_x = max_vel_x / vx; - - // limit strafing velocity - if (vy > max_vel_y || vy < -max_vel_y) - ratio_y = std::abs(max_vel_y / vy); - - // Limit angular velocity - if (omega > max_vel_theta || omega < -max_vel_theta) - ratio_omega = std::abs(max_vel_theta / omega); - - // Limit backwards velocity - if (max_vel_x_backwards<=0) - { - RCLCPP_WARN_ONCE( - logger_, - "TebLocalPlannerROS(): Do not choose max_vel_x_backwards to be <=0. Disable backwards driving by increasing the optimization weight for penalyzing backwards driving."); - } - else if (vx < -max_vel_x_backwards) - ratio_x = - max_vel_x_backwards / vx; - - if (cfg_->robot.use_proportional_saturation) - { - double ratio = std::min(std::min(ratio_x, ratio_y), ratio_omega); - vx *= ratio; - vy *= ratio; - omega *= ratio; - } - else - { - vx *= ratio_x; - vy *= ratio_y; - omega *= ratio_omega; - } -} - - -double TebLocalPlannerROS::convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius) const -{ - if (omega==0 || v==0) - return 0; - - double radius = v/omega; - - if (fabs(radius) < min_turning_radius) - radius = double(g2o::sign(radius)) * min_turning_radius; - - return std::atan(wheelbase / radius); -} - - -void TebLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist) -{ - RCLCPP_WARN_EXPRESSION( - logger_, opt_inscribed_radius + min_obst_dist < costmap_inscribed_radius, - "The inscribed radius of the footprint specified for TEB optimization (%f) + min_obstacle_dist (%f) are smaller " - "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). " - "Infeasible optimziation results might occur frequently!", opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius); -} - - - -void TebLocalPlannerROS::configureBackupModes(std::vector& transformed_plan, int& goal_idx) -{ - rclcpp::Time current_time = clock_->now(); - - // reduced horizon backup mode - if (cfg_->recovery.shrink_horizon_backup && - goal_idx < (int)transformed_plan.size()-1 && // we do not reduce if the goal is already selected (because the orientation might change -> can introduce oscillations) - (no_infeasible_plans_>0 || (current_time - time_last_infeasible_plan_).seconds() < cfg_->recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds - { - RCLCPP_INFO_EXPRESSION( - logger_, - no_infeasible_plans_==1, - "Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected).", cfg_->recovery.shrink_horizon_min_duration); - - - // Shorten horizon if requested - // reduce to 50 percent: - int horizon_reduction = goal_idx/2; - - if (no_infeasible_plans_ > 9) - { - RCLCPP_INFO_EXPRESSION( - logger_, - no_infeasible_plans_==10, - "Infeasible trajectory detected 10 times in a row: further reducing horizon..."); - horizon_reduction /= 2; - } - - // we have a small overhead here, since we already transformed 50% more of the trajectory. - // But that's ok for now, since we do not need to make transformGlobalPlan more complex - // and a reduced horizon should occur just rarely. - int new_goal_idx_transformed_plan = int(transformed_plan.size()) - horizon_reduction - 1; - goal_idx -= horizon_reduction; - if (new_goal_idx_transformed_plan>0 && goal_idx >= 0) - transformed_plan.erase(transformed_plan.begin()+new_goal_idx_transformed_plan, transformed_plan.end()); - else - goal_idx += horizon_reduction; // this should not happen, but safety first ;-) - } - - - // detect and resolve oscillations - if (cfg_->recovery.oscillation_recovery) - { - double max_vel_theta; - double max_vel_current = last_cmd_.linear.x >= 0 ? cfg_->robot.max_vel_x : cfg_->robot.max_vel_x_backwards; - if (cfg_->robot.min_turning_radius!=0 && max_vel_current>0) - max_vel_theta = std::max( max_vel_current/std::abs(cfg_->robot.min_turning_radius), cfg_->robot.max_vel_theta ); - else - max_vel_theta = cfg_->robot.max_vel_theta; - - failure_detector_.update(last_cmd_, cfg_->robot.max_vel_x, cfg_->robot.max_vel_x_backwards, max_vel_theta, - cfg_->recovery.oscillation_v_eps, cfg_->recovery.oscillation_omega_eps); - - bool oscillating = failure_detector_.isOscillating(); - bool recently_oscillated = (clock_->now()-time_last_oscillation_).seconds() < cfg_->recovery.oscillation_recovery_min_duration; // check if we have already detected an oscillation recently - - if (oscillating) - { - if (!recently_oscillated) - { - // save current turning direction - if (robot_vel_.angular.z > 0) - last_preferred_rotdir_ = RotType::left; - else - last_preferred_rotdir_ = RotType::right; - RCLCPP_INFO(logger_, "TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization)."); - } - time_last_oscillation_ = clock_->now(); - planner_->setPreferredTurningDir(last_preferred_rotdir_); - } - else if (!recently_oscillated && last_preferred_rotdir_ != RotType::none) // clear recovery behavior - { - last_preferred_rotdir_ = RotType::none; - planner_->setPreferredTurningDir(last_preferred_rotdir_); - RCLCPP_INFO(logger_, "TebLocalPlannerROS: oscillation recovery disabled/expired."); - } - } - -} - - -void TebLocalPlannerROS::setSpeedLimit( - const double & speed_limit, const bool & percentage) -{ - if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { - // Restore default value - cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x; - cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards; - cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y; - cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta; - } else { - if (percentage) { - // Speed limit is expressed in % from maximum speed of robot - cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * speed_limit / 100.0; - cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0; - cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0; - cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0; - } else { - // Speed limit is expressed in absolute value - double max_speed_xy = std::max( - std::max(cfg_->robot.base_max_vel_x,cfg_->robot.base_max_vel_x_backwards),cfg_->robot.base_max_vel_y); - if (speed_limit < max_speed_xy) { - // Handling components and angular velocity changes: - // Max velocities are being changed in the same proportion - // as absolute linear speed changed in order to preserve - // robot moving trajectories to be the same after speed change. - // G. Doisy: not sure if that's applicable to base_max_vel_x_backwards. - const double ratio = speed_limit / max_speed_xy; - cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * ratio; - cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * ratio; - cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * ratio; - cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * ratio; - } - } - } -} - -void TebLocalPlannerROS::customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg) -{ - std::lock_guard l(custom_obst_mutex_); - custom_obstacle_msg_ = *obst_msg; -} - -void TebLocalPlannerROS::customViaPointsCB(const nav_msgs::msg::Path::ConstSharedPtr via_points_msg) -{ - RCLCPP_INFO_ONCE(logger_, "Via-points received. This message is printed once."); - if (cfg_->trajectory.global_plan_viapoint_sep > 0) - { - RCLCPP_INFO(logger_, "Via-points are already obtained from the global plan (global_plan_viapoint_sep>0)." - "Ignoring custom via-points."); - custom_via_points_active_ = false; - return; - } - - std::lock_guard l(via_point_mutex_); - via_points_.clear(); - for (const geometry_msgs::msg::PoseStamped& pose : via_points_msg->poses) - { - via_points_.emplace_back(pose.pose.position.x, pose.pose.position.y); - } - custom_via_points_active_ = !via_points_.empty(); -} - -void TebLocalPlannerROS::activate() { - visualization_->on_activate(); - - return; -} -void TebLocalPlannerROS::deactivate() { - visualization_->on_deactivate(); - - return; -} -void TebLocalPlannerROS::cleanup() { - visualization_->on_cleanup(); - costmap_converter_->stopWorker(); - - return; -} - -} // end namespace teb_local_planner - -// register this planner as a nav2_core::Controller plugin -PLUGINLIB_EXPORT_CLASS(teb_local_planner::TebLocalPlannerROS, nav2_core::Controller) diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/test_optim_node.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/test_optim_node.cpp deleted file mode 100644 index 94d9c02..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/test_optim_node.cpp +++ /dev/null @@ -1,322 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2017. - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/teb_local_planner_ros.h" - -//#include -#include - - -using namespace teb_local_planner; // it is ok here to import everything for testing purposes - -// ============= Global Variables ================ -// Ok global variables are bad, but here we only have a simple testing node. -PlannerInterfacePtr planner; -TebVisualizationPtr visual; -std::vector obst_vector; -ViaPointContainer via_points; -TebConfig config; -//std::shared_ptr< dynamic_reconfigure::Server > dynamic_recfg; -ros::Subscriber custom_obst_sub; -ros::Subscriber via_points_sub; -ros::Subscriber clicked_points_sub; -std::vector obst_vel_subs; -unsigned int no_fixed_obstacles; - -// =========== Function declarations ============= -void CB_mainCycle(const ros::TimerEvent& e); -void CB_publishCycle(const ros::TimerEvent& e); -void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level); -void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); -void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb); -void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); -void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg); -void CB_via_points(const nav_msgs::msg::Path::ConstPtr& via_points_msg); -void CB_setObstacleVelocity(const geometry_msgs::msg::TwistConstPtr& twist_msg, const unsigned int id); - - -// =============== Main function ================= -int main( int argc, char** argv ) -{ - ros::init(argc, argv, "test_optim_node"); - rclcpp::Node::SharedPtr n("~"); - - - // load ros parameters from node handle - config.loadRosParamFromNodeHandle(n); - - ros::Timer cycle_timer = n.createTimer(ros::Duration(0.025), CB_mainCycle); - ros::Timer publish_timer = n.createTimer(ros::Duration(0.1), CB_publishCycle); - - // setup dynamic reconfigure - dynamic_recfg = std::make_shared< dynamic_reconfigure::Server >(n); - dynamic_reconfigure::Server::CallbackType cb = boost::bind(CB_reconfigure, _1, _2); - dynamic_recfg->setCallback(cb); - - // setup callback for custom obstacles - custom_obst_sub = n.subscribe("obstacles", 1, CB_customObstacle); - - // setup callback for clicked points (in rviz) that are considered as via-points - clicked_points_sub = n.subscribe("/clicked_point", 5, CB_clicked_points); - - // setup callback for via-points (callback overwrites previously set via-points) - via_points_sub = n.subscribe("via_points", 1, CB_via_points); - - // interactive marker server for simulated dynamic obstacles - interactive_markers::InteractiveMarkerServer marker_server("marker_obstacles"); - - obst_vector.push_back( std::make_shared(-3,1) ); - obst_vector.push_back( std::make_shared(6,2) ); - obst_vector.push_back( std::make_shared(0,0.1) ); -// obst_vector.push_back( std::make_shared(1,1.5,1,-1.5) ); //90 deg -// obst_vector.push_back( std::make_shared(1,0,-1,0) ); //180 deg -// obst_vector.push_back( std::make_shared(-1.5,-0.5) ); - - // Dynamic obstacles - Eigen::Vector2d vel (0.1, -0.3); - obst_vector.at(0)->setCentroidVelocity(vel); - vel = Eigen::Vector2d(-0.3, -0.2); - obst_vector.at(1)->setCentroidVelocity(vel); - - /* - PolygonObstacle* polyobst = new PolygonObstacle; - polyobst->pushBackVertex(1, -1); - polyobst->pushBackVertex(0, 1); - polyobst->pushBackVertex(1, 1); - polyobst->pushBackVertex(2, 1); - - polyobst->finalizePolygon(); - obst_vector.emplace_back(polyobst); - */ - - for (unsigned int i=0; i(topic, 1, boost::bind(&CB_setObstacleVelocity, _1, i))); - - //CreateInteractiveMarker(obst_vector.at(i)[0],obst_vector.at(i)[1],i,&marker_server, &CB_obstacle_marker); - // Add interactive markers for all point obstacles - std::shared_ptr pobst = boost::dynamic_pointer_cast(obst_vector.at(i)); - if (pobst) - { - CreateInteractiveMarker(pobst->x(),pobst->y(),i, config.map_frame, &marker_server, &CB_obstacle_marker); - } - } - marker_server.applyChanges(); - - // Setup visualization - visual = TebVisualizationPtr(new TebVisualization(n, config)); - - // Setup robot shape model - RobotFootprintModelPtr robot_model = TebLocalPlannerROS::getRobotFootprintFromParamServer(n); - - // Setup planner (homotopy class planning or just the local teb planner) - if (config.hcp.enable_homotopy_class_planning) - planner = PlannerInterfacePtr(new HomotopyClassPlanner(config, &obst_vector, robot_model, visual, &via_points)); - else - planner = PlannerInterfacePtr(new TebOptimalPlanner(config, &obst_vector, robot_model, visual, &via_points)); - - - no_fixed_obstacles = obst_vector.size(); - ros::spin(); - - return 0; -} - -// Planning loop -void CB_mainCycle(const ros::TimerEvent& e) -{ - planner->plan(PoseSE2(-4,0,0), PoseSE2(4,0,0)); // hardcoded start and goal for testing purposes -} - -// Visualization loop -void CB_publishCycle(const ros::TimerEvent& e) -{ - planner->visualize(); - visual->publishObstacles(obst_vector); - visual->publishViaPoints(via_points); -} - -void CB_reconfigure(TebLocalPlannerReconfigureConfig& reconfig, uint32_t level) -{ - config.reconfigure(reconfig); -} - -void CreateInteractiveMarker(const double& init_x, const double& init_y, unsigned int id, std::string frame, interactive_markers::InteractiveMarkerServer* marker_server, interactive_markers::InteractiveMarkerServer::FeedbackCallback feedback_cb) -{ - // create an interactive marker for our server - visualization_msgs::InteractiveMarker i_marker; - i_marker.header.frame_id = frame; - i_marker.header.stamp = ros::Time::now(); - std::ostringstream oss; - //oss << "obstacle" << id; - oss << id; - i_marker.name = oss.str(); - i_marker.description = "Obstacle"; - i_marker.pose.position.x = init_x; - i_marker.pose.position.y = init_y; - i_marker.pose.orientation.w = 1.0f; // make quaternion normalized - - // create a grey box marker - visualization_msgs::msg::Marker box_marker; - box_marker.type = visualization_msgs::msg::Marker::CUBE; - box_marker.id = id; - box_marker.scale.x = 0.2; - box_marker.scale.y = 0.2; - box_marker.scale.z = 0.2; - box_marker.color.r = 0.5; - box_marker.color.g = 0.5; - box_marker.color.b = 0.5; - box_marker.color.a = 1.0; - box_marker.pose.orientation.w = 1.0f; // make quaternion normalized - - // create a non-interactive control which contains the box - visualization_msgs::InteractiveMarkerControl box_control; - box_control.always_visible = true; - box_control.markers.push_back( box_marker ); - - // add the control to the interactive marker - i_marker.controls.push_back( box_control ); - - // create a control which will move the box, rviz will insert 2 arrows - visualization_msgs::InteractiveMarkerControl move_control; - move_control.name = "move_x"; - move_control.orientation.w = 0.707107f; - move_control.orientation.x = 0; - move_control.orientation.y = 0.707107f; - move_control.orientation.z = 0; - move_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; - - - // add the control to the interactive marker - i_marker.controls.push_back(move_control); - - // add the interactive marker to our collection - marker_server->insert(i_marker); - marker_server->setCallback(i_marker.name,feedback_cb); -} - -void CB_obstacle_marker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) -{ - std::stringstream ss(feedback->marker_name); - unsigned int index; - ss >> index; - - if (index>=no_fixed_obstacles) - return; - PointObstacle* pobst = static_cast(obst_vector.at(index).get()); - pobst->position() = Eigen::Vector2d(feedback->pose.position.x,feedback->pose.position.y); -} - -void CB_customObstacle(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg) -{ - // resize such that the vector contains only the fixed obstacles specified inside the main function - obst_vector.resize(no_fixed_obstacles); - - // Add custom obstacles obtained via message (assume that all obstacles coordiantes are specified in the default planning frame) - for (size_t i = 0; i < obst_msg->obstacles.size(); ++i) - { - if (obst_msg->obstacles.at(i).polygon.points.size() == 1 ) - { - if (obst_msg->obstacles.at(i).radius == 0) - { - obst_vector.push_back(ObstaclePtr(new PointObstacle( obst_msg->obstacles.at(i).polygon.points.front().x, - obst_msg->obstacles.at(i).polygon.points.front().y ))); - } - else - { - obst_vector.push_back(ObstaclePtr(new CircularObstacle( obst_msg->obstacles.at(i).polygon.points.front().x, - obst_msg->obstacles.at(i).polygon.points.front().y, - obst_msg->obstacles.at(i).radius ))); - } - } - else if (obst_msg->obstacles.at(i).polygon.points.empty()) - { - ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping..."); - continue; - } - else - { - PolygonObstacle* polyobst = new PolygonObstacle; - for (size_t j=0; jobstacles.at(i).polygon.points.size(); ++j) - { - polyobst->pushBackVertex( obst_msg->obstacles.at(i).polygon.points[j].x, - obst_msg->obstacles.at(i).polygon.points[j].y ); - } - polyobst->finalizePolygon(); - obst_vector.push_back(ObstaclePtr(polyobst)); - } - - if(!obst_vector.empty()) - obst_vector.back()->setCentroidVelocity(obst_msg->obstacles.at(i).velocities, obst_msg->obstacles.at(i).orientation); - } -} - - -void CB_clicked_points(const geometry_msgs::PointStampedConstPtr& point_msg) -{ - // we assume for simplicity that the fixed frame is already the map/planning frame - // consider clicked points as via-points - via_points.push_back( Eigen::Vector2d(point_msg->point.x, point_msg->point.y) ); - ROS_INFO_STREAM("Via-point (" << point_msg->point.x << "," << point_msg->point.y << ") added."); - if (config.optim.weight_viapoint<=0) - ROS_WARN("Note, via-points are deactivated, since 'weight_via_point' <= 0"); -} - -void CB_via_points(const nav_msgs::msg::Path::ConstPtr& via_points_msg) -{ - ROS_INFO_ONCE("Via-points received. This message is printed once."); - via_points.clear(); - for (const geometry_msgs::msg::PoseStamped& pose : via_points_msg->poses) - { - via_points.emplace_back(pose.pose.position.x, pose.pose.position.y); - } -} - -void CB_setObstacleVelocity(const geometry_msgs::msg::TwistConstPtr& twist_msg, const unsigned int id) -{ - if (id >= obst_vector.size()) - { - ROS_WARN("Cannot set velocity: unknown obstacle id."); - return; - } - - Eigen::Vector2d vel (twist_msg->linear.x, twist_msg->linear.y); - obst_vector.at(id)->setCentroidVelocity(vel); -} diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp deleted file mode 100644 index 4d7d37c..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/timed_elastic_band.cpp +++ /dev/null @@ -1,630 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -#include "teb_local_planner/timed_elastic_band.h" - -namespace teb_local_planner -{ - -namespace -{ - /** - * estimate the time to move from start to end. - * Assumes constant velocity for the motion. - */ - double estimateDeltaT(const PoseSE2& start, const PoseSE2& end, - double max_vel_x, double max_vel_theta) - { - double dt_constant_motion = 0.1; - if (max_vel_x > 0) { - double trans_dist = (end.position() - start.position()).norm(); - dt_constant_motion = trans_dist / max_vel_x; - } - if (max_vel_theta > 0) { - double rot_dist = std::abs(g2o::normalize_theta(end.theta() - start.theta())); - dt_constant_motion = std::max(dt_constant_motion, rot_dist / max_vel_theta); - } - return dt_constant_motion; - } -} // namespace - - -TimedElasticBand::TimedElasticBand() -{ -} - -TimedElasticBand::~TimedElasticBand() -{ - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "Destructor Timed_Elastic_Band..."); - clearTimedElasticBand(); -} - - -void TimedElasticBand::addPose(const PoseSE2& pose, bool fixed) -{ - VertexPose* pose_vertex = new VertexPose(pose, fixed); - pose_vec_.push_back( pose_vertex ); - return; -} - -void TimedElasticBand::addPose(const Eigen::Ref& position, double theta, bool fixed) -{ - VertexPose* pose_vertex = new VertexPose(position, theta, fixed); - pose_vec_.push_back( pose_vertex ); - return; -} - - void TimedElasticBand::addPose(double x, double y, double theta, bool fixed) -{ - VertexPose* pose_vertex = new VertexPose(x, y, theta, fixed); - pose_vec_.push_back( pose_vertex ); - return; -} - -void TimedElasticBand::addTimeDiff(double dt, bool fixed) -{ - assert(dt > 0.0 && "Adding a timediff requires a positive dt"); - VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt, fixed); - timediff_vec_.push_back( timediff_vertex ); - return; -} - - -void TimedElasticBand::addPoseAndTimeDiff(double x, double y, double angle, double dt) -{ - if (sizePoses() != sizeTimeDiffs()) - { - addPose(x,y,angle,false); - addTimeDiff(dt,false); - } - else { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), - "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf"); - } - return; -} - - - -void TimedElasticBand::addPoseAndTimeDiff(const PoseSE2& pose, double dt) -{ - if (sizePoses() != sizeTimeDiffs()) - { - addPose(pose,false); - addTimeDiff(dt,false); - } else { - RCLCPP_ERROR(rclcpp::get_logger("teb_local_planner"), "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf"); - } - return; -} - -void TimedElasticBand::addPoseAndTimeDiff(const Eigen::Ref& position, double theta, double dt) -{ - if (sizePoses() != sizeTimeDiffs()) - { - addPose(position, theta,false); - addTimeDiff(dt,false); - } else { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "Method addPoseAndTimeDiff: Add one single Pose first. Timediff describes the time difference between last conf and given conf"); - } - return; -} - - -void TimedElasticBand::deletePose(int index) -{ - assert(index& position, double theta) -{ - VertexPose* pose_vertex = new VertexPose(position, theta); - pose_vec_.insert(pose_vec_.begin()+index, pose_vertex); -} - -void TimedElasticBand::insertPose(int index, double x, double y, double theta) -{ - VertexPose* pose_vertex = new VertexPose(x, y, theta); - pose_vec_.insert(pose_vec_.begin()+index, pose_vertex); -} - -void TimedElasticBand::insertTimeDiff(int index, double dt) -{ - VertexTimeDiff* timediff_vertex = new VertexTimeDiff(dt); - timediff_vec_.insert(timediff_vec_.begin()+index, timediff_vertex); -} - - -void TimedElasticBand::clearTimedElasticBand() -{ - for (PoseSequence::iterator pose_it = pose_vec_.begin(); pose_it != pose_vec_.end(); ++pose_it) - delete *pose_it; - pose_vec_.clear(); - - for (TimeDiffSequence::iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it) - delete *dt_it; - timediff_vec_.clear(); -} - - -void TimedElasticBand::setPoseVertexFixed(int index, bool status) -{ - assert(indexsetFixed(status); -} - -void TimedElasticBand::setTimeDiffVertexFixed(int index, bool status) -{ - assert(indexsetFixed(status); -} - - -void TimedElasticBand::autoResize(double dt_ref, double dt_hysteresis, int min_samples, int max_samples, bool fast_mode) -{ - assert(sizeTimeDiffs() == 0 || sizeTimeDiffs() + 1 == sizePoses()); - /// iterate through all TEB states and add/remove states! - bool modified = true; - - for (int rep = 0; rep < 100 && modified; ++rep) // actually it should be while(), but we want to make sure to not get stuck in some oscillation, hence max 100 repitions. - { - modified = false; - - for(int i=0; i < sizeTimeDiffs(); ++i) // TimeDiff connects Point(i) with Point(i+1) - { - if(TimeDiff(i) > dt_ref + dt_hysteresis && sizeTimeDiffs()min_samples) // only remove samples if size is larger than min_samples. - { - //RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), "teb_local_planner: autoResize() deleting bandpoint i=%u, #TimeDiffs=%lu",i,sizeTimeDiffs()); - - if(i < ((int)sizeTimeDiffs()-1)) - { - TimeDiff(i+1) = TimeDiff(i+1) + TimeDiff(i); - deleteTimeDiff(i); - deletePose(i+1); - } - else - { // last motion should be adjusted, shift time to the interval before - TimeDiff(i-1) += TimeDiff(i); - deleteTimeDiff(i); - deletePose(i); - } - - modified = true; - } - } - if (fast_mode) break; - } -} - - -double TimedElasticBand::getSumOfAllTimeDiffs() const -{ - double time = 0; - - for(TimeDiffSequence::const_iterator dt_it = timediff_vec_.begin(); dt_it != timediff_vec_.end(); ++dt_it) - { - time += (*dt_it)->dt(); - } - return time; -} - -double TimedElasticBand::getSumOfTimeDiffsUpToIdx(int index) const -{ - assert(index<=timediff_vec_.size()); - - double time = 0; - - for(int i = 0; i < index; ++i) - { - time += timediff_vec_.at(i)->dt(); - } - - return time; -} - -double TimedElasticBand::getAccumulatedDistance() const -{ - double dist = 0; - - for(int i=1; i 0) timestep = diststep / max_vel_x; - - for (unsigned int i=1; i<=no_steps; i++) // start with 1! starting point had index 0 - { - if (i==no_steps && no_steps_d==(float) no_steps) - break; // if last conf (depending on stepsize) is equal to goal conf -> leave loop - addPoseAndTimeDiff(start.x()+i*dx,start.y()+i*dy,orient_init,timestep); - } - - } - - // if number of samples is not larger than min_samples, insert manually - if ( sizePoses() < min_samples-1 ) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples..."); - while (sizePoses() < min_samples-1) // subtract goal point that will be added later - { - // simple strategy: interpolate between the current pose and the goal - PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal); - if (max_vel_x > 0) timestep = (intermediate_pose.position()-BackPose().position()).norm()/max_vel_x; - addPoseAndTimeDiff( intermediate_pose, timestep ); // let the optimier correct the timestep (TODO: better initialization - } - } - - // add goal - if (max_vel_x > 0) timestep = (goal.position()-BackPose().position()).norm()/max_vel_x; - addPoseAndTimeDiff(goal,timestep); // add goal point - setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization - } - else // size!=0 - { - RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), - "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!"); - RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), - "Number of TEB configurations: %d, Number of TEB timediffs: %d",(unsigned int) sizePoses(),(unsigned int) sizeTimeDiffs()); - return false; - } - return true; -} - - -bool TimedElasticBand::initTrajectoryToGoal(const std::vector& plan, double max_vel_x, double max_vel_theta, bool estimate_orient, int min_samples, bool guess_backwards_motion) -{ - - if (!isInit()) - { - PoseSE2 start(plan.front().pose); - PoseSE2 goal(plan.back().pose); - - addPose(start); // add starting point with given orientation - setPoseVertexFixed(0,true); // StartConf is a fixed constraint during optimization - - bool backwards = false; - if (guess_backwards_motion && (goal.position()-start.position()).dot(start.orientationUnitVec()) < 0) // check if the goal is behind the start pose (w.r.t. start orientation) - backwards = true; - // TODO: dt ~ max_vel_x_backwards for backwards motions - - for (int i=1; i<(int)plan.size()-1; ++i) - { - double yaw; - if (estimate_orient) - { - // get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i} - double dx = plan[i+1].pose.position.x - plan[i].pose.position.x; - double dy = plan[i+1].pose.position.y - plan[i].pose.position.y; - yaw = std::atan2(dy,dx); - if (backwards) - yaw = g2o::normalize_theta(yaw+M_PI); - } - else - { - yaw = tf2::getYaw(plan[i].pose.orientation); - } - PoseSE2 intermediate_pose(plan[i].pose.position.x, plan[i].pose.position.y, yaw); - double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta); - addPoseAndTimeDiff(intermediate_pose, dt); - } - - // if number of samples is not larger than min_samples, insert manually - if ( sizePoses() < min_samples-1 ) - { - RCLCPP_DEBUG(rclcpp::get_logger("teb_local_planner"), - "initTEBtoGoal(): number of generated samples is less than specified by min_samples. Forcing the insertion of more samples..."); - while (sizePoses() < min_samples-1) // subtract goal point that will be added later - { - // simple strategy: interpolate between the current pose and the goal - PoseSE2 intermediate_pose = PoseSE2::average(BackPose(), goal); - double dt = estimateDeltaT(BackPose(), intermediate_pose, max_vel_x, max_vel_theta); - addPoseAndTimeDiff( intermediate_pose, dt ); // let the optimier correct the timestep (TODO: better initialization - } - } - - // Now add final state with given orientation - double dt = estimateDeltaT(BackPose(), goal, max_vel_x, max_vel_theta); - addPoseAndTimeDiff(goal, dt); - setPoseVertexFixed(sizePoses()-1,true); // GoalConf is a fixed constraint during optimization - } - else // size!=0 - { - RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), - "Cannot init TEB between given configuration and goal, because TEB vectors are not empty or TEB is already initialized (call this function before adding states yourself)!"); - RCLCPP_WARN(rclcpp::get_logger("teb_local_planner"), - "Number of TEB configurations: %d, Number of TEB timediffs: %d", sizePoses(), sizeTimeDiffs()); - return false; - } - - return true; -} - - -int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref& ref_point, double* distance, int begin_idx) const -{ - int n = sizePoses(); - if (begin_idx < 0 || begin_idx >= n) - return -1; - - double min_dist_sq = std::numeric_limits::max(); - int min_idx = -1; - - for (int i = begin_idx; i < n; i++) - { - double dist_sq = (ref_point - Pose(i).position()).squaredNorm(); - if (dist_sq < min_dist_sq) - { - min_dist_sq = dist_sq; - min_idx = i; - } - } - - if (distance) - *distance = std::sqrt(min_dist_sq); - - return min_idx; -} - - -int TimedElasticBand::findClosestTrajectoryPose(const Eigen::Ref& ref_line_start, const Eigen::Ref& ref_line_end, double* distance) const -{ - double min_dist = std::numeric_limits::max(); - int min_idx = -1; - - for (int i = 0; i < sizePoses(); i++) - { - Eigen::Vector2d point = Pose(i).position(); - double dist = distance_point_to_segment_2d(point, ref_line_start, ref_line_end); - if (dist < min_dist) - { - min_dist = dist; - min_idx = i; - } - } - - if (distance) - *distance = min_dist; - return min_idx; -} - -int TimedElasticBand::findClosestTrajectoryPose(const Point2dContainer& vertices, double* distance) const -{ - if (vertices.empty()) - return 0; - else if (vertices.size() == 1) - return findClosestTrajectoryPose(vertices.front()); - else if (vertices.size() == 2) - return findClosestTrajectoryPose(vertices.front(), vertices.back()); - - double min_dist = std::numeric_limits::max(); - int min_idx = -1; - - for (int i = 0; i < sizePoses(); i++) - { - Eigen::Vector2d point = Pose(i).position(); - double dist_to_polygon = std::numeric_limits::max(); - for (int j = 0; j < (int) vertices.size()-1; ++j) - { - dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices[j], vertices[j+1])); - } - dist_to_polygon = std::min(dist_to_polygon, distance_point_to_segment_2d(point, vertices.back(), vertices.front())); - if (dist_to_polygon < min_dist) - { - min_dist = dist_to_polygon; - min_idx = i; - } - } - - if (distance) - *distance = min_dist; - - return min_idx; -} - - -int TimedElasticBand::findClosestTrajectoryPose(const Obstacle& obstacle, double* distance) const -{ - const PointObstacle* pobst = dynamic_cast(&obstacle); - if (pobst) - return findClosestTrajectoryPose(pobst->position(), distance); - - const LineObstacle* lobst = dynamic_cast(&obstacle); - if (lobst) - return findClosestTrajectoryPose(lobst->start(), lobst->end(), distance); - - const PolygonObstacle* polyobst = dynamic_cast(&obstacle); - if (polyobst) - return findClosestTrajectoryPose(polyobst->vertices(), distance); - - return findClosestTrajectoryPose(obstacle.getCentroid(), distance); -} - - -void TimedElasticBand::updateAndPruneTEB(boost::optional new_start, boost::optional new_goal, int min_samples) -{ - // first and simple approach: change only start confs (and virtual start conf for inital velocity) - // TEST if optimizer can handle this "hard" placement - - if (new_start && sizePoses()>0) - { - // find nearest state (using l2-norm) in order to prune the trajectory - // (remove already passed states) - double dist_cache = (new_start->position()- Pose(0).position()).norm(); - double dist; - int lookahead = std::min( sizePoses()-min_samples, 10); // satisfy min_samples, otherwise max 10 samples - - int nearest_idx = 0; - for (int i = 1; i<=lookahead; ++i) - { - dist = (new_start->position()- Pose(i).position()).norm(); - if (dist0) - { - // nearest_idx is equal to the number of samples to be removed (since it counts from 0 ;-) ) - // WARNING delete starting at pose 1, and overwrite the original pose(0) with new_start, since Pose(0) is fixed during optimization! - deletePoses(1, nearest_idx); // delete first states such that the closest state is the new first one - deleteTimeDiffs(1, nearest_idx); // delete corresponding time differences - } - - // update start - Pose(0) = *new_start; - } - - if (new_goal && sizePoses()>0) - { - BackPose() = *new_goal; - } -}; - - -bool TimedElasticBand::isTrajectoryInsideRegion(double radius, double max_dist_behind_robot, int skip_poses) -{ - if (sizePoses()<=0) - return true; - - double radius_sq = radius*radius; - double max_dist_behind_robot_sq = max_dist_behind_robot*max_dist_behind_robot; - Eigen::Vector2d robot_orient = Pose(0).orientationUnitVec(); - - for (int i=1; i radius_sq) - { - RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), "outside robot"); - return false; - } - - // check behind the robot with a different distance, if specified (or >=0) - if (max_dist_behind_robot >= 0 && dist_vec.dot(robot_orient) < 0 && dist_sq > max_dist_behind_robot_sq) - { - RCLCPP_INFO(rclcpp::get_logger("teb_local_planner"), "outside robot behind"); - return false; - } - - } - return true; -} - - - - -} // namespace teb_local_planner diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/visualization.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/visualization.cpp deleted file mode 100644 index 025960d..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/src/visualization.cpp +++ /dev/null @@ -1,563 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2016, - * TU Dortmund - Institute of Control Theory and Systems Engineering. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * Author: Christoph Rösmann - *********************************************************************/ - -// ros stuff -#include "teb_local_planner/visualization.h" -#include "teb_local_planner/optimal_planner.h" - -namespace teb_local_planner -{ - -void publishPlan(const std::vector& path, - rclcpp::Publisher *pub) { - if(path.empty()) - return; - - nav_msgs::msg::Path gui_path; - gui_path.poses.resize(path.size()); - gui_path.header.frame_id = path[0].header.frame_id; - gui_path.header.stamp = path[0].header.stamp; - - // Extract the plan in world co-ordinates, we assume the path is all in the same frame - for(unsigned int i=0; i < path.size(); i++){ - gui_path.poses[i] = path[i]; - } - - pub->publish(gui_path); -} - -TebVisualization::TebVisualization(const rclcpp_lifecycle::LifecycleNode::SharedPtr & nh, const TebConfig& cfg) : nh_(nh), cfg_(&cfg), initialized_(false) -{ -} - -void TebVisualization::publishGlobalPlan(const std::vector& global_plan) const -{ - if ( printErrorWhenNotInitialized() ) - return; - publishPlan(global_plan, global_plan_pub_.get()); -} - -void TebVisualization::publishLocalPlan(const std::vector& local_plan) const -{ - if ( printErrorWhenNotInitialized() ) - return; - publishPlan(local_plan, local_plan_pub_.get()); -} - -void TebVisualization::publishLocalPlanAndPoses(const TimedElasticBand& teb) const -{ - if ( printErrorWhenNotInitialized() ) - return; - - // create path msg - nav_msgs::msg::Path teb_path; - teb_path.header.frame_id = cfg_->map_frame; - teb_path.header.stamp = nh_->now(); - - // create pose_array (along trajectory) - geometry_msgs::msg::PoseArray teb_poses; - teb_poses.header.frame_id = teb_path.header.frame_id; - teb_poses.header.stamp = teb_path.header.stamp; - - // fill path msgs with teb configurations - for (int i=0; i < teb.sizePoses(); i++) - { - geometry_msgs::msg::PoseStamped pose; - - pose.header.frame_id = teb_path.header.frame_id; - pose.header.stamp = teb_path.header.stamp; - teb.Pose(i).toPoseMsg(pose.pose); - pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*teb.getSumOfTimeDiffsUpToIdx(i); - teb_path.poses.push_back(pose); - teb_poses.poses.push_back(pose.pose); - } - local_plan_pub_->publish(teb_path); - teb_poses_pub_->publish(teb_poses); -} - - - -void TebVisualization::publishRobotFootprintModel(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model, const std::string& ns, - const std_msgs::msg::ColorRGBA &color) -{ - if ( printErrorWhenNotInitialized() ) - return; - - std::vector markers; - robot_model.visualizeRobot(current_pose, markers, color); - if (markers.empty()) - return; - - int idx = 1000000; // avoid overshadowing by obstacles - for (std::vector::iterator marker_it = markers.begin(); marker_it != markers.end(); ++marker_it, ++idx) - { - marker_it->header.frame_id = cfg_->map_frame; - marker_it->header.stamp = nh_->now(); - marker_it->action = visualization_msgs::msg::Marker::ADD; - marker_it->ns = ns; - marker_it->id = idx; - marker_it->lifetime = rclcpp::Duration(2, 0); - teb_marker_pub_->publish(*marker_it); - } - -} - -void TebVisualization::publishInfeasibleRobotPose(const PoseSE2& current_pose, const BaseRobotFootprintModel& robot_model) -{ - publishRobotFootprintModel(current_pose, robot_model, "InfeasibleRobotPoses", toColorMsg(0.5, 0.8, 0.0, 0.0)); -} - - -void TebVisualization::publishObstacles(const ObstContainer& obstacles) const -{ - if ( obstacles.empty() || printErrorWhenNotInitialized() ) - return; - - // Visualize point obstacles - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = "PointObstacles"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(2, 0); - marker.pose.orientation.w = 1.0; - - for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst) - { - std::shared_ptr pobst = std::dynamic_pointer_cast(*obst); - if (!pobst) - continue; - - if (cfg_->hcp.visualize_with_time_as_z_axis_scale < 0.001) - { - geometry_msgs::msg::Point point; - point.x = pobst->x(); - point.y = pobst->y(); - point.z = 0; - marker.points.push_back(point); - } - else // Spatiotemporally point obstacles become a line - { - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - geometry_msgs::msg::Point start; - start.x = pobst->x(); - start.y = pobst->y(); - start.z = 0; - marker.points.push_back(start); - - geometry_msgs::msg::Point end; - double t = 20; - Eigen::Vector2d pred; - pobst->predictCentroidConstantVelocity(t, pred); - end.x = pred[0]; - end.y = pred[1]; - end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*t; - marker.points.push_back(end); - } - } - - marker.scale.x = 0.1; - marker.scale.y = 0.1; - marker.color.a = 1.0; - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - - teb_marker_pub_->publish( marker ); - } - - // Visualize circular obstacles - { - std::size_t idx = 0; - for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst) - { - std::shared_ptr pobst = std::dynamic_pointer_cast(*obst); - if (!pobst) - continue; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = "CircularObstacles"; - marker.id = idx++; - marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(2, 0); - geometry_msgs::msg::Point point; - point.x = pobst->x(); - point.y = pobst->y(); - point.z = 0; - marker.points.push_back(point); - - marker.scale.x = pobst->radius(); - marker.scale.y = pobst->radius(); - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - teb_marker_pub_->publish( marker ); - } - } - - // Visualize line obstacles - { - std::size_t idx = 0; - for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst) - { - std::shared_ptr pobst = std::dynamic_pointer_cast(*obst); - if (!pobst) - continue; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = "LineObstacles"; - marker.id = idx++; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(2, 0); - marker.pose.orientation.w = 1.0; - - geometry_msgs::msg::Point start; - start.x = pobst->start().x(); - start.y = pobst->start().y(); - start.z = 0; - marker.points.push_back(start); - geometry_msgs::msg::Point end; - end.x = pobst->end().x(); - end.y = pobst->end().y(); - end.z = 0; - marker.points.push_back(end); - - marker.scale.x = 0.1; - marker.scale.y = 0.1; - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - teb_marker_pub_->publish( marker ); - } - } - - - // Visualize polygon obstacles - { - std::size_t idx = 0; - for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst) - { - std::shared_ptr pobst = std::dynamic_pointer_cast(*obst); - if (!pobst) - continue; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = "PolyObstacles"; - marker.id = idx++; - marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(2, 0); - marker.pose.orientation.w = 1.0; - - for (Point2dContainer::const_iterator vertex = pobst->vertices().begin(); vertex != pobst->vertices().end(); ++vertex) - { - geometry_msgs::msg::Point point; - point.x = vertex->x(); - point.y = vertex->y(); - point.z = 0; - marker.points.push_back(point); - } - - // Also add last point to close the polygon - // but only if polygon has more than 2 points (it is not a line) - if (pobst->vertices().size() > 2) - { - geometry_msgs::msg::Point point; - point.x = pobst->vertices().front().x(); - point.y = pobst->vertices().front().y(); - point.z = 0; - marker.points.push_back(point); - } - marker.scale.x = 0.1; - marker.scale.y = 0.1; - marker.color.a = 1.0; - marker.color.r = 1.0; - marker.color.g = 0.0; - marker.color.b = 0.0; - - teb_marker_pub_->publish( marker ); - } - } -} - - -void TebVisualization::publishViaPoints(const std::vector< Eigen::Vector2d, Eigen::aligned_allocator >& via_points, const std::string& ns) const -{ - if ( via_points.empty() || printErrorWhenNotInitialized() ) - return; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = ns; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration(2, 0); - marker.pose.orientation.w = 1.0; - - for (std::size_t i=0; i < via_points.size(); ++i) - { - geometry_msgs::msg::Point point; - point.x = via_points[i].x(); - point.y = via_points[i].y(); - point.z = 0; - marker.points.push_back(point); - } - - marker.scale.x = 0.1; - marker.scale.y = 0.1; - marker.color.a = 1.0; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - - teb_marker_pub_->publish( marker ); -} - -void TebVisualization::publishTebContainer(const TebOptPlannerContainer& teb_planner, const std::string& ns) -{ -if ( printErrorWhenNotInitialized() ) - return; - - visualization_msgs::msg::Marker marker; - marker.header.frame_id = cfg_->map_frame; - marker.header.stamp = nh_->now(); - marker.ns = ns; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::LINE_LIST; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose.orientation.w = 1.0; - - // Iterate through teb pose sequence - for( TebOptPlannerContainer::const_iterator it_teb = teb_planner.begin(); it_teb != teb_planner.end(); ++it_teb ) - { - // iterate single poses - PoseSequence::const_iterator it_pose = it_teb->get()->teb().poses().begin(); - TimeDiffSequence::const_iterator it_timediff = it_teb->get()->teb().timediffs().begin(); - PoseSequence::const_iterator it_pose_end = it_teb->get()->teb().poses().end(); - std::advance(it_pose_end, -1); // since we are interested in line segments, reduce end iterator by one. - double time = 0; - - while (it_pose != it_pose_end) - { - geometry_msgs::msg::Point point_start; - point_start.x = (*it_pose)->x(); - point_start.y = (*it_pose)->y(); - point_start.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time; - marker.points.push_back(point_start); - - time += (*it_timediff)->dt(); - - geometry_msgs::msg::Point point_end; - point_end.x = (*boost::next(it_pose))->x(); - point_end.y = (*boost::next(it_pose))->y(); - point_end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale*time; - marker.points.push_back(point_end); - ++it_pose; - ++it_timediff; - } - } - marker.scale.x = 0.01; - marker.color.a = 1.0; - marker.color.r = 0.5; - marker.color.g = 1.0; - marker.color.b = 0.0; - - teb_marker_pub_->publish( marker ); -} - -void TebVisualization::publishFeedbackMessage(const std::vector< std::shared_ptr >& teb_planners, - unsigned int selected_trajectory_idx, const ObstContainer& obstacles) -{ - teb_msgs::msg::FeedbackMsg msg; - msg.header.stamp = nh_->now(); - msg.header.frame_id = cfg_->map_frame; - msg.selected_trajectory_idx = selected_trajectory_idx; - - - msg.trajectories.resize(teb_planners.size()); - - // Iterate through teb pose sequence - std::size_t idx_traj = 0; - for( TebOptPlannerContainer::const_iterator it_teb = teb_planners.begin(); it_teb != teb_planners.end(); ++it_teb, ++idx_traj ) - { - msg.trajectories[idx_traj].header = msg.header; - it_teb->get()->getFullTrajectory(msg.trajectories[idx_traj].trajectory); - } - - // add obstacles - msg.obstacles_msg.obstacles.resize(obstacles.size()); - for (std::size_t i=0; itoPolygonMsg(msg.obstacles_msg.obstacles[i].polygon); - - // copy id - msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet - - // orientation - //msg.obstacles_msg.obstacles[i].orientation =; // TODO - - // copy velocities - obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities); - } - - feedback_pub_->publish(msg); -} - -void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles) -{ - teb_msgs::msg::FeedbackMsg msg; - msg.header.stamp = nh_->now(); - msg.header.frame_id = cfg_->map_frame; - msg.selected_trajectory_idx = 0; - - msg.trajectories.resize(1); - msg.trajectories.front().header = msg.header; - teb_planner.getFullTrajectory(msg.trajectories.front().trajectory); - - // add obstacles - msg.obstacles_msg.obstacles.resize(obstacles.size()); - for (std::size_t i=0; itoPolygonMsg(msg.obstacles_msg.obstacles[i].polygon); - - // copy id - msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet - - // orientation - //msg.obstacles_msg.obstacles[i].orientation =; // TODO - - // copy velocities - obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities); - } - - feedback_pub_->publish(msg); -} - -std_msgs::msg::ColorRGBA TebVisualization::toColorMsg(double a, double r, double g, double b) -{ - std_msgs::msg::ColorRGBA color; - color.a = a; - color.r = r; - color.g = g; - color.b = b; - return color; -} - -bool TebVisualization::printErrorWhenNotInitialized() const -{ - if (!initialized_) - { - RCLCPP_ERROR(nh_->get_logger(), "TebVisualization class not initialized. You must call initialize or an appropriate constructor"); - return true; - } - return false; -} - -nav2_util::CallbackReturn TebVisualization::on_configure() -{ - // register topics - global_plan_pub_ = nh_->create_publisher("global_plan", 1);; - local_plan_pub_ = nh_->create_publisher("local_plan",1); - teb_poses_pub_ = nh_->create_publisher("teb_poses", 1); - teb_marker_pub_ = nh_->create_publisher("teb_markers", 1); - feedback_pub_ = nh_->create_publisher("teb_feedback", 1); - - initialized_ = true; - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -TebVisualization::on_activate() -{ - global_plan_pub_->on_activate(); - local_plan_pub_->on_activate(); - teb_poses_pub_->on_activate(); - teb_marker_pub_->on_activate(); - feedback_pub_->on_activate(); - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -TebVisualization::on_deactivate() -{ - global_plan_pub_->on_deactivate(); - local_plan_pub_->on_deactivate(); - teb_poses_pub_->on_deactivate(); - teb_marker_pub_->on_deactivate(); - feedback_pub_->on_deactivate(); - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -TebVisualization::on_cleanup() -{ - global_plan_pub_.reset(); - local_plan_pub_.reset(); - teb_poses_pub_.reset(); - teb_marker_pub_.reset(); - feedback_pub_.reset(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -} // namespace teb_local_planner diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/teb_local_planner_plugin.xml b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/teb_local_planner_plugin.xml deleted file mode 100644 index 45d0bdf..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/teb_local_planner_plugin.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - The teb_local_planner package implements a plugin - to the base_local_planner of the 2D navigation stack. - - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/homotopy_class_planner_test.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/homotopy_class_planner_test.cpp deleted file mode 100644 index 8f9251b..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/homotopy_class_planner_test.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "teb_local_planner/homotopy_class_planner.h" -#include - -#include - -class HomotopyClassPlannerTest : public teb_local_planner::HomotopyClassPlanner { - public: - void SetUp(rclcpp::Node::SharedPtr node) { - teb_local_planner::RobotFootprintModelPtr robot_model; - teb_local_planner::TebVisualizationPtr visualization; - teb_local_planner::HomotopyClassPlannerPtr homotopy_class_planner; - - robot_model.reset(new teb_local_planner::CircularRobotFootprint(0.25)); - - obstacles.push_back( - teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 2)) - ); - - obstacles.push_back( - teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 3)) - ); - - obstacles.push_back( - teb_local_planner::ObstaclePtr(new teb_local_planner::PointObstacle(2, 4)) - ); - - visualization.reset( - new teb_local_planner::TebVisualization(node, cfg) - ); - - cfg.hcp.visualize_hc_graph = true; - - initialize(node, cfg, &obstacles, robot_model, visualization); - } - teb_local_planner::ObstContainer obstacles; - teb_local_planner::TebConfig cfg; -}; - -TEST(test, test) { - HomotopyClassPlannerTest test; - rclcpp::Node::SharedPtr node( new rclcpp::Node("test") ); - test.SetUp(node); - - using namespace teb_local_planner; - PoseSE2 start(0, 0, 0); - PoseSE2 goal(5, 5, 0); - geometry_msgs::msg::Twist twist; - twist.linear.x = 0; - twist.linear.y = 0; - twist.angular.z = 0; - - std::vector initial_plan; - - geometry_msgs::msg::PoseStamped start_pose; - geometry_msgs::msg::PoseStamped goal_pose; - - start.toPoseMsg(start_pose.pose); - goal.toPoseMsg(goal_pose.pose); - - initial_plan.push_back(start_pose); - initial_plan.push_back(goal_pose); - - test.plan(initial_plan, &twist, false); - //homotopy_class_planner_->exploreEquivalenceClassesAndInitTebs(start, goal, 0.3, &twist); - - rclcpp::Rate rate(10); - while(rclcpp::ok()) { - test.visualize(); - rclcpp::spin_some(node); - rate.sleep(); - } - - ASSERT_TRUE(true); -} - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/teb_basics.cpp b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/teb_basics.cpp deleted file mode 100644 index b490974..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_local_planner/test/teb_basics.cpp +++ /dev/null @@ -1,73 +0,0 @@ -#include - -#include "teb_local_planner/timed_elastic_band.h" - -TEST(TEBBasic, autoResizeLargeValueAtEnd) -{ - double dt = 0.1; - double dt_hysteresis = dt/3.; - teb_local_planner::TimedElasticBand teb; - - teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.)); - for (int i = 1; i < 10; ++i) { - teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt); - } - // add a pose with a large timediff as the last one - teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt + 2*dt_hysteresis); - - // auto resize + test of the result - teb.autoResize(dt, dt_hysteresis, 3, 100, false); - for (int i = 0; i < teb.sizeTimeDiffs(); ++i) { - ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i; - ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i; - } -} - -TEST(TEBBasic, autoResizeSmallValueAtEnd) -{ - double dt = 0.1; - double dt_hysteresis = dt/3.; - teb_local_planner::TimedElasticBand teb; - - teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.)); - for (int i = 1; i < 10; ++i) { - teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt); - } - // add a pose with a small timediff as the last one - teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis); - - // auto resize + test of the result - teb.autoResize(dt, dt_hysteresis, 3, 100, false); - for (int i = 0; i < teb.sizeTimeDiffs(); ++i) { - ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i; - ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i; - } -} - -TEST(TEBBasic, autoResize) -{ - double dt = 0.1; - double dt_hysteresis = dt/3.; - teb_local_planner::TimedElasticBand teb; - - teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.)); - for (int i = 1; i < 10; ++i) { - teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt); - } - // modify the timediff in the middle and add a pose with a smaller timediff as the last one - teb.TimeDiff(5) = dt + 2*dt_hysteresis; - teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2*dt_hysteresis); - - // auto resize - teb.autoResize(dt, dt_hysteresis, 3, 100, false); - for (int i = 0; i < teb.sizeTimeDiffs(); ++i) { - ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i; - ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i; - } -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/CMakeLists.txt b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/CMakeLists.txt deleted file mode 100644 index b3ad44e..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -cmake_minimum_required(VERSION 3.5) - -project(teb_msgs) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(costmap_converter_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces(teb_msgs - "msg/FeedbackMsg.msg" - "msg/TrajectoryMsg.msg" - "msg/TrajectoryPointMsg.msg" - DEPENDENCIES builtin_interfaces costmap_converter_msgs geometry_msgs std_msgs -) - -ament_package() diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/FeedbackMsg.msg b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/FeedbackMsg.msg deleted file mode 100644 index aa97c96..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/FeedbackMsg.msg +++ /dev/null @@ -1,15 +0,0 @@ -# Message that contains intermediate results -# and diagnostics of the (predictive) planner. - -std_msgs/Header header - -# The planned trajectory (or if multiple plans exist, all of them) -teb_msgs/TrajectoryMsg[] trajectories - -# Index of the trajectory in 'trajectories' that is selected currently -uint16 selected_trajectory_idx - -# List of active obstacles -costmap_converter_msgs/ObstacleArrayMsg obstacles_msg - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryMsg.msg b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryMsg.msg deleted file mode 100644 index 14bf608..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryMsg.msg +++ /dev/null @@ -1,6 +0,0 @@ -# Message that contains a trajectory for mobile robot navigation - -std_msgs/Header header -teb_msgs/TrajectoryPointMsg[] trajectory - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryPointMsg.msg b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryPointMsg.msg deleted file mode 100644 index 47b186a..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/msg/TrajectoryPointMsg.msg +++ /dev/null @@ -1,21 +0,0 @@ -# Message that contains single point on a trajectory suited for mobile navigation. -# The trajectory is described by a sequence of poses, velocities, -# accelerations and temporal information. - -# Why this message type? -# nav_msgs/Path describes only a path without temporal information. -# trajectory_msgs package contains only messages for joint trajectories. - -# The pose of the robot -geometry_msgs/Pose pose - -# Corresponding velocity -geometry_msgs/Twist velocity - -# Corresponding acceleration -geometry_msgs/Twist acceleration - -builtin_interfaces/Duration time_from_start - - - diff --git a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/package.xml b/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/package.xml deleted file mode 100644 index 9cfb7d0..0000000 --- a/src/rm_nav_bringup/config/rm_navigation/teb_local_planner/teb_msgs/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - teb_msgs - 0.0.1 - A package containing message definitions for teb_local_planner. - Vinnam Kim - Apache License 2.0 - - ament_cmake - - rosidl_default_generators - - builtin_interfaces - costmap_converter_msgs - geometry_msgs - std_msgs - - builtin_interfaces - costmap_converter_msgs - geometry_msgs - rosidl_default_runtime - std_msgs - - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - \ No newline at end of file