From ac932ea443ad1b3cd5e769aecb3c768baeb49359 Mon Sep 17 00:00:00 2001 From: Steven Macenski Date: Wed, 10 Oct 2018 17:57:48 +0200 Subject: [PATCH] Metapackage + Reorg + build script updates (#107) * moved over * moving all messages into an internal msgs metapackage grouping * moving all the messages into a homologated package * making necessary changes for the msgs package homologation * adding metapackage * adding myself as a maintainer * build script and doc updates for package in workspace * dealing with merge issues * remove author fields * moving around * merge conflict v3 * remove costmap 2d from utils * killing src dir * remove costmap 2d clone #3 * remove extraneous file * kill dwa for some reason * dwb rectification * removing oregon author -> maintainer * controller -> nav2_controller * adding new msgs to new things * tags * bringup pkg * compiling with task status mode * getting the task status to compile for dwa * adding costmap2d into nav2 naming nominclature * adding controller metapackage * recursive definitions -- bringup cannot be inside of the metapackage * kill bad line * hopefully actual last git merge error... * changelog resolve --- doc/BUILD.md | 14 +- .../doc => nav2_amcl}/.gitignore | 0 .../CHANGELOG.rst | 0 .../nav2_amcl => nav2_amcl}/CMakeLists.txt | 0 .../README.md | 0 .../nav2_amcl => nav2_amcl}/package.xml | 0 .../nav2_amcl => nav2_amcl}/src/amcl_node.cpp | 0 .../nav2_amcl => nav2_amcl}/src/amcl_node.hpp | 0 .../nav2_amcl => nav2_amcl}/src/main.cpp | 0 .../test/CMakeLists.txt | 0 .../test/integration/CMakeLists.txt | 0 .../test/integration/README.md | 0 .../integration/test_localization_launch.py | 0 .../integration/test_localization_node.cpp | 0 {launch => nav2_amcl/test/maps}/test_map.pgm | 0 .../test/maps/test_map.yaml | 0 .../localization_node.launch.py | 0 .../test/unit/CMakeLists.txt | 0 .../test/unit/test_localization_amcl.cpp | 0 .../CHANGELOG.rst | 0 .../CMakeLists.txt | 4 +- .../README.md | 0 .../doc/..gitignore | 0 .../nav2_astar_planner/astar_planner.hpp | 0 .../package.xml | 4 +- .../src/astar_planner.cpp | 0 .../src/main.cpp | 0 .../test/.gitignore | 0 .../nav2_robot => nav2_bringup}/CHANGELOG.rst | 0 .../CMakeLists.txt | 12 +- .../nav2_robot => nav2_bringup}/README.md | 0 .../launch}/core.launch.py | 0 .../launch}/system_test.rviz | 0 .../maps => nav2_bringup/launch}/test_map.pgm | 0 {launch => nav2_bringup/launch}/test_map.yaml | 0 .../package.xml | 11 +- .../CHANGELOG.rst | 0 .../CMakeLists.txt | 6 +- .../README.md | 0 .../doc/.gitignore | 0 .../nav2_bt_navigator/bt_navigator.hpp | 1 + .../package.xml | 4 +- .../src/bt_navigator.cpp | 0 .../src/main.cpp | 0 .../test/.gitignore | 0 .../costmap_queue/CMakeLists.txt | 4 +- .../include/costmap_queue/costmap_queue.h | 6 +- .../costmap_queue/limited_costmap_queue.h | 2 +- .../include/costmap_queue/map_based_queue.h | 0 .../costmap_queue/package.xml | 2 +- .../costmap_queue/src/costmap_queue.cpp | 2 +- .../src/limited_costmap_queue.cpp | 2 +- .../costmap_queue/test/mbq_test.cpp | 0 .../costmap_queue/test/utest.cpp | 4 +- .../dwb_core/CMakeLists.txt | 4 +- .../dwb_core/include/dwb_core/common_types.h | 4 +- .../dwb_core/include/dwb_core/dwb_core.h | 0 .../dwb_core/include/dwb_core/exceptions.h | 0 .../dwb_core/include/dwb_core/goal_checker.h | 0 .../dwb_core/illegal_trajectory_tracker.h | 0 .../dwb_core/include/dwb_core/publisher.h | 0 .../include/dwb_core/trajectory_critic.h | 0 .../include/dwb_core/trajectory_generator.h | 0 .../dwb_core/package.xml | 4 +- .../dwb_core/src/dwb_core.cpp | 2 +- .../src/illegal_trajectory_tracker.cpp | 0 .../dwb_core/src/publisher.cpp | 2 +- .../dwb_critics/CMakeLists.txt | 4 +- .../dwb_critics/default_critics.xml | 0 .../include/dwb_critics/alignment_util.h | 0 .../include/dwb_critics/base_obstacle.h | 2 +- .../include/dwb_critics/goal_align.h | 0 .../include/dwb_critics/goal_dist.h | 0 .../include/dwb_critics/line_iterator.h | 0 .../include/dwb_critics/map_grid.h | 4 +- .../include/dwb_critics/obstacle_footprint.h | 0 .../include/dwb_critics/oscillation.h | 0 .../include/dwb_critics/path_align.h | 0 .../include/dwb_critics/path_dist.h | 0 .../include/dwb_critics/prefer_forward.h | 0 .../include/dwb_critics/rotate_to_goal.h | 0 .../include/dwb_critics/twirling.h | 0 .../dwb_critics/package.xml | 2 +- .../dwb_critics/src/alignment_util.cpp | 0 .../dwb_critics/src/base_obstacle.cpp | 6 +- .../dwb_critics/src/goal_align.cpp | 0 .../dwb_critics/src/goal_dist.cpp | 4 +- .../dwb_critics/src/map_grid.cpp | 8 +- .../dwb_critics/src/obstacle_footprint.cpp | 6 +- .../dwb_critics/src/oscillation.cpp | 0 .../dwb_critics/src/path_align.cpp | 0 .../dwb_critics/src/path_dist.cpp | 4 +- .../dwb_critics/src/prefer_forward.cpp | 0 .../dwb_critics/src/rotate_to_goal.cpp | 0 .../dwb_critics/src/twirling.cpp | 0 .../dwb_msgs/CMakeLists.txt | 0 .../dwb_msgs/msg/CriticScore.msg | 0 .../dwb_msgs/msg/LocalPlanEvaluation.msg | 0 .../dwb_msgs/msg/Trajectory2D.msg | 0 .../dwb_msgs/msg/TrajectoryScore.msg | 0 .../dwb_msgs/package.xml | 0 .../dwb_msgs/srv/DebugLocalPlan.srv | 0 .../dwb_msgs/srv/GenerateTrajectory.srv | 0 .../dwb_msgs/srv/GenerateTwists.srv | 0 .../dwb_msgs/srv/GetCriticScore.srv | 0 .../dwb_msgs/srv/ScoreTrajectory.srv | 0 .../dwb_plugins/CMakeLists.txt | 0 .../dwb_plugins/cfg/KinematicParams.cfg | 0 .../dwb_plugins/kinematic_parameters.h | 0 .../dwb_plugins/limited_accel_generator.h | 0 .../dwb_plugins/one_d_velocity_iterator.h | 0 .../include/dwb_plugins/simple_goal_checker.h | 0 .../dwb_plugins/standard_traj_generator.h | 0 .../dwb_plugins/stopped_goal_checker.h | 0 .../include/dwb_plugins/velocity_iterator.h | 0 .../include/dwb_plugins/xy_theta_iterator.h | 0 .../dwb_plugins/package.xml | 0 .../dwb_plugins/plugins.xml | 0 .../dwb_plugins/src/kinematic_parameters.cpp | 0 .../src/limited_accel_generator.cpp | 0 .../dwb_plugins/src/simple_goal_checker.cpp | 0 .../src/standard_traj_generator.cpp | 0 .../dwb_plugins/src/stopped_goal_checker.cpp | 0 .../dwb_plugins/src/xy_theta_iterator.cpp | 0 .../dwb_plugins/test/goal_checker.cpp | 0 .../dwb_plugins/test/goal_checker.launch | 0 .../dwb_plugins/test/twist_gen.cpp | 0 .../dwb_plugins/test/twist_gen.launch | 0 .../test/velocity_iterator_test.cpp | 0 .../nav2_controller}/CHANGELOG.rst | 0 .../nav2_controller/CMakeLists.txt | 10 + nav2_controller/nav2_controller/package.xml | 28 ++ .../nav2_controller_dwb/CMakeLists.txt | 4 +- .../nav2_controller_dwb/dwb_controller.hpp | 0 .../nav2_controller_dwb/package.xml | 4 +- .../src/dwb_controller.cpp | 2 +- .../nav2_controller_dwb/src/main.cpp | 0 .../nav2_controller_example}/CHANGELOG.rst | 0 .../nav2_controller_example/CMakeLists.txt | 4 +- .../nav2_controller_example}/README.md | 0 .../nav2_controller_example/doc/.gitignore | 0 .../dwa_controller.hpp | 0 .../nav2_controller_example/package.xml | 4 +- .../src/dwa_controller.cpp | 0 .../nav2_controller_example/src/main.cpp | 0 .../nav2_controller_example/test/.gitignore | 0 .../nav_2d_msgs/CMakeLists.txt | 0 .../nav_2d_msgs/msg/Path2D.msg | 0 .../nav_2d_msgs/msg/Pose2D32.msg | 0 .../nav_2d_msgs/msg/Pose2DStamped.msg | 0 .../nav_2d_msgs/msg/Twist2D.msg | 0 .../nav_2d_msgs/msg/Twist2D32.msg | 0 .../nav_2d_msgs/msg/Twist2DStamped.msg | 0 .../nav_2d_msgs/package.xml | 0 .../nav_2d_utils/CMakeLists.txt | 0 .../include/nav_2d_utils/conversions.h | 0 .../include/nav_2d_utils/odom_subscriber.h | 0 .../include/nav_2d_utils/parameters.h | 0 .../include/nav_2d_utils/path_ops.h | 0 .../include/nav_2d_utils/tf_help.h | 0 .../nav_2d_utils/package.xml | 0 .../nav_2d_utils/src/conversions.cpp | 0 .../nav_2d_utils/src/path_ops.cpp | 0 nav2_costmap_2d/CHANGELOG.rst | 7 + .../CMakeLists.txt | 30 +-- .../cfg/Costmap2D.cfg | 2 +- .../cfg/GenericPlugin.cfg | 2 +- .../cfg/InflationPlugin.cfg | 2 +- .../cfg/ObstaclePlugin.cfg | 2 +- .../cfg/VoxelPlugin.cfg | 2 +- .../costmap_plugins.xml | 8 +- .../include/nav2_costmap_2d}/array_parser.h | 10 +- .../include/nav2_costmap_2d}/cost_values.h | 8 +- .../include/nav2_costmap_2d}/costmap_2d.h | 10 +- .../nav2_costmap_2d}/costmap_2d_publisher.h | 12 +- .../include/nav2_costmap_2d}/costmap_2d_ros.h | 34 +-- .../include/nav2_costmap_2d}/costmap_layer.h | 22 +- .../include/nav2_costmap_2d}/costmap_math.h | 6 +- .../include/nav2_costmap_2d}/footprint.h | 10 +- .../nav2_costmap_2d}/inflation_layer.h | 22 +- .../include/nav2_costmap_2d}/layer.h | 14 +- .../nav2_costmap_2d}/layered_costmap.h | 18 +- .../include/nav2_costmap_2d}/observation.h | 10 +- .../nav2_costmap_2d}/observation_buffer.h | 12 +- .../include/nav2_costmap_2d}/obstacle_layer.h | 48 ++-- .../include/nav2_costmap_2d}/static_layer.h | 22 +- .../include/nav2_costmap_2d}/testing_helper.h | 52 ++-- .../include/nav2_costmap_2d}/voxel_layer.h | 28 +- .../launch/example.launch | 6 +- .../launch/example_params.yaml | 0 .../msg/VoxelGrid.msg | 0 .../package.xml | 10 +- .../plugins/inflation_layer.cpp | 36 +-- .../plugins/obstacle_layer.cpp | 32 +-- .../plugins/static_layer.cpp | 42 +-- .../plugins/voxel_layer.cpp | 28 +- .../src/array_parser.cpp | 4 +- .../src/costmap_2d.cpp | 6 +- .../src/costmap_2d_cloud.cpp | 8 +- .../src/costmap_2d_markers.cpp | 8 +- .../src/costmap_2d_node.cpp | 4 +- .../src/costmap_2d_publisher.cpp | 8 +- .../src/costmap_2d_ros.cpp | 46 ++-- .../src/costmap_layer.cpp | 18 +- .../src/costmap_math.cpp | 2 +- .../src/footprint.cpp | 24 +- .../src/layer.cpp | 6 +- .../src/layered_costmap.cpp | 14 +- .../src/observation_buffer.cpp | 14 +- .../test/TenByTen.pgm | Bin .../test/TenByTen.yaml | 0 .../test/array_parser_test.cpp | 4 +- .../test/costmap_params.yaml | 0 .../test/costmap_tester.cpp | 30 +-- .../test/footprint_tests.cpp | 2 +- .../test/footprint_tests.launch | 2 +- .../test/inflation_tests.cpp | 38 +-- .../test/inflation_tests.launch | 4 +- .../test/module_tests.cpp | 78 +++--- .../test/obstacle_tests.cpp | 26 +- nav2_costmap_2d/test/obstacle_tests.launch | 5 + nav2_costmap_2d/test/simple_driving_test.xml | 14 + .../test/static_tests.cpp | 24 +- nav2_costmap_2d/test/static_tests.launch | 5 + .../CHANGELOG.rst | 0 .../CMakeLists.txt | 6 +- .../README.md | 0 .../doc/.gitignore | 0 .../costmap_world_model.hpp | 12 +- .../package.xml | 6 +- .../src/costmap_world_model.cpp | 6 +- .../src/main.cpp | 0 .../test/.gitignore | 0 .../CHANGELOG.rst | 0 .../CMakeLists.txt | 8 +- .../README.md | 0 .../doc/..gitignore | 0 .../dijkstra_planner.hpp | 14 +- .../include/nav2_dijkstra_planner/navfn.hpp | 0 .../package.xml | 8 +- .../src/dijkstra_planner.cpp | 16 +- .../src/main.cpp | 0 .../src/navfn.cpp | 0 .../test/.gitignore | 0 .../.gitignore | 0 .../README.md => nav2_map_server/.gitignore | 0 .../CHANGELOG.rst | 0 .../CMakeLists.txt | 0 .../README.md | 0 .../nav2_map_server/base_map_server.hpp | 0 .../include/nav2_map_server/map_factory.hpp | 0 .../nav2_map_server/map_reps/map_reps.hpp | 0 .../map_reps/occ_grid_server.hpp | 0 .../nav2_map_server/map_server_ros.hpp | 0 .../package.xml | 0 .../src/main.cpp | 0 .../src/map_reps/map_factory.cpp | 0 .../src/map_reps/occ_grid_server.cpp | 0 .../src/map_server_ros.cpp | 0 .../test/CMakeLists.txt | 0 .../test/component/CMakeLists.txt | 0 .../test/component/test_occ_grid_launch.py | 0 .../test/component/test_occ_grid_node.cpp | 0 .../test/test_constants.cpp | 0 .../test/test_constants/test_constants.h | 0 .../map_server_node.launch.cpython-35.pyc | Bin 0 -> 659 bytes .../map_server_node.launch.py | 0 .../test/testmap.bmp | Bin .../test/testmap.png | Bin .../test/testmap.yaml | 0 .../test/unit/CMakeLists.txt | 0 .../test/unit/test_occ_grid.cpp | 0 .../CHANGELOG.rst | 0 .../CMakeLists.txt | 4 +- .../README.md | 0 .../doc/.gitignore | 0 .../mission_executor.hpp | 3 +- .../package.xml | 4 +- .../src/main.cpp | 0 .../src/mission_executor.cpp | 4 +- .../test/.gitignore | 0 nav2_msgs/CHANGELOG.rst | 0 .../CMakeLists.txt | 7 +- nav2_msgs/README.md | 0 .../msg/Costmap.msg | 0 .../msg/CostmapMetaData.msg | 0 .../msg/MissionPlan.msg | 0 .../msg/Path.msg | 2 +- .../msg/PathEndPoints.msg | 2 +- nav2_msgs/msg/TaskStatus.msg | 5 + .../package.xml | 5 +- .../srv/GetCostmap.srv | 4 +- .../CMakeLists.txt | 8 +- .../example_result.png | Bin .../maps/map.pgm | Bin .../maps/map.xcf | Bin .../maps/map_circular.pgm | 0 .../maps/map_circular.yaml | 0 .../package.xml | 8 +- .../planner_tester.cpp | 8 +- .../planner_tester.hpp | 6 +- .../test_planner_launch.py | 0 .../test_planner_node.cpp | 0 nav2_robot/CHANGELOG.rst | 0 .../nav2_robot => nav2_robot}/CMakeLists.txt | 0 nav2_robot/README.md | 0 nav2_robot/doc/.gitignore | 0 .../include/nav2_robot/robot.hpp | 0 .../include/nav2_robot/ros_robot.hpp | 0 .../nav2_robot => nav2_robot}/package.xml | 0 .../src/ros_robot.cpp | 0 nav2_robot/test/.gitignore | 0 nav2_simple_navigator/CHANGELOG.rst | 0 .../CMakeLists.txt | 4 +- nav2_simple_navigator/README.md | 0 nav2_simple_navigator/doc/.gitignore | 0 .../simple_navigator.hpp | 1 + .../package.xml | 4 +- .../src/main.cpp | 0 .../src/simple_navigator.cpp | 0 nav2_simple_navigator/test/.gitignore | 0 nav2_tasks/CHANGELOG.rst | 0 .../nav2_tasks => nav2_tasks}/CMakeLists.txt | 6 +- nav2_tasks/README.md | 0 nav2_tasks/doc/.gitignore | 0 .../nav2_tasks/compute_path_to_pose_task.hpp | 8 +- .../nav2_tasks/costmap_service_client.hpp | 10 +- .../nav2_tasks/execute_mission_task.hpp | 4 +- .../include/nav2_tasks/follow_path_task.hpp | 4 +- .../include/nav2_tasks/map_service_client.hpp | 0 .../nav2_tasks/navigate_to_pose_task.hpp | 0 .../include/nav2_tasks/service_client.hpp | 0 .../include/nav2_tasks/task_client.hpp | 8 +- .../include/nav2_tasks/task_server.hpp | 10 +- .../include/nav2_tasks/task_status.hpp | 10 +- .../msg/TaskStatus.msg | 0 .../nav2_tasks => nav2_tasks}/package.xml | 2 + nav2_tasks/test/.gitignore | 0 nav2_util/CHANGELOG.rst | 0 .../nav2_util => nav2_util}/CMakeLists.txt | 4 +- nav2_util/README.md | 0 nav2_util/doc/.gitignore | 0 .../include/nav2_util/costmap.hpp | 10 +- .../include/nav2_util/map/map.h | 0 .../nav2_util/map_loader/map_loader.hpp | 0 .../include/nav2_util/pf/eig3.h | 0 .../include/nav2_util/pf/pf.h | 0 .../include/nav2_util/pf/pf_kdtree.h | 0 .../include/nav2_util/pf/pf_pdf.h | 0 .../include/nav2_util/pf/pf_vector.h | 0 .../include/nav2_util/sensors/laser.h | 0 .../include/nav2_util/sensors/odom.h | 0 .../include/nav2_util/sensors/sensor.h | 0 .../include/nav2_util/strutils.hpp | 0 {src/libs/nav2_util => nav2_util}/package.xml | 4 +- .../nav2_util => nav2_util}/src/costmap.cpp | 6 +- .../nav2_util => nav2_util}/src/map/map.c | 0 .../src/map/map_cspace.cpp | 0 .../src/map/map_draw.c | 0 .../src/map/map_range.c | 0 .../src/map/map_store.c | 0 .../src/map_loader/map_loader.cpp | 0 .../nav2_util => nav2_util}/src/pf/eig3.c | 0 {src/libs/nav2_util => nav2_util}/src/pf/pf.c | 0 .../nav2_util => nav2_util}/src/pf/pf_draw.c | 0 .../src/pf/pf_kdtree.c | 0 .../nav2_util => nav2_util}/src/pf/pf_pdf.c | 0 .../src/pf/pf_vector.c | 0 .../src/sensors/laser.cpp | 0 .../src/sensors/odom.cpp | 0 .../src/sensors/sensor.cpp | 0 nav2_util/test/.gitignore | 0 navigation2/CHANGELOG.rst | 0 navigation2/CMakeLists.txt | 10 + navigation2/package.xml | 35 +++ src/libs/costmap_2d/CHANGELOG.rst | 245 ------------------ .../costmap_2d/test/obstacle_tests.launch | 5 - .../costmap_2d/test/simple_driving_test.xml | 14 - src/libs/costmap_2d/test/static_tests.launch | 5 - src/libs/nav2_util/msg/Costmap.msg | 9 - src/libs/nav2_util/msg/CostmapMetaData.msg | 23 -- .../nav2_mission_execution_msgs/package.xml | 29 --- .../nav2_planning_msgs/CMakeLists.txt | 27 -- .../nav2_world_model_msgs/CMakeLists.txt | 27 -- .../nav2_world_model_msgs/package.xml | 31 --- tools/build_all.sh | 4 +- tools/initial_ros_setup.sh | 165 ++++++------ 387 files changed, 844 insertions(+), 1157 deletions(-) rename {src/control/nav2_controller_example/doc => nav2_amcl}/.gitignore (100%) rename {src/control/nav2_controller_example => nav2_amcl}/CHANGELOG.rst (100%) rename {src/localization/nav2_amcl => nav2_amcl}/CMakeLists.txt (100%) rename {src/control/nav2_controller_example => nav2_amcl}/README.md (100%) rename {src/localization/nav2_amcl => nav2_amcl}/package.xml (100%) rename {src/localization/nav2_amcl => nav2_amcl}/src/amcl_node.cpp (100%) rename {src/localization/nav2_amcl => nav2_amcl}/src/amcl_node.hpp (100%) rename {src/localization/nav2_amcl => nav2_amcl}/src/main.cpp (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/CMakeLists.txt (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/integration/CMakeLists.txt (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/integration/README.md (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/integration/test_localization_launch.py (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/integration/test_localization_node.cpp (100%) rename {launch => nav2_amcl/test/maps}/test_map.pgm (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/maps/test_map.yaml (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/test_launch_files/localization_node.launch.py (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/unit/CMakeLists.txt (100%) rename {src/localization/nav2_amcl => nav2_amcl}/test/unit/test_localization_amcl.cpp (100%) rename {src/libs/nav2_libs_msgs => nav2_astar_planner}/CHANGELOG.rst (100%) rename {src/planning/nav2_astar_planner => nav2_astar_planner}/CMakeLists.txt (95%) rename {src/libs/nav2_libs_msgs => nav2_astar_planner}/README.md (100%) rename {src/planning/nav2_astar_planner => nav2_astar_planner}/doc/..gitignore (100%) rename {src/planning/nav2_astar_planner => nav2_astar_planner}/include/nav2_astar_planner/astar_planner.hpp (100%) rename {src/planning/nav2_astar_planner => nav2_astar_planner}/package.xml (89%) rename {src/planning/nav2_astar_planner => nav2_astar_planner}/src/astar_planner.cpp (100%) rename {src/planning/nav2_astar_planner => nav2_astar_planner}/src/main.cpp (100%) rename {src/control/nav2_controller_example => nav2_astar_planner}/test/.gitignore (100%) rename {src/libs/nav2_robot => nav2_bringup}/CHANGELOG.rst (100%) rename {src/mission_execution/nav2_mission_execution_msgs => nav2_bringup}/CMakeLists.txt (66%) rename {src/libs/nav2_robot => nav2_bringup}/README.md (100%) rename {launch => nav2_bringup/launch}/core.launch.py (100%) rename {launch => nav2_bringup/launch}/system_test.rviz (100%) rename {src/localization/nav2_amcl/test/maps => nav2_bringup/launch}/test_map.pgm (100%) rename {launch => nav2_bringup/launch}/test_map.yaml (100%) rename {src/libs/nav2_libs_msgs => nav2_bringup}/package.xml (73%) rename {src/libs/nav2_tasks => nav2_bt_navigator}/CHANGELOG.rst (100%) rename {src/navigation/nav2_bt_navigator => nav2_bt_navigator}/CMakeLists.txt (95%) rename {src/libs/nav2_tasks => nav2_bt_navigator}/README.md (100%) rename src/libs/nav2_util/CHANGELOG.rst => nav2_bt_navigator/doc/.gitignore (100%) rename {src/navigation/nav2_bt_navigator => nav2_bt_navigator}/include/nav2_bt_navigator/bt_navigator.hpp (97%) rename {src/navigation/nav2_bt_navigator => nav2_bt_navigator}/package.xml (88%) rename {src/navigation/nav2_bt_navigator => nav2_bt_navigator}/src/bt_navigator.cpp (100%) rename {src/navigation/nav2_bt_navigator => nav2_bt_navigator}/src/main.cpp (100%) rename src/libs/nav2_util/README.md => nav2_bt_navigator/test/.gitignore (100%) rename {src/control => nav2_controller}/costmap_queue/CMakeLists.txt (95%) rename {src/control => nav2_controller}/costmap_queue/include/costmap_queue/costmap_queue.h (97%) rename {src/control => nav2_controller}/costmap_queue/include/costmap_queue/limited_costmap_queue.h (96%) rename {src/control => nav2_controller}/costmap_queue/include/costmap_queue/map_based_queue.h (100%) rename {src/control => nav2_controller}/costmap_queue/package.xml (95%) rename {src/control => nav2_controller}/costmap_queue/src/costmap_queue.cpp (98%) rename {src/control => nav2_controller}/costmap_queue/src/limited_costmap_queue.cpp (94%) rename {src/control => nav2_controller}/costmap_queue/test/mbq_test.cpp (100%) rename {src/control => nav2_controller}/costmap_queue/test/utest.cpp (97%) rename {src/control => nav2_controller}/dwb_core/CMakeLists.txt (96%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/common_types.h (88%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/dwb_core.h (100%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/exceptions.h (100%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/goal_checker.h (100%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/illegal_trajectory_tracker.h (100%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/publisher.h (100%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/trajectory_critic.h (100%) rename {src/control => nav2_controller}/dwb_core/include/dwb_core/trajectory_generator.h (100%) rename {src/control => nav2_controller}/dwb_core/package.xml (94%) rename {src/control => nav2_controller}/dwb_core/src/dwb_core.cpp (99%) rename {src/control => nav2_controller}/dwb_core/src/illegal_trajectory_tracker.cpp (100%) rename {src/control => nav2_controller}/dwb_core/src/publisher.cpp (99%) rename {src/control => nav2_controller}/dwb_critics/CMakeLists.txt (96%) rename {src/control => nav2_controller}/dwb_critics/default_critics.xml (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/alignment_util.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/base_obstacle.h (98%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/goal_align.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/goal_dist.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/line_iterator.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/map_grid.h (97%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/obstacle_footprint.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/oscillation.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/path_align.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/path_dist.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/prefer_forward.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/rotate_to_goal.h (100%) rename {src/control => nav2_controller}/dwb_critics/include/dwb_critics/twirling.h (100%) rename {src/control => nav2_controller}/dwb_critics/package.xml (96%) rename {src/control => nav2_controller}/dwb_critics/src/alignment_util.cpp (100%) rename {src/control => nav2_controller}/dwb_critics/src/base_obstacle.cpp (94%) rename {src/control => nav2_controller}/dwb_critics/src/goal_align.cpp (100%) rename {src/control => nav2_controller}/dwb_critics/src/goal_dist.cpp (96%) rename {src/control => nav2_controller}/dwb_critics/src/map_grid.cpp (95%) rename {src/control => nav2_controller}/dwb_critics/src/obstacle_footprint.cpp (97%) rename {src/control => nav2_controller}/dwb_critics/src/oscillation.cpp (100%) rename {src/control => nav2_controller}/dwb_critics/src/path_align.cpp (100%) rename {src/control => nav2_controller}/dwb_critics/src/path_dist.cpp (96%) rename {src/control => nav2_controller}/dwb_critics/src/prefer_forward.cpp (100%) rename {src/control => nav2_controller}/dwb_critics/src/rotate_to_goal.cpp (100%) rename {src/control => nav2_controller}/dwb_critics/src/twirling.cpp (100%) rename {src/control => nav2_controller}/dwb_msgs/CMakeLists.txt (100%) rename {src/control => nav2_controller}/dwb_msgs/msg/CriticScore.msg (100%) rename {src/control => nav2_controller}/dwb_msgs/msg/LocalPlanEvaluation.msg (100%) rename {src/control => nav2_controller}/dwb_msgs/msg/Trajectory2D.msg (100%) rename {src/control => nav2_controller}/dwb_msgs/msg/TrajectoryScore.msg (100%) rename {src/control => nav2_controller}/dwb_msgs/package.xml (100%) rename {src/control => nav2_controller}/dwb_msgs/srv/DebugLocalPlan.srv (100%) rename {src/control => nav2_controller}/dwb_msgs/srv/GenerateTrajectory.srv (100%) rename {src/control => nav2_controller}/dwb_msgs/srv/GenerateTwists.srv (100%) rename {src/control => nav2_controller}/dwb_msgs/srv/GetCriticScore.srv (100%) rename {src/control => nav2_controller}/dwb_msgs/srv/ScoreTrajectory.srv (100%) rename {src/control => nav2_controller}/dwb_plugins/CMakeLists.txt (100%) rename {src/control => nav2_controller}/dwb_plugins/cfg/KinematicParams.cfg (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/kinematic_parameters.h (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/limited_accel_generator.h (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.h (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/simple_goal_checker.h (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/standard_traj_generator.h (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/stopped_goal_checker.h (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/velocity_iterator.h (100%) rename {src/control => nav2_controller}/dwb_plugins/include/dwb_plugins/xy_theta_iterator.h (100%) rename {src/control => nav2_controller}/dwb_plugins/package.xml (100%) rename {src/control => nav2_controller}/dwb_plugins/plugins.xml (100%) rename {src/control => nav2_controller}/dwb_plugins/src/kinematic_parameters.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/src/limited_accel_generator.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/src/simple_goal_checker.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/src/standard_traj_generator.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/src/stopped_goal_checker.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/src/xy_theta_iterator.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/test/goal_checker.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/test/goal_checker.launch (100%) rename {src/control => nav2_controller}/dwb_plugins/test/twist_gen.cpp (100%) rename {src/control => nav2_controller}/dwb_plugins/test/twist_gen.launch (100%) rename {src/control => nav2_controller}/dwb_plugins/test/velocity_iterator_test.cpp (100%) rename {src/localization/nav2_amcl => nav2_controller/nav2_controller}/CHANGELOG.rst (100%) create mode 100644 nav2_controller/nav2_controller/CMakeLists.txt create mode 100644 nav2_controller/nav2_controller/package.xml rename {src/control => nav2_controller}/nav2_controller_dwb/CMakeLists.txt (95%) rename {src/control => nav2_controller}/nav2_controller_dwb/include/nav2_controller_dwb/dwb_controller.hpp (100%) rename {src/control => nav2_controller}/nav2_controller_dwb/package.xml (89%) rename {src/control => nav2_controller}/nav2_controller_dwb/src/dwb_controller.cpp (97%) rename {src/control => nav2_controller}/nav2_controller_dwb/src/main.cpp (100%) rename {src/mapping/nav2_map_server => nav2_controller/nav2_controller_example}/CHANGELOG.rst (100%) rename {src/control => nav2_controller}/nav2_controller_example/CMakeLists.txt (95%) rename {src/localization/nav2_amcl => nav2_controller/nav2_controller_example}/README.md (100%) rename src/mapping/nav2_map_server/README.md => nav2_controller/nav2_controller_example/doc/.gitignore (100%) rename {src/control => nav2_controller}/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp (100%) rename {src/control => nav2_controller}/nav2_controller_example/package.xml (89%) rename {src/control => nav2_controller}/nav2_controller_example/src/dwa_controller.cpp (100%) rename {src/control => nav2_controller}/nav2_controller_example/src/main.cpp (100%) rename src/mission_execution/nav2_mission_execution_msgs/CHANGELOG.rst => nav2_controller/nav2_controller_example/test/.gitignore (100%) rename {src/control => nav2_controller}/nav_2d_msgs/CMakeLists.txt (100%) rename {src/control => nav2_controller}/nav_2d_msgs/msg/Path2D.msg (100%) rename {src/control => nav2_controller}/nav_2d_msgs/msg/Pose2D32.msg (100%) rename {src/control => nav2_controller}/nav_2d_msgs/msg/Pose2DStamped.msg (100%) rename {src/control => nav2_controller}/nav_2d_msgs/msg/Twist2D.msg (100%) rename {src/control => nav2_controller}/nav_2d_msgs/msg/Twist2D32.msg (100%) rename {src/control => nav2_controller}/nav_2d_msgs/msg/Twist2DStamped.msg (100%) rename {src/control => nav2_controller}/nav_2d_msgs/package.xml (100%) rename {src/control => nav2_controller}/nav_2d_utils/CMakeLists.txt (100%) rename {src/control => nav2_controller}/nav_2d_utils/include/nav_2d_utils/conversions.h (100%) rename {src/control => nav2_controller}/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h (100%) rename {src/control => nav2_controller}/nav_2d_utils/include/nav_2d_utils/parameters.h (100%) rename {src/control => nav2_controller}/nav_2d_utils/include/nav_2d_utils/path_ops.h (100%) rename {src/control => nav2_controller}/nav_2d_utils/include/nav_2d_utils/tf_help.h (100%) rename {src/control => nav2_controller}/nav_2d_utils/package.xml (100%) rename {src/control => nav2_controller}/nav_2d_utils/src/conversions.cpp (100%) rename {src/control => nav2_controller}/nav_2d_utils/src/path_ops.cpp (100%) create mode 100644 nav2_costmap_2d/CHANGELOG.rst rename {src/libs/costmap_2d => nav2_costmap_2d}/CMakeLists.txt (78%) rename {src/libs/costmap_2d => nav2_costmap_2d}/cfg/Costmap2D.cfg (95%) rename {src/libs/costmap_2d => nav2_costmap_2d}/cfg/GenericPlugin.cfg (74%) rename {src/libs/costmap_2d => nav2_costmap_2d}/cfg/InflationPlugin.cfg (87%) rename {src/libs/costmap_2d => nav2_costmap_2d}/cfg/ObstaclePlugin.cfg (94%) rename {src/libs/costmap_2d => nav2_costmap_2d}/cfg/VoxelPlugin.cfg (94%) rename {src/libs/costmap_2d => nav2_costmap_2d}/costmap_plugins.xml (61%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/array_parser.h (92%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/cost_values.h (93%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/costmap_2d.h (98%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/costmap_2d_publisher.h (93%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/costmap_2d_ros.h (91%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/costmap_layer.h (89%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/costmap_math.h (95%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/footprint.h (97%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/inflation_layer.h (91%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/layer.h (94%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/layered_costmap.h (92%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/observation.h (95%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/observation_buffer.h (95%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/obstacle_layer.h (76%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/static_layer.h (86%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/testing_helper.h (50%) rename {src/libs/costmap_2d/include/costmap_2d => nav2_costmap_2d/include/nav2_costmap_2d}/voxel_layer.h (87%) rename {src/libs/costmap_2d => nav2_costmap_2d}/launch/example.launch (67%) rename {src/libs/costmap_2d => nav2_costmap_2d}/launch/example_params.yaml (100%) rename {src/libs/costmap_2d => nav2_costmap_2d}/msg/VoxelGrid.msg (100%) rename {src/libs/costmap_2d => nav2_costmap_2d}/package.xml (82%) rename {src/libs/costmap_2d => nav2_costmap_2d}/plugins/inflation_layer.cpp (91%) rename {src/libs/costmap_2d => nav2_costmap_2d}/plugins/obstacle_layer.cpp (95%) rename {src/libs/costmap_2d => nav2_costmap_2d}/plugins/static_layer.cpp (88%) rename {src/libs/costmap_2d => nav2_costmap_2d}/plugins/voxel_layer.cpp (95%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/array_parser.cpp (98%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_2d.cpp (99%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_2d_cloud.cpp (96%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_2d_markers.cpp (94%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_2d_node.cpp (95%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_2d_publisher.cpp (97%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_2d_ros.cpp (92%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_layer.cpp (84%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/costmap_math.cpp (98%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/footprint.cpp (95%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/layer.cpp (95%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/layered_costmap.cpp (92%) rename {src/libs/costmap_2d => nav2_costmap_2d}/src/observation_buffer.cpp (96%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/TenByTen.pgm (100%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/TenByTen.yaml (100%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/array_parser_test.cpp (97%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/costmap_params.yaml (100%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/costmap_tester.cpp (84%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/footprint_tests.cpp (99%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/footprint_tests.launch (91%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/inflation_tests.cpp (90%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/inflation_tests.launch (52%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/module_tests.cpp (91%) rename {src/libs/costmap_2d => nav2_costmap_2d}/test/obstacle_tests.cpp (90%) create mode 100644 nav2_costmap_2d/test/obstacle_tests.launch create mode 100644 nav2_costmap_2d/test/simple_driving_test.xml rename {src/libs/costmap_2d => nav2_costmap_2d}/test/static_tests.cpp (94%) create mode 100644 nav2_costmap_2d/test/static_tests.launch rename {src/mission_execution/nav2_mission_executor => nav2_costmap_world_model}/CHANGELOG.rst (100%) rename {src/world_model/nav2_costmap_world_model => nav2_costmap_world_model}/CMakeLists.txt (92%) rename {src/mission_execution/nav2_mission_execution_msgs => nav2_costmap_world_model}/README.md (100%) rename src/mission_execution/nav2_mission_executor/README.md => nav2_costmap_world_model/doc/.gitignore (100%) rename {src/world_model/nav2_costmap_world_model => nav2_costmap_world_model}/include/nav2_costmap_world_model/costmap_world_model.hpp (79%) rename {src/world_model/nav2_costmap_world_model => nav2_costmap_world_model}/package.xml (81%) rename {src/world_model/nav2_costmap_world_model => nav2_costmap_world_model}/src/costmap_world_model.cpp (87%) rename {src/world_model/nav2_costmap_world_model => nav2_costmap_world_model}/src/main.cpp (100%) rename src/navigation/nav2_bt_navigator/CHANGELOG.rst => nav2_costmap_world_model/test/.gitignore (100%) rename {src/navigation/nav2_simple_navigator => nav2_dijkstra_planner}/CHANGELOG.rst (100%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/CMakeLists.txt (88%) rename {src/navigation/nav2_bt_navigator => nav2_dijkstra_planner}/README.md (100%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/doc/..gitignore (100%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/include/nav2_dijkstra_planner/dijkstra_planner.hpp (91%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/include/nav2_dijkstra_planner/navfn.hpp (100%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/package.xml (81%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/src/dijkstra_planner.cpp (97%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/src/main.cpp (100%) rename {src/planning/nav2_dijkstra_planner => nav2_dijkstra_planner}/src/navfn.cpp (100%) rename src/navigation/nav2_simple_navigator/README.md => nav2_dijkstra_planner/test/.gitignore (100%) rename src/planning/nav2_astar_planner/CHANGELOG.rst => nav2_gazebo_localizer/.gitignore (100%) rename src/planning/nav2_astar_planner/README.md => nav2_map_server/.gitignore (100%) rename {src/planning/nav2_dijkstra_planner => nav2_map_server}/CHANGELOG.rst (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/CMakeLists.txt (100%) rename {src/planning/nav2_dijkstra_planner => nav2_map_server}/README.md (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/include/nav2_map_server/base_map_server.hpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/include/nav2_map_server/map_factory.hpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/include/nav2_map_server/map_reps/map_reps.hpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/include/nav2_map_server/map_reps/occ_grid_server.hpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/include/nav2_map_server/map_server_ros.hpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/package.xml (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/src/main.cpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/src/map_reps/map_factory.cpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/src/map_reps/occ_grid_server.cpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/src/map_server_ros.cpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/CMakeLists.txt (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/component/CMakeLists.txt (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/component/test_occ_grid_launch.py (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/component/test_occ_grid_node.cpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/test_constants.cpp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/test_constants/test_constants.h (100%) create mode 100644 nav2_map_server/test/test_launch_files/__pycache__/map_server_node.launch.cpython-35.pyc rename {src/mapping/nav2_map_server => nav2_map_server}/test/test_launch_files/map_server_node.launch.py (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/testmap.bmp (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/testmap.png (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/testmap.yaml (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/unit/CMakeLists.txt (100%) rename {src/mapping/nav2_map_server => nav2_map_server}/test/unit/test_occ_grid.cpp (100%) rename {src/planning/nav2_planning_msgs => nav2_mission_executor}/CHANGELOG.rst (100%) rename {src/mission_execution/nav2_mission_executor => nav2_mission_executor}/CMakeLists.txt (93%) rename {src/planning/nav2_planning_msgs => nav2_mission_executor}/README.md (100%) rename src/world_model/nav2_costmap_world_model/CHANGELOG.rst => nav2_mission_executor/doc/.gitignore (100%) rename {src/mission_execution/nav2_mission_executor => nav2_mission_executor}/include/nav2_mission_executor/mission_executor.hpp (94%) rename {src/mission_execution/nav2_mission_executor => nav2_mission_executor}/package.xml (87%) rename {src/mission_execution/nav2_mission_executor => nav2_mission_executor}/src/main.cpp (100%) rename {src/mission_execution/nav2_mission_executor => nav2_mission_executor}/src/mission_executor.cpp (93%) rename src/world_model/nav2_costmap_world_model/README.md => nav2_mission_executor/test/.gitignore (100%) create mode 100644 nav2_msgs/CHANGELOG.rst rename {src/libs/nav2_libs_msgs => nav2_msgs}/CMakeLists.txt (83%) create mode 100644 nav2_msgs/README.md rename {src/libs/nav2_libs_msgs => nav2_msgs}/msg/Costmap.msg (100%) rename {src/libs/nav2_libs_msgs => nav2_msgs}/msg/CostmapMetaData.msg (100%) rename {src/mission_execution/nav2_mission_execution_msgs => nav2_msgs}/msg/MissionPlan.msg (100%) rename {src/planning/nav2_planning_msgs => nav2_msgs}/msg/Path.msg (76%) rename {src/planning/nav2_planning_msgs => nav2_msgs}/msg/PathEndPoints.msg (93%) create mode 100644 nav2_msgs/msg/TaskStatus.msg rename {src/planning/nav2_planning_msgs => nav2_msgs}/package.xml (83%) rename {src/world_model/nav2_world_model_msgs => nav2_msgs}/srv/GetCostmap.srv (50%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/CMakeLists.txt (87%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/example_result.png (100%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/maps/map.pgm (100%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/maps/map.xcf (100%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/maps/map_circular.pgm (100%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/maps/map_circular.yaml (100%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/package.xml (77%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/planner_tester.cpp (97%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/planner_tester.hpp (95%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/test_planner_launch.py (100%) rename {src/planning/nav2_planning_tests => nav2_planning_tests}/test_planner_node.cpp (100%) create mode 100644 nav2_robot/CHANGELOG.rst rename {src/libs/nav2_robot => nav2_robot}/CMakeLists.txt (100%) create mode 100644 nav2_robot/README.md create mode 100644 nav2_robot/doc/.gitignore rename {src/libs/nav2_robot => nav2_robot}/include/nav2_robot/robot.hpp (100%) rename {src/libs/nav2_robot => nav2_robot}/include/nav2_robot/ros_robot.hpp (100%) rename {src/libs/nav2_robot => nav2_robot}/package.xml (100%) rename {src/libs/nav2_robot => nav2_robot}/src/ros_robot.cpp (100%) create mode 100644 nav2_robot/test/.gitignore create mode 100644 nav2_simple_navigator/CHANGELOG.rst rename {src/navigation/nav2_simple_navigator => nav2_simple_navigator}/CMakeLists.txt (95%) create mode 100644 nav2_simple_navigator/README.md create mode 100644 nav2_simple_navigator/doc/.gitignore rename {src/navigation/nav2_simple_navigator => nav2_simple_navigator}/include/nav2_simple_navigator/simple_navigator.hpp (97%) rename {src/navigation/nav2_simple_navigator => nav2_simple_navigator}/package.xml (89%) rename {src/navigation/nav2_simple_navigator => nav2_simple_navigator}/src/main.cpp (100%) rename {src/navigation/nav2_simple_navigator => nav2_simple_navigator}/src/simple_navigator.cpp (100%) create mode 100644 nav2_simple_navigator/test/.gitignore create mode 100644 nav2_tasks/CHANGELOG.rst rename {src/libs/nav2_tasks => nav2_tasks}/CMakeLists.txt (87%) create mode 100644 nav2_tasks/README.md create mode 100644 nav2_tasks/doc/.gitignore rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/compute_path_to_pose_task.hpp (84%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/costmap_service_client.hpp (72%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/execute_mission_task.hpp (90%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/follow_path_task.hpp (92%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/map_service_client.hpp (100%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/navigate_to_pose_task.hpp (100%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/service_client.hpp (100%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/task_client.hpp (96%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/task_server.hpp (94%) rename {src/libs/nav2_tasks => nav2_tasks}/include/nav2_tasks/task_status.hpp (76%) rename {src/libs/nav2_tasks => nav2_tasks}/msg/TaskStatus.msg (100%) rename {src/libs/nav2_tasks => nav2_tasks}/package.xml (93%) create mode 100644 nav2_tasks/test/.gitignore create mode 100644 nav2_util/CHANGELOG.rst rename {src/libs/nav2_util => nav2_util}/CMakeLists.txt (96%) create mode 100644 nav2_util/README.md create mode 100644 nav2_util/doc/.gitignore rename {src/libs/nav2_util => nav2_util}/include/nav2_util/costmap.hpp (87%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/map/map.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/map_loader/map_loader.hpp (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/pf/eig3.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/pf/pf.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/pf/pf_kdtree.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/pf/pf_pdf.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/pf/pf_vector.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/sensors/laser.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/sensors/odom.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/sensors/sensor.h (100%) rename {src/libs/nav2_util => nav2_util}/include/nav2_util/strutils.hpp (100%) rename {src/libs/nav2_util => nav2_util}/package.xml (92%) rename {src/libs/nav2_util => nav2_util}/src/costmap.cpp (98%) rename {src/libs/nav2_util => nav2_util}/src/map/map.c (100%) rename {src/libs/nav2_util => nav2_util}/src/map/map_cspace.cpp (100%) rename {src/libs/nav2_util => nav2_util}/src/map/map_draw.c (100%) rename {src/libs/nav2_util => nav2_util}/src/map/map_range.c (100%) rename {src/libs/nav2_util => nav2_util}/src/map/map_store.c (100%) rename {src/libs/nav2_util => nav2_util}/src/map_loader/map_loader.cpp (100%) rename {src/libs/nav2_util => nav2_util}/src/pf/eig3.c (100%) rename {src/libs/nav2_util => nav2_util}/src/pf/pf.c (100%) rename {src/libs/nav2_util => nav2_util}/src/pf/pf_draw.c (100%) rename {src/libs/nav2_util => nav2_util}/src/pf/pf_kdtree.c (100%) rename {src/libs/nav2_util => nav2_util}/src/pf/pf_pdf.c (100%) rename {src/libs/nav2_util => nav2_util}/src/pf/pf_vector.c (100%) rename {src/libs/nav2_util => nav2_util}/src/sensors/laser.cpp (100%) rename {src/libs/nav2_util => nav2_util}/src/sensors/odom.cpp (100%) rename {src/libs/nav2_util => nav2_util}/src/sensors/sensor.cpp (100%) create mode 100644 nav2_util/test/.gitignore create mode 100644 navigation2/CHANGELOG.rst create mode 100644 navigation2/CMakeLists.txt create mode 100644 navigation2/package.xml delete mode 100644 src/libs/costmap_2d/CHANGELOG.rst delete mode 100644 src/libs/costmap_2d/test/obstacle_tests.launch delete mode 100644 src/libs/costmap_2d/test/simple_driving_test.xml delete mode 100644 src/libs/costmap_2d/test/static_tests.launch delete mode 100644 src/libs/nav2_util/msg/Costmap.msg delete mode 100644 src/libs/nav2_util/msg/CostmapMetaData.msg delete mode 100644 src/mission_execution/nav2_mission_execution_msgs/package.xml delete mode 100644 src/planning/nav2_planning_msgs/CMakeLists.txt delete mode 100644 src/world_model/nav2_world_model_msgs/CMakeLists.txt delete mode 100644 src/world_model/nav2_world_model_msgs/package.xml diff --git a/doc/BUILD.md b/doc/BUILD.md index 5a8a255904..058dad8ba7 100644 --- a/doc/BUILD.md +++ b/doc/BUILD.md @@ -19,8 +19,8 @@ RUN apt-get install -y \ Third, ensure there are no ROS environment variables set in your terminal or `.bashrc` file before taking the steps below.* ```sh -mkdir -cd +mkdir +cd wget https://raw.githubusercontent.com/ros-planning/navigation2/master/tools/initial_ros_setup.sh chmod a+x initial_ros_setup.sh ./initial_ros_setup.sh @@ -38,7 +38,7 @@ The `initial_ros_setup.sh` script downloads four ROS workspaces and then builds * Navigation 2 - This repo. - After all the workspaces are downloaded, the `navigtion2/tools/build_all.sh` script is run which builds each repo in the order listed above using the `colcon build --symlink-install` command (except ROS 1 dependencies which are built using `catkin_make`) + After all the workspaces are downloaded, the `navigation2/tools/build_all.sh` script is run which builds each repo in the order listed above using the `colcon build --symlink-install` command (except ROS 1 dependencies which are built using `catkin_make`) ### Options @@ -58,13 +58,13 @@ Most work will be done in the `navigation2` workspace, so just building that wil To build just `navigation2`, ```sh -cd /navigation2 +cd /navigation2_ws source ../navstack_dependencies_ws/install/setup.sh colcon build --symlink-install ``` In the case that the developer changes any dependencies, they can run -`/navigation2/tools/build_all.sh` in a clean environment to get everything rebuilt easily +`/navigation2_ws/src/navigation2/tools/build_all.sh` in a clean environment to get everything rebuilt easily ### Build System @@ -75,9 +75,9 @@ Instead, it would be better to do an initial download of all the source and depe ./initial_ros_setup.sh --no-ros1 --download-only ``` -Then the CI tool can monitor the `navigation2` repo, update it as necessary, and rebuild using either the `/navigation2/tools/build_all.sh` script or by running +Then the CI tool can monitor the `navigation2` repo, update it as necessary, and rebuild using either the `/navigation2_ws/src/navigation2/tools/build_all.sh` script or by running ```sh -cd /navigation2 +cd /navigation2_ws/src/navigation2 source ../navstack_dependencies_ws/install/setup.sh colcon build --symlink-install ``` diff --git a/src/control/nav2_controller_example/doc/.gitignore b/nav2_amcl/.gitignore similarity index 100% rename from src/control/nav2_controller_example/doc/.gitignore rename to nav2_amcl/.gitignore diff --git a/src/control/nav2_controller_example/CHANGELOG.rst b/nav2_amcl/CHANGELOG.rst similarity index 100% rename from src/control/nav2_controller_example/CHANGELOG.rst rename to nav2_amcl/CHANGELOG.rst diff --git a/src/localization/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt similarity index 100% rename from src/localization/nav2_amcl/CMakeLists.txt rename to nav2_amcl/CMakeLists.txt diff --git a/src/control/nav2_controller_example/README.md b/nav2_amcl/README.md similarity index 100% rename from src/control/nav2_controller_example/README.md rename to nav2_amcl/README.md diff --git a/src/localization/nav2_amcl/package.xml b/nav2_amcl/package.xml similarity index 100% rename from src/localization/nav2_amcl/package.xml rename to nav2_amcl/package.xml diff --git a/src/localization/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp similarity index 100% rename from src/localization/nav2_amcl/src/amcl_node.cpp rename to nav2_amcl/src/amcl_node.cpp diff --git a/src/localization/nav2_amcl/src/amcl_node.hpp b/nav2_amcl/src/amcl_node.hpp similarity index 100% rename from src/localization/nav2_amcl/src/amcl_node.hpp rename to nav2_amcl/src/amcl_node.hpp diff --git a/src/localization/nav2_amcl/src/main.cpp b/nav2_amcl/src/main.cpp similarity index 100% rename from src/localization/nav2_amcl/src/main.cpp rename to nav2_amcl/src/main.cpp diff --git a/src/localization/nav2_amcl/test/CMakeLists.txt b/nav2_amcl/test/CMakeLists.txt similarity index 100% rename from src/localization/nav2_amcl/test/CMakeLists.txt rename to nav2_amcl/test/CMakeLists.txt diff --git a/src/localization/nav2_amcl/test/integration/CMakeLists.txt b/nav2_amcl/test/integration/CMakeLists.txt similarity index 100% rename from src/localization/nav2_amcl/test/integration/CMakeLists.txt rename to nav2_amcl/test/integration/CMakeLists.txt diff --git a/src/localization/nav2_amcl/test/integration/README.md b/nav2_amcl/test/integration/README.md similarity index 100% rename from src/localization/nav2_amcl/test/integration/README.md rename to nav2_amcl/test/integration/README.md diff --git a/src/localization/nav2_amcl/test/integration/test_localization_launch.py b/nav2_amcl/test/integration/test_localization_launch.py similarity index 100% rename from src/localization/nav2_amcl/test/integration/test_localization_launch.py rename to nav2_amcl/test/integration/test_localization_launch.py diff --git a/src/localization/nav2_amcl/test/integration/test_localization_node.cpp b/nav2_amcl/test/integration/test_localization_node.cpp similarity index 100% rename from src/localization/nav2_amcl/test/integration/test_localization_node.cpp rename to nav2_amcl/test/integration/test_localization_node.cpp diff --git a/launch/test_map.pgm b/nav2_amcl/test/maps/test_map.pgm similarity index 100% rename from launch/test_map.pgm rename to nav2_amcl/test/maps/test_map.pgm diff --git a/src/localization/nav2_amcl/test/maps/test_map.yaml b/nav2_amcl/test/maps/test_map.yaml similarity index 100% rename from src/localization/nav2_amcl/test/maps/test_map.yaml rename to nav2_amcl/test/maps/test_map.yaml diff --git a/src/localization/nav2_amcl/test/test_launch_files/localization_node.launch.py b/nav2_amcl/test/test_launch_files/localization_node.launch.py similarity index 100% rename from src/localization/nav2_amcl/test/test_launch_files/localization_node.launch.py rename to nav2_amcl/test/test_launch_files/localization_node.launch.py diff --git a/src/localization/nav2_amcl/test/unit/CMakeLists.txt b/nav2_amcl/test/unit/CMakeLists.txt similarity index 100% rename from src/localization/nav2_amcl/test/unit/CMakeLists.txt rename to nav2_amcl/test/unit/CMakeLists.txt diff --git a/src/localization/nav2_amcl/test/unit/test_localization_amcl.cpp b/nav2_amcl/test/unit/test_localization_amcl.cpp similarity index 100% rename from src/localization/nav2_amcl/test/unit/test_localization_amcl.cpp rename to nav2_amcl/test/unit/test_localization_amcl.cpp diff --git a/src/libs/nav2_libs_msgs/CHANGELOG.rst b/nav2_astar_planner/CHANGELOG.rst similarity index 100% rename from src/libs/nav2_libs_msgs/CHANGELOG.rst rename to nav2_astar_planner/CHANGELOG.rst diff --git a/src/planning/nav2_astar_planner/CMakeLists.txt b/nav2_astar_planner/CMakeLists.txt similarity index 95% rename from src/planning/nav2_astar_planner/CMakeLists.txt rename to nav2_astar_planner/CMakeLists.txt index d1d3cbac35..dea2710a68 100644 --- a/src/planning/nav2_astar_planner/CMakeLists.txt +++ b/nav2_astar_planner/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_tasks REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav2_planning_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) include_directories( include @@ -39,7 +39,7 @@ set(dependencies nav2_tasks task nav2_util - nav2_planning_msgs + nav2_msgs ) ament_target_dependencies(${executable_name} diff --git a/src/libs/nav2_libs_msgs/README.md b/nav2_astar_planner/README.md similarity index 100% rename from src/libs/nav2_libs_msgs/README.md rename to nav2_astar_planner/README.md diff --git a/src/planning/nav2_astar_planner/doc/..gitignore b/nav2_astar_planner/doc/..gitignore similarity index 100% rename from src/planning/nav2_astar_planner/doc/..gitignore rename to nav2_astar_planner/doc/..gitignore diff --git a/src/planning/nav2_astar_planner/include/nav2_astar_planner/astar_planner.hpp b/nav2_astar_planner/include/nav2_astar_planner/astar_planner.hpp similarity index 100% rename from src/planning/nav2_astar_planner/include/nav2_astar_planner/astar_planner.hpp rename to nav2_astar_planner/include/nav2_astar_planner/astar_planner.hpp diff --git a/src/planning/nav2_astar_planner/package.xml b/nav2_astar_planner/package.xml similarity index 89% rename from src/planning/nav2_astar_planner/package.xml rename to nav2_astar_planner/package.xml index 410d063d11..0726b523a2 100644 --- a/src/planning/nav2_astar_planner/package.xml +++ b/nav2_astar_planner/package.xml @@ -12,11 +12,11 @@ rclcpp nav2_tasks nav2_util - nav2_planning_msgs + nav2_msgs rclcpp nav2_tasks - nav2_planning_msgs + nav2_msgs ament_lint_common ament_lint_auto diff --git a/src/planning/nav2_astar_planner/src/astar_planner.cpp b/nav2_astar_planner/src/astar_planner.cpp similarity index 100% rename from src/planning/nav2_astar_planner/src/astar_planner.cpp rename to nav2_astar_planner/src/astar_planner.cpp diff --git a/src/planning/nav2_astar_planner/src/main.cpp b/nav2_astar_planner/src/main.cpp similarity index 100% rename from src/planning/nav2_astar_planner/src/main.cpp rename to nav2_astar_planner/src/main.cpp diff --git a/src/control/nav2_controller_example/test/.gitignore b/nav2_astar_planner/test/.gitignore similarity index 100% rename from src/control/nav2_controller_example/test/.gitignore rename to nav2_astar_planner/test/.gitignore diff --git a/src/libs/nav2_robot/CHANGELOG.rst b/nav2_bringup/CHANGELOG.rst similarity index 100% rename from src/libs/nav2_robot/CHANGELOG.rst rename to nav2_bringup/CHANGELOG.rst diff --git a/src/mission_execution/nav2_mission_execution_msgs/CMakeLists.txt b/nav2_bringup/CMakeLists.txt similarity index 66% rename from src/mission_execution/nav2_mission_execution_msgs/CMakeLists.txt rename to nav2_bringup/CMakeLists.txt index 05067d8ad1..0ea5bc69bc 100644 --- a/src/mission_execution/nav2_mission_execution_msgs/CMakeLists.txt +++ b/nav2_bringup/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_mission_execution_msgs) +project(nav2_bringup) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -12,15 +12,11 @@ endif() find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/MissionPlan.msg" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs -) +find_package(navigation2 REQUIRED) ament_export_dependencies(rosidl_default_runtime) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + ament_package() diff --git a/src/libs/nav2_robot/README.md b/nav2_bringup/README.md similarity index 100% rename from src/libs/nav2_robot/README.md rename to nav2_bringup/README.md diff --git a/launch/core.launch.py b/nav2_bringup/launch/core.launch.py similarity index 100% rename from launch/core.launch.py rename to nav2_bringup/launch/core.launch.py diff --git a/launch/system_test.rviz b/nav2_bringup/launch/system_test.rviz similarity index 100% rename from launch/system_test.rviz rename to nav2_bringup/launch/system_test.rviz diff --git a/src/localization/nav2_amcl/test/maps/test_map.pgm b/nav2_bringup/launch/test_map.pgm similarity index 100% rename from src/localization/nav2_amcl/test/maps/test_map.pgm rename to nav2_bringup/launch/test_map.pgm diff --git a/launch/test_map.yaml b/nav2_bringup/launch/test_map.yaml similarity index 100% rename from launch/test_map.yaml rename to nav2_bringup/launch/test_map.yaml diff --git a/src/libs/nav2_libs_msgs/package.xml b/nav2_bringup/package.xml similarity index 73% rename from src/libs/nav2_libs_msgs/package.xml rename to nav2_bringup/package.xml index 67b9e46739..be9e566ed9 100644 --- a/src/libs/nav2_libs_msgs/package.xml +++ b/nav2_bringup/package.xml @@ -1,25 +1,24 @@ - nav2_libs_msgs + nav2_bringup 0.1.0 - TODO + Bringup scripts and configurations for the navigation2 stack Michael Jeronimo + Steve Macenski Apache License 2.0 ament_cmake rclcpp - std_msgs + navigation2 builtin_interfaces rosidl_default_generators - geometry_msgs rclcpp - std_msgs builtin_interfaces rosidl_default_runtime - geometry_msgs + navigation2 rosidl_interface_packages diff --git a/src/libs/nav2_tasks/CHANGELOG.rst b/nav2_bt_navigator/CHANGELOG.rst similarity index 100% rename from src/libs/nav2_tasks/CHANGELOG.rst rename to nav2_bt_navigator/CHANGELOG.rst diff --git a/src/navigation/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt similarity index 95% rename from src/navigation/nav2_bt_navigator/CMakeLists.txt rename to nav2_bt_navigator/CMakeLists.txt index 6e4eea188a..88a0023365 100644 --- a/src/navigation/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_tasks REQUIRED) -find_package(nav2_planning_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) include_directories( include @@ -36,8 +36,8 @@ set(dependencies rclcpp std_msgs nav2_tasks - nav2_planning_msgs -) + nav2_msgs +) ament_target_dependencies(${executable_name} ${dependencies} diff --git a/src/libs/nav2_tasks/README.md b/nav2_bt_navigator/README.md similarity index 100% rename from src/libs/nav2_tasks/README.md rename to nav2_bt_navigator/README.md diff --git a/src/libs/nav2_util/CHANGELOG.rst b/nav2_bt_navigator/doc/.gitignore similarity index 100% rename from src/libs/nav2_util/CHANGELOG.rst rename to nav2_bt_navigator/doc/.gitignore diff --git a/src/navigation/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp similarity index 97% rename from src/navigation/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp rename to nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 2d9733f820..b2beeb1240 100644 --- a/src/navigation/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -17,6 +17,7 @@ #include #include +#include "nav2_tasks/task_status.hpp" #include "nav2_tasks/navigate_to_pose_task.hpp" #include "nav2_tasks/compute_path_to_pose_task.hpp" #include "nav2_tasks/follow_path_task.hpp" diff --git a/src/navigation/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml similarity index 88% rename from src/navigation/nav2_bt_navigator/package.xml rename to nav2_bt_navigator/package.xml index 36a344466c..c3a2b7d2d4 100644 --- a/src/navigation/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -11,11 +11,11 @@ rclcpp nav2_tasks - nav2_planning_msgs + nav2_msgs rclcpp nav2_tasks - nav2_planning_msgs + nav2_msgs ament_lint_common ament_lint_auto diff --git a/src/navigation/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp similarity index 100% rename from src/navigation/nav2_bt_navigator/src/bt_navigator.cpp rename to nav2_bt_navigator/src/bt_navigator.cpp diff --git a/src/navigation/nav2_bt_navigator/src/main.cpp b/nav2_bt_navigator/src/main.cpp similarity index 100% rename from src/navigation/nav2_bt_navigator/src/main.cpp rename to nav2_bt_navigator/src/main.cpp diff --git a/src/libs/nav2_util/README.md b/nav2_bt_navigator/test/.gitignore similarity index 100% rename from src/libs/nav2_util/README.md rename to nav2_bt_navigator/test/.gitignore diff --git a/src/control/costmap_queue/CMakeLists.txt b/nav2_controller/costmap_queue/CMakeLists.txt similarity index 95% rename from src/control/costmap_queue/CMakeLists.txt rename to nav2_controller/costmap_queue/CMakeLists.txt index 9dab06342e..c92d6ed6c8 100644 --- a/src/control/costmap_queue/CMakeLists.txt +++ b/nav2_controller/costmap_queue/CMakeLists.txt @@ -11,7 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(costmap_2d REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(rclcpp REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread) @@ -27,7 +27,7 @@ add_library(${PROJECT_NAME} set(dependencies rclcpp - costmap_2d + nav2_costmap_2d ) ament_target_dependencies(${PROJECT_NAME} diff --git a/src/control/costmap_queue/include/costmap_queue/costmap_queue.h b/nav2_controller/costmap_queue/include/costmap_queue/costmap_queue.h similarity index 97% rename from src/control/costmap_queue/include/costmap_queue/costmap_queue.h rename to nav2_controller/costmap_queue/include/costmap_queue/costmap_queue.h index 5f0ddc05b5..33c6449f36 100644 --- a/src/control/costmap_queue/include/costmap_queue/costmap_queue.h +++ b/nav2_controller/costmap_queue/include/costmap_queue/costmap_queue.h @@ -39,7 +39,7 @@ #include #include #include -#include "costmap_2d/costmap_2d.h" +#include "nav2_costmap_2d/costmap_2d.h" #include "costmap_queue/map_based_queue.h" namespace costmap_queue @@ -113,7 +113,7 @@ class CostmapQueue : public MapBasedQueue * @param costmap Costmap which defines the size/number of cells * @param manhattan If true, sort cells by Manhattan distance, otherwise use Euclidean distance */ - explicit CostmapQueue(costmap_2d::Costmap2D & costmap, bool manhattan = false); + explicit CostmapQueue(nav2_costmap_2d::Costmap2D & costmap, bool manhattan = false); /** * @brief Clear the queue @@ -162,7 +162,7 @@ class CostmapQueue : public MapBasedQueue */ void computeCache(); - costmap_2d::Costmap2D & costmap_; + nav2_costmap_2d::Costmap2D & costmap_; std::vector seen_; int max_distance_; bool manhattan_; diff --git a/src/control/costmap_queue/include/costmap_queue/limited_costmap_queue.h b/nav2_controller/costmap_queue/include/costmap_queue/limited_costmap_queue.h similarity index 96% rename from src/control/costmap_queue/include/costmap_queue/limited_costmap_queue.h rename to nav2_controller/costmap_queue/include/costmap_queue/limited_costmap_queue.h index fee03da8bf..0a1fa4d995 100644 --- a/src/control/costmap_queue/include/costmap_queue/limited_costmap_queue.h +++ b/nav2_controller/costmap_queue/include/costmap_queue/limited_costmap_queue.h @@ -50,7 +50,7 @@ class LimitedCostmapQueue : public CostmapQueue /** * @brief Constructor with limit as an integer number of cells. */ - LimitedCostmapQueue(costmap_2d::Costmap2D & costmap, const int cell_distance_limit); + LimitedCostmapQueue(nav2_costmap_2d::Costmap2D & costmap, const int cell_distance_limit); bool validCellToQueue(const CellData & cell) override; }; } // namespace costmap_queue diff --git a/src/control/costmap_queue/include/costmap_queue/map_based_queue.h b/nav2_controller/costmap_queue/include/costmap_queue/map_based_queue.h similarity index 100% rename from src/control/costmap_queue/include/costmap_queue/map_based_queue.h rename to nav2_controller/costmap_queue/include/costmap_queue/map_based_queue.h diff --git a/src/control/costmap_queue/package.xml b/nav2_controller/costmap_queue/package.xml similarity index 95% rename from src/control/costmap_queue/package.xml rename to nav2_controller/costmap_queue/package.xml index 277ea17ae6..f5720fb11b 100644 --- a/src/control/costmap_queue/package.xml +++ b/nav2_controller/costmap_queue/package.xml @@ -9,7 +9,7 @@ BSD ament_cmake - costmap_2d + nav2_costmap_2d rclcpp ament_cmake_cppcheck diff --git a/src/control/costmap_queue/src/costmap_queue.cpp b/nav2_controller/costmap_queue/src/costmap_queue.cpp similarity index 98% rename from src/control/costmap_queue/src/costmap_queue.cpp rename to nav2_controller/costmap_queue/src/costmap_queue.cpp index f1e2f2056e..b4bdc90d8e 100644 --- a/src/control/costmap_queue/src/costmap_queue.cpp +++ b/nav2_controller/costmap_queue/src/costmap_queue.cpp @@ -41,7 +41,7 @@ using std::hypot; namespace costmap_queue { -CostmapQueue::CostmapQueue(costmap_2d::Costmap2D & costmap, bool manhattan) +CostmapQueue::CostmapQueue(nav2_costmap_2d::Costmap2D & costmap, bool manhattan) : MapBasedQueue(), costmap_(costmap), max_distance_(-1), manhattan_(manhattan), cached_max_distance_(-1) { diff --git a/src/control/costmap_queue/src/limited_costmap_queue.cpp b/nav2_controller/costmap_queue/src/limited_costmap_queue.cpp similarity index 94% rename from src/control/costmap_queue/src/limited_costmap_queue.cpp rename to nav2_controller/costmap_queue/src/limited_costmap_queue.cpp index 6321f03a2b..982a92c580 100644 --- a/src/control/costmap_queue/src/limited_costmap_queue.cpp +++ b/nav2_controller/costmap_queue/src/limited_costmap_queue.cpp @@ -37,7 +37,7 @@ namespace costmap_queue { -LimitedCostmapQueue::LimitedCostmapQueue(costmap_2d::Costmap2D & costmap, const int distance_limit) +LimitedCostmapQueue::LimitedCostmapQueue(nav2_costmap_2d::Costmap2D & costmap, const int distance_limit) : CostmapQueue(costmap) { max_distance_ = distance_limit; diff --git a/src/control/costmap_queue/test/mbq_test.cpp b/nav2_controller/costmap_queue/test/mbq_test.cpp similarity index 100% rename from src/control/costmap_queue/test/mbq_test.cpp rename to nav2_controller/costmap_queue/test/mbq_test.cpp diff --git a/src/control/costmap_queue/test/utest.cpp b/nav2_controller/costmap_queue/test/utest.cpp similarity index 97% rename from src/control/costmap_queue/test/utest.cpp rename to nav2_controller/costmap_queue/test/utest.cpp index f9cbf11e76..615868b116 100644 --- a/src/control/costmap_queue/test/utest.cpp +++ b/nav2_controller/costmap_queue/test/utest.cpp @@ -42,7 +42,7 @@ using std::hypot; -costmap_2d::Costmap2D costmap(5, 5, 1.0, 0.0, 0.0); +nav2_costmap_2d::Costmap2D costmap(5, 5, 1.0, 0.0, 0.0); TEST(CostmapQueue, basicQueue) { @@ -59,7 +59,7 @@ TEST(CostmapQueue, basicQueue) TEST(CostmapQueue, bigTest) { - costmap_2d::Costmap2D big_map(500, 500, 1.0, 0.0, 0.0); + nav2_costmap_2d::Costmap2D big_map(500, 500, 1.0, 0.0, 0.0); costmap_queue::CostmapQueue q(big_map); int count = 0; q.enqueueCell(0, 0); diff --git a/src/control/dwb_core/CMakeLists.txt b/nav2_controller/dwb_core/CMakeLists.txt similarity index 96% rename from src/control/dwb_core/CMakeLists.txt rename to nav2_controller/dwb_core/CMakeLists.txt index 71f55ffd83..d3348a165d 100644 --- a/src/control/dwb_core/CMakeLists.txt +++ b/nav2_controller/dwb_core/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_2d_msgs REQUIRED) find_package(dwb_msgs REQUIRED) -find_package(costmap_2d REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(pluginlib REQUIRED) find_package(sensor_msgs REQUIRED) find_package(visualization_msgs REQUIRED) @@ -32,7 +32,7 @@ set(dependencies geometry_msgs nav_2d_msgs dwb_msgs - costmap_2d + nav2_costmap_2d pluginlib sensor_msgs visualization_msgs diff --git a/src/control/dwb_core/include/dwb_core/common_types.h b/nav2_controller/dwb_core/include/dwb_core/common_types.h similarity index 88% rename from src/control/dwb_core/include/dwb_core/common_types.h rename to nav2_controller/dwb_core/include/dwb_core/common_types.h index 189d1eaf5c..0809988faf 100644 --- a/src/control/dwb_core/include/dwb_core/common_types.h +++ b/nav2_controller/dwb_core/include/dwb_core/common_types.h @@ -16,7 +16,7 @@ #define DWB_CORE__COMMON_TYPES_H_ #include -#include "costmap_2d/costmap_2d_ros.h" +#include "nav2_costmap_2d/costmap_2d_ros.h" #include "tf2_ros/transform_listener.h" @@ -24,7 +24,7 @@ namespace dwb_core { typedef std::shared_ptr TFBufferPtr; -typedef std::shared_ptr CostmapROSPtr; +typedef std::shared_ptr CostmapROSPtr; } diff --git a/src/control/dwb_core/include/dwb_core/dwb_core.h b/nav2_controller/dwb_core/include/dwb_core/dwb_core.h similarity index 100% rename from src/control/dwb_core/include/dwb_core/dwb_core.h rename to nav2_controller/dwb_core/include/dwb_core/dwb_core.h diff --git a/src/control/dwb_core/include/dwb_core/exceptions.h b/nav2_controller/dwb_core/include/dwb_core/exceptions.h similarity index 100% rename from src/control/dwb_core/include/dwb_core/exceptions.h rename to nav2_controller/dwb_core/include/dwb_core/exceptions.h diff --git a/src/control/dwb_core/include/dwb_core/goal_checker.h b/nav2_controller/dwb_core/include/dwb_core/goal_checker.h similarity index 100% rename from src/control/dwb_core/include/dwb_core/goal_checker.h rename to nav2_controller/dwb_core/include/dwb_core/goal_checker.h diff --git a/src/control/dwb_core/include/dwb_core/illegal_trajectory_tracker.h b/nav2_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.h similarity index 100% rename from src/control/dwb_core/include/dwb_core/illegal_trajectory_tracker.h rename to nav2_controller/dwb_core/include/dwb_core/illegal_trajectory_tracker.h diff --git a/src/control/dwb_core/include/dwb_core/publisher.h b/nav2_controller/dwb_core/include/dwb_core/publisher.h similarity index 100% rename from src/control/dwb_core/include/dwb_core/publisher.h rename to nav2_controller/dwb_core/include/dwb_core/publisher.h diff --git a/src/control/dwb_core/include/dwb_core/trajectory_critic.h b/nav2_controller/dwb_core/include/dwb_core/trajectory_critic.h similarity index 100% rename from src/control/dwb_core/include/dwb_core/trajectory_critic.h rename to nav2_controller/dwb_core/include/dwb_core/trajectory_critic.h diff --git a/src/control/dwb_core/include/dwb_core/trajectory_generator.h b/nav2_controller/dwb_core/include/dwb_core/trajectory_generator.h similarity index 100% rename from src/control/dwb_core/include/dwb_core/trajectory_generator.h rename to nav2_controller/dwb_core/include/dwb_core/trajectory_generator.h diff --git a/src/control/dwb_core/package.xml b/nav2_controller/dwb_core/package.xml similarity index 94% rename from src/control/dwb_core/package.xml rename to nav2_controller/dwb_core/package.xml index f980c3134d..4fed5111f6 100644 --- a/src/control/dwb_core/package.xml +++ b/nav2_controller/dwb_core/package.xml @@ -15,7 +15,7 @@ geometry_msgs nav_2d_msgs dwb_msgs - costmap_2d + nav2_costmap_2d pluginlib sensor_msgs visualization_msgs @@ -27,7 +27,7 @@ std_msgs geometry_msgs dwb_msgs - costmap_2d + nav2_costmap_2d nav_2d_utils pluginlib diff --git a/src/control/dwb_core/src/dwb_core.cpp b/nav2_controller/dwb_core/src/dwb_core.cpp similarity index 99% rename from src/control/dwb_core/src/dwb_core.cpp rename to nav2_controller/dwb_core/src/dwb_core.cpp index a6e864e0d3..f6d56e4372 100644 --- a/src/control/dwb_core/src/dwb_core.cpp +++ b/nav2_controller/dwb_core/src/dwb_core.cpp @@ -393,7 +393,7 @@ nav_2d_msgs::msg::Path2D DWBLocalPlanner::transformGlobalPlan( transformed_plan.header.stamp = pose.header.stamp; // we'll discard points on the plan that are outside the local costmap - costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * costmap->getResolution() / 2.0; double sq_dist_threshold = dist_threshold * dist_threshold; diff --git a/src/control/dwb_core/src/illegal_trajectory_tracker.cpp b/nav2_controller/dwb_core/src/illegal_trajectory_tracker.cpp similarity index 100% rename from src/control/dwb_core/src/illegal_trajectory_tracker.cpp rename to nav2_controller/dwb_core/src/illegal_trajectory_tracker.cpp diff --git a/src/control/dwb_core/src/publisher.cpp b/nav2_controller/dwb_core/src/publisher.cpp similarity index 99% rename from src/control/dwb_core/src/publisher.cpp rename to nav2_controller/dwb_core/src/publisher.cpp index 569b8f6b31..f98886edbe 100644 --- a/src/control/dwb_core/src/publisher.cpp +++ b/nav2_controller/dwb_core/src/publisher.cpp @@ -159,7 +159,7 @@ void DWBPublisher::publishCostGrid( cost_grid_pc.header.frame_id = costmap_ros->getGlobalFrameID(); cost_grid_pc.header.stamp = rclcpp::Clock().now(); - costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap(); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap(); double x_coord, y_coord; unsigned int size_x = costmap->getSizeInCellsX(); unsigned int size_y = costmap->getSizeInCellsY(); diff --git a/src/control/dwb_critics/CMakeLists.txt b/nav2_controller/dwb_critics/CMakeLists.txt similarity index 96% rename from src/control/dwb_critics/CMakeLists.txt rename to nav2_controller/dwb_critics/CMakeLists.txt index 36af2fc1fa..4e246963e7 100644 --- a/src/control/dwb_critics/CMakeLists.txt +++ b/nav2_controller/dwb_critics/CMakeLists.txt @@ -12,7 +12,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) -find_package(costmap_2d REQUIRED) +find_package(nav2_costmap_2d REQUIRED) find_package(costmap_queue REQUIRED) find_package(dwb_core REQUIRED) find_package(geometry_msgs REQUIRED) @@ -43,7 +43,7 @@ add_library(${PROJECT_NAME} set(dependencies angles - costmap_2d + nav2_costmap_2d costmap_queue dwb_core geometry_msgs diff --git a/src/control/dwb_critics/default_critics.xml b/nav2_controller/dwb_critics/default_critics.xml similarity index 100% rename from src/control/dwb_critics/default_critics.xml rename to nav2_controller/dwb_critics/default_critics.xml diff --git a/src/control/dwb_critics/include/dwb_critics/alignment_util.h b/nav2_controller/dwb_critics/include/dwb_critics/alignment_util.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/alignment_util.h rename to nav2_controller/dwb_critics/include/dwb_critics/alignment_util.h diff --git a/src/control/dwb_critics/include/dwb_critics/base_obstacle.h b/nav2_controller/dwb_critics/include/dwb_critics/base_obstacle.h similarity index 98% rename from src/control/dwb_critics/include/dwb_critics/base_obstacle.h rename to nav2_controller/dwb_critics/include/dwb_critics/base_obstacle.h index 7cbd709492..1b325bc826 100644 --- a/src/control/dwb_critics/include/dwb_critics/base_obstacle.h +++ b/nav2_controller/dwb_critics/include/dwb_critics/base_obstacle.h @@ -71,7 +71,7 @@ class BaseObstacleCritic : public dwb_core::TrajectoryCritic virtual bool isValidCost(const unsigned char cost); protected: - costmap_2d::Costmap2D * costmap_; + nav2_costmap_2d::Costmap2D * costmap_; bool sum_scores_; }; } // namespace dwb_critics diff --git a/src/control/dwb_critics/include/dwb_critics/goal_align.h b/nav2_controller/dwb_critics/include/dwb_critics/goal_align.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/goal_align.h rename to nav2_controller/dwb_critics/include/dwb_critics/goal_align.h diff --git a/src/control/dwb_critics/include/dwb_critics/goal_dist.h b/nav2_controller/dwb_critics/include/dwb_critics/goal_dist.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/goal_dist.h rename to nav2_controller/dwb_critics/include/dwb_critics/goal_dist.h diff --git a/src/control/dwb_critics/include/dwb_critics/line_iterator.h b/nav2_controller/dwb_critics/include/dwb_critics/line_iterator.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/line_iterator.h rename to nav2_controller/dwb_critics/include/dwb_critics/line_iterator.h diff --git a/src/control/dwb_critics/include/dwb_critics/map_grid.h b/nav2_controller/dwb_critics/include/dwb_critics/map_grid.h similarity index 97% rename from src/control/dwb_critics/include/dwb_critics/map_grid.h rename to nav2_controller/dwb_critics/include/dwb_critics/map_grid.h index c512c1036b..541812d60a 100644 --- a/src/control/dwb_critics/include/dwb_critics/map_grid.h +++ b/nav2_controller/dwb_critics/include/dwb_critics/map_grid.h @@ -107,7 +107,7 @@ class MapGridCritic : public dwb_core::TrajectoryCritic class MapGridQueue : public costmap_queue::CostmapQueue { public: - MapGridQueue(costmap_2d::Costmap2D & costmap, MapGridCritic & parent) + MapGridQueue(nav2_costmap_2d::Costmap2D & costmap, MapGridCritic & parent) : costmap_queue::CostmapQueue(costmap, true), parent_(parent) {} bool validCellToQueue(const costmap_queue::CellData & cell) override; @@ -126,7 +126,7 @@ class MapGridCritic : public dwb_core::TrajectoryCritic void propogateManhattanDistances(); std::shared_ptr queue_; - costmap_2d::Costmap2D * costmap_; + nav2_costmap_2d::Costmap2D * costmap_; std::vector cell_values_; double obstacle_score_, unreachable_score_; ///< Special cell_values bool stop_on_failure_; diff --git a/src/control/dwb_critics/include/dwb_critics/obstacle_footprint.h b/nav2_controller/dwb_critics/include/dwb_critics/obstacle_footprint.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/obstacle_footprint.h rename to nav2_controller/dwb_critics/include/dwb_critics/obstacle_footprint.h diff --git a/src/control/dwb_critics/include/dwb_critics/oscillation.h b/nav2_controller/dwb_critics/include/dwb_critics/oscillation.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/oscillation.h rename to nav2_controller/dwb_critics/include/dwb_critics/oscillation.h diff --git a/src/control/dwb_critics/include/dwb_critics/path_align.h b/nav2_controller/dwb_critics/include/dwb_critics/path_align.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/path_align.h rename to nav2_controller/dwb_critics/include/dwb_critics/path_align.h diff --git a/src/control/dwb_critics/include/dwb_critics/path_dist.h b/nav2_controller/dwb_critics/include/dwb_critics/path_dist.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/path_dist.h rename to nav2_controller/dwb_critics/include/dwb_critics/path_dist.h diff --git a/src/control/dwb_critics/include/dwb_critics/prefer_forward.h b/nav2_controller/dwb_critics/include/dwb_critics/prefer_forward.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/prefer_forward.h rename to nav2_controller/dwb_critics/include/dwb_critics/prefer_forward.h diff --git a/src/control/dwb_critics/include/dwb_critics/rotate_to_goal.h b/nav2_controller/dwb_critics/include/dwb_critics/rotate_to_goal.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/rotate_to_goal.h rename to nav2_controller/dwb_critics/include/dwb_critics/rotate_to_goal.h diff --git a/src/control/dwb_critics/include/dwb_critics/twirling.h b/nav2_controller/dwb_critics/include/dwb_critics/twirling.h similarity index 100% rename from src/control/dwb_critics/include/dwb_critics/twirling.h rename to nav2_controller/dwb_critics/include/dwb_critics/twirling.h diff --git a/src/control/dwb_critics/package.xml b/nav2_controller/dwb_critics/package.xml similarity index 96% rename from src/control/dwb_critics/package.xml rename to nav2_controller/dwb_critics/package.xml index 9bb4e289ad..2e7a34d17b 100644 --- a/src/control/dwb_critics/package.xml +++ b/nav2_controller/dwb_critics/package.xml @@ -11,7 +11,7 @@ ament_cmake angles - costmap_2d + nav2_costmap_2d costmap_queue dwb_core geometry_msgs diff --git a/src/control/dwb_critics/src/alignment_util.cpp b/nav2_controller/dwb_critics/src/alignment_util.cpp similarity index 100% rename from src/control/dwb_critics/src/alignment_util.cpp rename to nav2_controller/dwb_critics/src/alignment_util.cpp diff --git a/src/control/dwb_critics/src/base_obstacle.cpp b/nav2_controller/dwb_critics/src/base_obstacle.cpp similarity index 94% rename from src/control/dwb_critics/src/base_obstacle.cpp rename to nav2_controller/dwb_critics/src/base_obstacle.cpp index 2fc6b3b1c4..eaf43c533a 100644 --- a/src/control/dwb_critics/src/base_obstacle.cpp +++ b/nav2_controller/dwb_critics/src/base_obstacle.cpp @@ -35,7 +35,7 @@ #include "dwb_critics/base_obstacle.h" #include "dwb_core/exceptions.h" #include "pluginlib/class_list_macros.hpp" -#include "costmap_2d/cost_values.h" +#include "nav2_costmap_2d/cost_values.h" PLUGINLIB_EXPORT_CLASS(dwb_critics::BaseObstacleCritic, dwb_core::TrajectoryCritic) @@ -75,8 +75,8 @@ double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose2D & pose) bool BaseObstacleCritic::isValidCost(const unsigned char cost) { - return cost != costmap_2d::LETHAL_OBSTACLE && cost != costmap_2d::INSCRIBED_INFLATED_OBSTACLE && - cost != costmap_2d::NO_INFORMATION; + return cost != nav2_costmap_2d::LETHAL_OBSTACLE && cost != nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && + cost != nav2_costmap_2d::NO_INFORMATION; } void BaseObstacleCritic::addGridScores(sensor_msgs::msg::PointCloud & pc) diff --git a/src/control/dwb_critics/src/goal_align.cpp b/nav2_controller/dwb_critics/src/goal_align.cpp similarity index 100% rename from src/control/dwb_critics/src/goal_align.cpp rename to nav2_controller/dwb_critics/src/goal_align.cpp diff --git a/src/control/dwb_critics/src/goal_dist.cpp b/nav2_controller/dwb_critics/src/goal_dist.cpp similarity index 96% rename from src/control/dwb_critics/src/goal_dist.cpp rename to nav2_controller/dwb_critics/src/goal_dist.cpp index 38ae97724e..857177d748 100644 --- a/src/control/dwb_critics/src/goal_dist.cpp +++ b/nav2_controller/dwb_critics/src/goal_dist.cpp @@ -36,7 +36,7 @@ #include #include "pluginlib/class_list_macros.hpp" #include "nav_2d_utils/path_ops.h" -#include "costmap_2d/cost_values.h" +#include "nav2_costmap_2d/cost_values.h" namespace dwb_critics { @@ -76,7 +76,7 @@ bool GoalDistCritic::getLastPoseOnCostmap( double g_y = adjusted_global_plan.poses[i].y; unsigned int map_x, map_y; if (costmap_->worldToMap(g_x, g_y, map_x, - map_y) && costmap_->getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) + map_y) && costmap_->getCost(map_x, map_y) != nav2_costmap_2d::NO_INFORMATION) { // Still on the costmap. Continue. x = map_x; diff --git a/src/control/dwb_critics/src/map_grid.cpp b/nav2_controller/dwb_critics/src/map_grid.cpp similarity index 95% rename from src/control/dwb_critics/src/map_grid.cpp rename to nav2_controller/dwb_critics/src/map_grid.cpp index 3509598f50..83720908c1 100644 --- a/src/control/dwb_critics/src/map_grid.cpp +++ b/nav2_controller/dwb_critics/src/map_grid.cpp @@ -38,7 +38,7 @@ #include #include #include "dwb_core/exceptions.h" -#include "costmap_2d/cost_values.h" +#include "nav2_costmap_2d/cost_values.h" using std::abs; using costmap_queue::CellData; @@ -50,8 +50,8 @@ namespace dwb_critics bool MapGridCritic::MapGridQueue::validCellToQueue(const costmap_queue::CellData & cell) { unsigned char cost = costmap_.getCost(cell.x_, cell.y_); - if (cost == costmap_2d::LETHAL_OBSTACLE || cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || - cost == costmap_2d::NO_INFORMATION) + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || + cost == nav2_costmap_2d::NO_INFORMATION) { parent_.setAsObstacle(cell.index_); return false; @@ -162,7 +162,7 @@ void MapGridCritic::addGridScores(sensor_msgs::msg::PointCloud & pc) sensor_msgs::msg::ChannelFloat32 grid_scores; grid_scores.name = name_; - costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); unsigned int size_x = costmap->getSizeInCellsX(); unsigned int size_y = costmap->getSizeInCellsY(); grid_scores.values.resize(size_x * size_y); diff --git a/src/control/dwb_critics/src/obstacle_footprint.cpp b/nav2_controller/dwb_critics/src/obstacle_footprint.cpp similarity index 97% rename from src/control/dwb_critics/src/obstacle_footprint.cpp rename to nav2_controller/dwb_critics/src/obstacle_footprint.cpp index 03c46e2e49..3e0ecc597a 100644 --- a/src/control/dwb_critics/src/obstacle_footprint.cpp +++ b/nav2_controller/dwb_critics/src/obstacle_footprint.cpp @@ -38,7 +38,7 @@ #include "dwb_critics/line_iterator.h" #include "dwb_core/exceptions.h" #include "pluginlib/class_list_macros.hpp" -#include "costmap_2d/cost_values.h" +#include "nav2_costmap_2d/cost_values.h" PLUGINLIB_EXPORT_CLASS(dwb_critics::ObstacleFootprintCritic, dwb_core::TrajectoryCritic) @@ -145,9 +145,9 @@ double ObstacleFootprintCritic::pointCost(int x, int y) { unsigned char cost = costmap_->getCost(x, y); // if the cell is in an obstacle the path is invalid or unknown - if (cost == costmap_2d::LETHAL_OBSTACLE) { + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) { throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Obstacle."); - } else if (cost == costmap_2d::NO_INFORMATION) { + } else if (cost == nav2_costmap_2d::NO_INFORMATION) { throw nav_core2::IllegalTrajectoryException(name_, "Trajectory Hits Unknown Region."); } diff --git a/src/control/dwb_critics/src/oscillation.cpp b/nav2_controller/dwb_critics/src/oscillation.cpp similarity index 100% rename from src/control/dwb_critics/src/oscillation.cpp rename to nav2_controller/dwb_critics/src/oscillation.cpp diff --git a/src/control/dwb_critics/src/path_align.cpp b/nav2_controller/dwb_critics/src/path_align.cpp similarity index 100% rename from src/control/dwb_critics/src/path_align.cpp rename to nav2_controller/dwb_critics/src/path_align.cpp diff --git a/src/control/dwb_critics/src/path_dist.cpp b/nav2_controller/dwb_critics/src/path_dist.cpp similarity index 96% rename from src/control/dwb_critics/src/path_dist.cpp rename to nav2_controller/dwb_critics/src/path_dist.cpp index a013718722..597fdec084 100644 --- a/src/control/dwb_critics/src/path_dist.cpp +++ b/nav2_controller/dwb_critics/src/path_dist.cpp @@ -36,7 +36,7 @@ #include #include "pluginlib/class_list_macros.hpp" #include "nav_2d_utils/path_ops.h" -#include "costmap_2d/cost_values.h" +#include "nav2_costmap_2d/cost_values.h" namespace dwb_critics { @@ -64,7 +64,7 @@ bool PathDistCritic::prepare( double g_y = adjusted_global_plan.poses[i].y; unsigned int map_x, map_y; if (costmap_->worldToMap(g_x, g_y, map_x, - map_y) && costmap_->getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) + map_y) && costmap_->getCost(map_x, map_y) != nav2_costmap_2d::NO_INFORMATION) { int index = costmap_->getIndex(map_x, map_y); cell_values_[index] = 0.0; diff --git a/src/control/dwb_critics/src/prefer_forward.cpp b/nav2_controller/dwb_critics/src/prefer_forward.cpp similarity index 100% rename from src/control/dwb_critics/src/prefer_forward.cpp rename to nav2_controller/dwb_critics/src/prefer_forward.cpp diff --git a/src/control/dwb_critics/src/rotate_to_goal.cpp b/nav2_controller/dwb_critics/src/rotate_to_goal.cpp similarity index 100% rename from src/control/dwb_critics/src/rotate_to_goal.cpp rename to nav2_controller/dwb_critics/src/rotate_to_goal.cpp diff --git a/src/control/dwb_critics/src/twirling.cpp b/nav2_controller/dwb_critics/src/twirling.cpp similarity index 100% rename from src/control/dwb_critics/src/twirling.cpp rename to nav2_controller/dwb_critics/src/twirling.cpp diff --git a/src/control/dwb_msgs/CMakeLists.txt b/nav2_controller/dwb_msgs/CMakeLists.txt similarity index 100% rename from src/control/dwb_msgs/CMakeLists.txt rename to nav2_controller/dwb_msgs/CMakeLists.txt diff --git a/src/control/dwb_msgs/msg/CriticScore.msg b/nav2_controller/dwb_msgs/msg/CriticScore.msg similarity index 100% rename from src/control/dwb_msgs/msg/CriticScore.msg rename to nav2_controller/dwb_msgs/msg/CriticScore.msg diff --git a/src/control/dwb_msgs/msg/LocalPlanEvaluation.msg b/nav2_controller/dwb_msgs/msg/LocalPlanEvaluation.msg similarity index 100% rename from src/control/dwb_msgs/msg/LocalPlanEvaluation.msg rename to nav2_controller/dwb_msgs/msg/LocalPlanEvaluation.msg diff --git a/src/control/dwb_msgs/msg/Trajectory2D.msg b/nav2_controller/dwb_msgs/msg/Trajectory2D.msg similarity index 100% rename from src/control/dwb_msgs/msg/Trajectory2D.msg rename to nav2_controller/dwb_msgs/msg/Trajectory2D.msg diff --git a/src/control/dwb_msgs/msg/TrajectoryScore.msg b/nav2_controller/dwb_msgs/msg/TrajectoryScore.msg similarity index 100% rename from src/control/dwb_msgs/msg/TrajectoryScore.msg rename to nav2_controller/dwb_msgs/msg/TrajectoryScore.msg diff --git a/src/control/dwb_msgs/package.xml b/nav2_controller/dwb_msgs/package.xml similarity index 100% rename from src/control/dwb_msgs/package.xml rename to nav2_controller/dwb_msgs/package.xml diff --git a/src/control/dwb_msgs/srv/DebugLocalPlan.srv b/nav2_controller/dwb_msgs/srv/DebugLocalPlan.srv similarity index 100% rename from src/control/dwb_msgs/srv/DebugLocalPlan.srv rename to nav2_controller/dwb_msgs/srv/DebugLocalPlan.srv diff --git a/src/control/dwb_msgs/srv/GenerateTrajectory.srv b/nav2_controller/dwb_msgs/srv/GenerateTrajectory.srv similarity index 100% rename from src/control/dwb_msgs/srv/GenerateTrajectory.srv rename to nav2_controller/dwb_msgs/srv/GenerateTrajectory.srv diff --git a/src/control/dwb_msgs/srv/GenerateTwists.srv b/nav2_controller/dwb_msgs/srv/GenerateTwists.srv similarity index 100% rename from src/control/dwb_msgs/srv/GenerateTwists.srv rename to nav2_controller/dwb_msgs/srv/GenerateTwists.srv diff --git a/src/control/dwb_msgs/srv/GetCriticScore.srv b/nav2_controller/dwb_msgs/srv/GetCriticScore.srv similarity index 100% rename from src/control/dwb_msgs/srv/GetCriticScore.srv rename to nav2_controller/dwb_msgs/srv/GetCriticScore.srv diff --git a/src/control/dwb_msgs/srv/ScoreTrajectory.srv b/nav2_controller/dwb_msgs/srv/ScoreTrajectory.srv similarity index 100% rename from src/control/dwb_msgs/srv/ScoreTrajectory.srv rename to nav2_controller/dwb_msgs/srv/ScoreTrajectory.srv diff --git a/src/control/dwb_plugins/CMakeLists.txt b/nav2_controller/dwb_plugins/CMakeLists.txt similarity index 100% rename from src/control/dwb_plugins/CMakeLists.txt rename to nav2_controller/dwb_plugins/CMakeLists.txt diff --git a/src/control/dwb_plugins/cfg/KinematicParams.cfg b/nav2_controller/dwb_plugins/cfg/KinematicParams.cfg similarity index 100% rename from src/control/dwb_plugins/cfg/KinematicParams.cfg rename to nav2_controller/dwb_plugins/cfg/KinematicParams.cfg diff --git a/src/control/dwb_plugins/include/dwb_plugins/kinematic_parameters.h b/nav2_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/kinematic_parameters.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.h diff --git a/src/control/dwb_plugins/include/dwb_plugins/limited_accel_generator.h b/nav2_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/limited_accel_generator.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.h diff --git a/src/control/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.h b/nav2_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/one_d_velocity_iterator.h diff --git a/src/control/dwb_plugins/include/dwb_plugins/simple_goal_checker.h b/nav2_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/simple_goal_checker.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/simple_goal_checker.h diff --git a/src/control/dwb_plugins/include/dwb_plugins/standard_traj_generator.h b/nav2_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/standard_traj_generator.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.h diff --git a/src/control/dwb_plugins/include/dwb_plugins/stopped_goal_checker.h b/nav2_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/stopped_goal_checker.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/stopped_goal_checker.h diff --git a/src/control/dwb_plugins/include/dwb_plugins/velocity_iterator.h b/nav2_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/velocity_iterator.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.h diff --git a/src/control/dwb_plugins/include/dwb_plugins/xy_theta_iterator.h b/nav2_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.h similarity index 100% rename from src/control/dwb_plugins/include/dwb_plugins/xy_theta_iterator.h rename to nav2_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.h diff --git a/src/control/dwb_plugins/package.xml b/nav2_controller/dwb_plugins/package.xml similarity index 100% rename from src/control/dwb_plugins/package.xml rename to nav2_controller/dwb_plugins/package.xml diff --git a/src/control/dwb_plugins/plugins.xml b/nav2_controller/dwb_plugins/plugins.xml similarity index 100% rename from src/control/dwb_plugins/plugins.xml rename to nav2_controller/dwb_plugins/plugins.xml diff --git a/src/control/dwb_plugins/src/kinematic_parameters.cpp b/nav2_controller/dwb_plugins/src/kinematic_parameters.cpp similarity index 100% rename from src/control/dwb_plugins/src/kinematic_parameters.cpp rename to nav2_controller/dwb_plugins/src/kinematic_parameters.cpp diff --git a/src/control/dwb_plugins/src/limited_accel_generator.cpp b/nav2_controller/dwb_plugins/src/limited_accel_generator.cpp similarity index 100% rename from src/control/dwb_plugins/src/limited_accel_generator.cpp rename to nav2_controller/dwb_plugins/src/limited_accel_generator.cpp diff --git a/src/control/dwb_plugins/src/simple_goal_checker.cpp b/nav2_controller/dwb_plugins/src/simple_goal_checker.cpp similarity index 100% rename from src/control/dwb_plugins/src/simple_goal_checker.cpp rename to nav2_controller/dwb_plugins/src/simple_goal_checker.cpp diff --git a/src/control/dwb_plugins/src/standard_traj_generator.cpp b/nav2_controller/dwb_plugins/src/standard_traj_generator.cpp similarity index 100% rename from src/control/dwb_plugins/src/standard_traj_generator.cpp rename to nav2_controller/dwb_plugins/src/standard_traj_generator.cpp diff --git a/src/control/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_controller/dwb_plugins/src/stopped_goal_checker.cpp similarity index 100% rename from src/control/dwb_plugins/src/stopped_goal_checker.cpp rename to nav2_controller/dwb_plugins/src/stopped_goal_checker.cpp diff --git a/src/control/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_controller/dwb_plugins/src/xy_theta_iterator.cpp similarity index 100% rename from src/control/dwb_plugins/src/xy_theta_iterator.cpp rename to nav2_controller/dwb_plugins/src/xy_theta_iterator.cpp diff --git a/src/control/dwb_plugins/test/goal_checker.cpp b/nav2_controller/dwb_plugins/test/goal_checker.cpp similarity index 100% rename from src/control/dwb_plugins/test/goal_checker.cpp rename to nav2_controller/dwb_plugins/test/goal_checker.cpp diff --git a/src/control/dwb_plugins/test/goal_checker.launch b/nav2_controller/dwb_plugins/test/goal_checker.launch similarity index 100% rename from src/control/dwb_plugins/test/goal_checker.launch rename to nav2_controller/dwb_plugins/test/goal_checker.launch diff --git a/src/control/dwb_plugins/test/twist_gen.cpp b/nav2_controller/dwb_plugins/test/twist_gen.cpp similarity index 100% rename from src/control/dwb_plugins/test/twist_gen.cpp rename to nav2_controller/dwb_plugins/test/twist_gen.cpp diff --git a/src/control/dwb_plugins/test/twist_gen.launch b/nav2_controller/dwb_plugins/test/twist_gen.launch similarity index 100% rename from src/control/dwb_plugins/test/twist_gen.launch rename to nav2_controller/dwb_plugins/test/twist_gen.launch diff --git a/src/control/dwb_plugins/test/velocity_iterator_test.cpp b/nav2_controller/dwb_plugins/test/velocity_iterator_test.cpp similarity index 100% rename from src/control/dwb_plugins/test/velocity_iterator_test.cpp rename to nav2_controller/dwb_plugins/test/velocity_iterator_test.cpp diff --git a/src/localization/nav2_amcl/CHANGELOG.rst b/nav2_controller/nav2_controller/CHANGELOG.rst similarity index 100% rename from src/localization/nav2_amcl/CHANGELOG.rst rename to nav2_controller/nav2_controller/CHANGELOG.rst diff --git a/nav2_controller/nav2_controller/CMakeLists.txt b/nav2_controller/nav2_controller/CMakeLists.txt new file mode 100644 index 0000000000..545b593d6a --- /dev/null +++ b/nav2_controller/nav2_controller/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.5) +project(nav2_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/nav2_controller/nav2_controller/package.xml b/nav2_controller/nav2_controller/package.xml new file mode 100644 index 0000000000..ad6b0070ab --- /dev/null +++ b/nav2_controller/nav2_controller/package.xml @@ -0,0 +1,28 @@ + + + + nav2_controller + 0.0.1 + + ROS2 controller (DWB) metapackage + + Oregon Robotics Team + Steve Macenski + Apache License 2.0 + + ament_cmake + + costmap_queue + dwb_core + dwb_critics + dwb_msgs + dwb_plugins + nav2_controller_dwb + nav_2d_msgs + nav_2d_utils + + + ament_cmake + + + diff --git a/src/control/nav2_controller_dwb/CMakeLists.txt b/nav2_controller/nav2_controller_dwb/CMakeLists.txt similarity index 95% rename from src/control/nav2_controller_dwb/CMakeLists.txt rename to nav2_controller/nav2_controller_dwb/CMakeLists.txt index 303f0d4567..b4b3cfefdd 100644 --- a/src/control/nav2_controller_dwb/CMakeLists.txt +++ b/nav2_controller/nav2_controller_dwb/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_tasks REQUIRED) find_package(nav2_robot REQUIRED) -find_package(nav2_planning_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(dwb_core REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread) @@ -41,7 +41,7 @@ set(dependencies std_msgs nav2_tasks nav2_robot - nav2_planning_msgs + nav2_msgs dwb_core ) diff --git a/src/control/nav2_controller_dwb/include/nav2_controller_dwb/dwb_controller.hpp b/nav2_controller/nav2_controller_dwb/include/nav2_controller_dwb/dwb_controller.hpp similarity index 100% rename from src/control/nav2_controller_dwb/include/nav2_controller_dwb/dwb_controller.hpp rename to nav2_controller/nav2_controller_dwb/include/nav2_controller_dwb/dwb_controller.hpp diff --git a/src/control/nav2_controller_dwb/package.xml b/nav2_controller/nav2_controller_dwb/package.xml similarity index 89% rename from src/control/nav2_controller_dwb/package.xml rename to nav2_controller/nav2_controller_dwb/package.xml index 308e242ef4..cedb1e9dcb 100644 --- a/src/control/nav2_controller_dwb/package.xml +++ b/nav2_controller/nav2_controller_dwb/package.xml @@ -13,12 +13,12 @@ std_msgs nav2_tasks nav2_robot - nav2_planning_msgs + nav2_msgs dwb_core rclcpp std_msgs - nav2_planning_msgs + nav2_msgs ament_lint_common ament_lint_auto diff --git a/src/control/nav2_controller_dwb/src/dwb_controller.cpp b/nav2_controller/nav2_controller_dwb/src/dwb_controller.cpp similarity index 97% rename from src/control/nav2_controller_dwb/src/dwb_controller.cpp rename to nav2_controller/nav2_controller_dwb/src/dwb_controller.cpp index e19071496d..7334699168 100644 --- a/src/control/nav2_controller_dwb/src/dwb_controller.cpp +++ b/nav2_controller/nav2_controller_dwb/src/dwb_controller.cpp @@ -45,7 +45,7 @@ DwbController::execute(const nav2_tasks::FollowPathCommand::SharedPtr /*command* // get path from command tf2_ros::Buffer tfBuffer; tf2_ros::TransformListener tfListener(tfBuffer); - cm_ = std::make_shared("local_costmap", tfBuffer); + cm_ = std::make_shared("local_costmap", tfBuffer); auto nh = shared_from_this(); odom_sub_ = std::make_shared(*this); vel_pub_ = this->create_publisher("cmd_vel", 1); diff --git a/src/control/nav2_controller_dwb/src/main.cpp b/nav2_controller/nav2_controller_dwb/src/main.cpp similarity index 100% rename from src/control/nav2_controller_dwb/src/main.cpp rename to nav2_controller/nav2_controller_dwb/src/main.cpp diff --git a/src/mapping/nav2_map_server/CHANGELOG.rst b/nav2_controller/nav2_controller_example/CHANGELOG.rst similarity index 100% rename from src/mapping/nav2_map_server/CHANGELOG.rst rename to nav2_controller/nav2_controller_example/CHANGELOG.rst diff --git a/src/control/nav2_controller_example/CMakeLists.txt b/nav2_controller/nav2_controller_example/CMakeLists.txt similarity index 95% rename from src/control/nav2_controller_example/CMakeLists.txt rename to nav2_controller/nav2_controller_example/CMakeLists.txt index 93a30c10e4..8f5a1d9653 100644 --- a/src/control/nav2_controller_example/CMakeLists.txt +++ b/nav2_controller/nav2_controller_example/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_tasks REQUIRED) find_package(nav2_robot REQUIRED) -find_package(nav2_planning_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) include_directories( include @@ -38,7 +38,7 @@ set(dependencies std_msgs nav2_tasks nav2_robot - nav2_planning_msgs + nav2_msgs ) ament_target_dependencies(${executable_name} diff --git a/src/localization/nav2_amcl/README.md b/nav2_controller/nav2_controller_example/README.md similarity index 100% rename from src/localization/nav2_amcl/README.md rename to nav2_controller/nav2_controller_example/README.md diff --git a/src/mapping/nav2_map_server/README.md b/nav2_controller/nav2_controller_example/doc/.gitignore similarity index 100% rename from src/mapping/nav2_map_server/README.md rename to nav2_controller/nav2_controller_example/doc/.gitignore diff --git a/src/control/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp b/nav2_controller/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp similarity index 100% rename from src/control/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp rename to nav2_controller/nav2_controller_example/include/nav2_controller_example/dwa_controller.hpp diff --git a/src/control/nav2_controller_example/package.xml b/nav2_controller/nav2_controller_example/package.xml similarity index 89% rename from src/control/nav2_controller_example/package.xml rename to nav2_controller/nav2_controller_example/package.xml index 9234e295b1..546f99f6b6 100644 --- a/src/control/nav2_controller_example/package.xml +++ b/nav2_controller/nav2_controller_example/package.xml @@ -13,11 +13,11 @@ std_msgs nav2_tasks nav2_robot - nav2_planning_msgs + nav2_msgs rclcpp std_msgs - nav2_planning_msgs + nav2_msgs ament_lint_common ament_lint_auto diff --git a/src/control/nav2_controller_example/src/dwa_controller.cpp b/nav2_controller/nav2_controller_example/src/dwa_controller.cpp similarity index 100% rename from src/control/nav2_controller_example/src/dwa_controller.cpp rename to nav2_controller/nav2_controller_example/src/dwa_controller.cpp diff --git a/src/control/nav2_controller_example/src/main.cpp b/nav2_controller/nav2_controller_example/src/main.cpp similarity index 100% rename from src/control/nav2_controller_example/src/main.cpp rename to nav2_controller/nav2_controller_example/src/main.cpp diff --git a/src/mission_execution/nav2_mission_execution_msgs/CHANGELOG.rst b/nav2_controller/nav2_controller_example/test/.gitignore similarity index 100% rename from src/mission_execution/nav2_mission_execution_msgs/CHANGELOG.rst rename to nav2_controller/nav2_controller_example/test/.gitignore diff --git a/src/control/nav_2d_msgs/CMakeLists.txt b/nav2_controller/nav_2d_msgs/CMakeLists.txt similarity index 100% rename from src/control/nav_2d_msgs/CMakeLists.txt rename to nav2_controller/nav_2d_msgs/CMakeLists.txt diff --git a/src/control/nav_2d_msgs/msg/Path2D.msg b/nav2_controller/nav_2d_msgs/msg/Path2D.msg similarity index 100% rename from src/control/nav_2d_msgs/msg/Path2D.msg rename to nav2_controller/nav_2d_msgs/msg/Path2D.msg diff --git a/src/control/nav_2d_msgs/msg/Pose2D32.msg b/nav2_controller/nav_2d_msgs/msg/Pose2D32.msg similarity index 100% rename from src/control/nav_2d_msgs/msg/Pose2D32.msg rename to nav2_controller/nav_2d_msgs/msg/Pose2D32.msg diff --git a/src/control/nav_2d_msgs/msg/Pose2DStamped.msg b/nav2_controller/nav_2d_msgs/msg/Pose2DStamped.msg similarity index 100% rename from src/control/nav_2d_msgs/msg/Pose2DStamped.msg rename to nav2_controller/nav_2d_msgs/msg/Pose2DStamped.msg diff --git a/src/control/nav_2d_msgs/msg/Twist2D.msg b/nav2_controller/nav_2d_msgs/msg/Twist2D.msg similarity index 100% rename from src/control/nav_2d_msgs/msg/Twist2D.msg rename to nav2_controller/nav_2d_msgs/msg/Twist2D.msg diff --git a/src/control/nav_2d_msgs/msg/Twist2D32.msg b/nav2_controller/nav_2d_msgs/msg/Twist2D32.msg similarity index 100% rename from src/control/nav_2d_msgs/msg/Twist2D32.msg rename to nav2_controller/nav_2d_msgs/msg/Twist2D32.msg diff --git a/src/control/nav_2d_msgs/msg/Twist2DStamped.msg b/nav2_controller/nav_2d_msgs/msg/Twist2DStamped.msg similarity index 100% rename from src/control/nav_2d_msgs/msg/Twist2DStamped.msg rename to nav2_controller/nav_2d_msgs/msg/Twist2DStamped.msg diff --git a/src/control/nav_2d_msgs/package.xml b/nav2_controller/nav_2d_msgs/package.xml similarity index 100% rename from src/control/nav_2d_msgs/package.xml rename to nav2_controller/nav_2d_msgs/package.xml diff --git a/src/control/nav_2d_utils/CMakeLists.txt b/nav2_controller/nav_2d_utils/CMakeLists.txt similarity index 100% rename from src/control/nav_2d_utils/CMakeLists.txt rename to nav2_controller/nav_2d_utils/CMakeLists.txt diff --git a/src/control/nav_2d_utils/include/nav_2d_utils/conversions.h b/nav2_controller/nav_2d_utils/include/nav_2d_utils/conversions.h similarity index 100% rename from src/control/nav_2d_utils/include/nav_2d_utils/conversions.h rename to nav2_controller/nav_2d_utils/include/nav_2d_utils/conversions.h diff --git a/src/control/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h b/nav2_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h similarity index 100% rename from src/control/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h rename to nav2_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.h diff --git a/src/control/nav_2d_utils/include/nav_2d_utils/parameters.h b/nav2_controller/nav_2d_utils/include/nav_2d_utils/parameters.h similarity index 100% rename from src/control/nav_2d_utils/include/nav_2d_utils/parameters.h rename to nav2_controller/nav_2d_utils/include/nav_2d_utils/parameters.h diff --git a/src/control/nav_2d_utils/include/nav_2d_utils/path_ops.h b/nav2_controller/nav_2d_utils/include/nav_2d_utils/path_ops.h similarity index 100% rename from src/control/nav_2d_utils/include/nav_2d_utils/path_ops.h rename to nav2_controller/nav_2d_utils/include/nav_2d_utils/path_ops.h diff --git a/src/control/nav_2d_utils/include/nav_2d_utils/tf_help.h b/nav2_controller/nav_2d_utils/include/nav_2d_utils/tf_help.h similarity index 100% rename from src/control/nav_2d_utils/include/nav_2d_utils/tf_help.h rename to nav2_controller/nav_2d_utils/include/nav_2d_utils/tf_help.h diff --git a/src/control/nav_2d_utils/package.xml b/nav2_controller/nav_2d_utils/package.xml similarity index 100% rename from src/control/nav_2d_utils/package.xml rename to nav2_controller/nav_2d_utils/package.xml diff --git a/src/control/nav_2d_utils/src/conversions.cpp b/nav2_controller/nav_2d_utils/src/conversions.cpp similarity index 100% rename from src/control/nav_2d_utils/src/conversions.cpp rename to nav2_controller/nav_2d_utils/src/conversions.cpp diff --git a/src/control/nav_2d_utils/src/path_ops.cpp b/nav2_controller/nav_2d_utils/src/path_ops.cpp similarity index 100% rename from src/control/nav_2d_utils/src/path_ops.cpp rename to nav2_controller/nav_2d_utils/src/path_ops.cpp diff --git a/nav2_costmap_2d/CHANGELOG.rst b/nav2_costmap_2d/CHANGELOG.rst new file mode 100644 index 0000000000..7bab0fce32 --- /dev/null +++ b/nav2_costmap_2d/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package nav2_costmap_2d +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +(2018-10-9) +------------------- +* Port nav2_costmap_2d from costmap_2d version 1.16.2 (2018-07-31) diff --git a/src/libs/costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt similarity index 78% rename from src/libs/costmap_2d/CMakeLists.txt rename to nav2_costmap_2d/CMakeLists.txt index 899f75953d..35b9f6d479 100644 --- a/src/libs/costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(costmap_2d) +project(nav2_costmap_2d) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -99,30 +99,30 @@ ament_target_dependencies(layers nav2_costmap_2d ) -# TODO(bpwilcox): port costmap_2d_markers -#add_executable(costmap_2d_markers src/costmap_2d_markers.cpp) -#ament_target_dependencies(costmap_2d_markers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS}) -#target_link_libraries(costmap_2d_markers - #costmap_2d +# TODO(bpwilcox): port nav2_costmap_2d_markers +#add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) +#ament_target_dependencies(nav2_costmap_2d_markers ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS}) +#target_link_libraries(nav2_costmap_2d_markers + #nav2_costmap_2d #) -# TODO(bpwilcox): port costmap_2d_cloud -#add_executable(costmap_2d_cloud src/costmap_2d_cloud.cpp) -#ament_target_dependencies(costmap_2d_cloud ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS}) -#target_link_libraries(costmap_2d_cloud - #costmap_2d +# TODO(bpwilcox): port nav2_costmap_2d_cloud +#add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp) +#ament_target_dependencies(nav2_costmap_2d_cloud ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ament_EXPORTED_TARGETS}) +#target_link_libraries(nav2_costmap_2d_cloud + #nav2_costmap_2d #) -add_executable(costmap_2d_node src/costmap_2d_node.cpp) -ament_target_dependencies(costmap_2d_node +add_executable(nav2_costmap_2d_node src/costmap_2d_node.cpp) +ament_target_dependencies(nav2_costmap_2d_node ${dependencies} ) -target_link_libraries(costmap_2d_node +target_link_libraries(nav2_costmap_2d_node nav2_costmap_2d ) -install(TARGETS nav2_costmap_2d layers costmap_2d_node +install(TARGETS nav2_costmap_2d layers nav2_costmap_2d_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME} diff --git a/src/libs/costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg similarity index 95% rename from src/libs/costmap_2d/cfg/Costmap2D.cfg rename to nav2_costmap_2d/cfg/Costmap2D.cfg index 772eb1cd98..07c4a1628b 100755 --- a/src/libs/costmap_2d/cfg/Costmap2D.cfg +++ b/nav2_costmap_2d/cfg/Costmap2D.cfg @@ -20,4 +20,4 @@ gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robo gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) -exit(gen.generate("costmap_2d", "costmap_2d", "Costmap2D")) +exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D")) diff --git a/src/libs/costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg similarity index 74% rename from src/libs/costmap_2d/cfg/GenericPlugin.cfg rename to nav2_costmap_2d/cfg/GenericPlugin.cfg index 11cd4d14e0..555e2b5415 100755 --- a/src/libs/costmap_2d/cfg/GenericPlugin.cfg +++ b/nav2_costmap_2d/cfg/GenericPlugin.cfg @@ -4,4 +4,4 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, b gen = ParameterGenerator() gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -exit(gen.generate("costmap_2d", "costmap_2d", "GenericPlugin")) +exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin")) diff --git a/src/libs/costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg similarity index 87% rename from src/libs/costmap_2d/cfg/InflationPlugin.cfg rename to nav2_costmap_2d/cfg/InflationPlugin.cfg index c15152581f..5c11eaf791 100755 --- a/src/libs/costmap_2d/cfg/InflationPlugin.cfg +++ b/nav2_costmap_2d/cfg/InflationPlugin.cfg @@ -9,4 +9,4 @@ gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost v gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) -exit(gen.generate("costmap_2d", "costmap_2d", "InflationPlugin")) +exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin")) diff --git a/src/libs/costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg similarity index 94% rename from src/libs/costmap_2d/cfg/ObstaclePlugin.cfg rename to nav2_costmap_2d/cfg/ObstaclePlugin.cfg index 7b434a8e78..0337e4123a 100755 --- a/src/libs/costmap_2d/cfg/ObstaclePlugin.cfg +++ b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg @@ -17,4 +17,4 @@ gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, ed #gen.add("max_obstacle_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) #gen.add("raytrace_range", double_t, 0, "The default range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) -exit(gen.generate("costmap_2d", "costmap_2d", "ObstaclePlugin")) +exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) diff --git a/src/libs/costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg similarity index 94% rename from src/libs/costmap_2d/cfg/VoxelPlugin.cfg rename to nav2_costmap_2d/cfg/VoxelPlugin.cfg index ec621c5ae2..977fcb99d4 100755 --- a/src/libs/costmap_2d/cfg/VoxelPlugin.cfg +++ b/nav2_costmap_2d/cfg/VoxelPlugin.cfg @@ -19,4 +19,4 @@ combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), "Method for combining layers enum") gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) -exit(gen.generate("costmap_2d", "costmap_2d", "VoxelPlugin")) +exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin")) diff --git a/src/libs/costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml similarity index 61% rename from src/libs/costmap_2d/costmap_plugins.xml rename to nav2_costmap_2d/costmap_plugins.xml index 800f0288de..754c56d1a9 100644 --- a/src/libs/costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -1,15 +1,15 @@ - + Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles. - + Listens to laser scan and point cloud messages and marks and clears grid cells. - + Listens to OccupancyGrid messages and copies them in, like from map_server. - + Similar to obstacle costmap, but uses 3D voxel grid to store data. diff --git a/src/libs/costmap_2d/include/costmap_2d/array_parser.h b/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.h similarity index 92% rename from src/libs/costmap_2d/include/costmap_2d/array_parser.h rename to nav2_costmap_2d/include/nav2_costmap_2d/array_parser.h index 8d2ecf351f..7acc55bb0b 100644 --- a/src/libs/costmap_2d/include/costmap_2d/array_parser.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/array_parser.h @@ -28,13 +28,13 @@ * * author: Dave Hershberger */ -#ifndef COSTMAP_2D_ARRAY_PARSER_H -#define COSTMAP_2D_ARRAY_PARSER_H +#ifndef nav2_costmap_2d_ARRAY_PARSER_H +#define nav2_costmap_2d_ARRAY_PARSER_H #include #include -namespace costmap_2d +namespace nav2_costmap_2d { /** @brief Parse a vector of vectors of floats from a string. @@ -46,6 +46,6 @@ namespace costmap_2d * anything, like part of a successful parse. */ std::vector > parseVVF(const std::string & input, std::string & error_return); -} // end namespace costmap_2d +} // end namespace nav2_costmap_2d -#endif // COSTMAP_2D_ARRAY_PARSER_H +#endif // nav2_costmap_2d_ARRAY_PARSER_H diff --git a/src/libs/costmap_2d/include/costmap_2d/cost_values.h b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.h similarity index 93% rename from src/libs/costmap_2d/include/costmap_2d/cost_values.h rename to nav2_costmap_2d/include/nav2_costmap_2d/cost_values.h index 7f042ff48b..6703f8af3d 100644 --- a/src/libs/costmap_2d/include/costmap_2d/cost_values.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.h @@ -34,14 +34,14 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef COSTMAP_2D_COST_VALUES_H_ -#define COSTMAP_2D_COST_VALUES_H_ +#ifndef nav2_costmap_2d_COST_VALUES_H_ +#define nav2_costmap_2d_COST_VALUES_H_ /** Provides a mapping for often used cost values */ -namespace costmap_2d +namespace nav2_costmap_2d { static const unsigned char NO_INFORMATION = 255; static const unsigned char LETHAL_OBSTACLE = 254; static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; static const unsigned char FREE_SPACE = 0; } -#endif // COSTMAP_2D_COST_VALUES_H_ +#endif // nav2_costmap_2d_COST_VALUES_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/costmap_2d.h b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.h similarity index 98% rename from src/libs/costmap_2d/include/costmap_2d/costmap_2d.h rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.h index 26fe0b3b36..6d21696eee 100644 --- a/src/libs/costmap_2d/include/costmap_2d/costmap_2d.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.h @@ -35,8 +35,8 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_COSTMAP_2D_H_ -#define COSTMAP_2D_COSTMAP_2D_H_ +#ifndef nav2_costmap_2d_nav2_costmap_2d_H_ +#define nav2_costmap_2d_nav2_costmap_2d_H_ #include #include @@ -46,7 +46,7 @@ #include #include -namespace costmap_2d +namespace nav2_costmap_2d { // convenient for storing x/y point pairs @@ -477,6 +477,6 @@ class Costmap2D std::vector & cells_; }; }; -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_COSTMAP_2D_H +#endif // nav2_costmap_2d_nav2_costmap_2d_H diff --git a/src/libs/costmap_2d/include/costmap_2d/costmap_2d_publisher.h b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.h similarity index 93% rename from src/libs/costmap_2d/include/costmap_2d/costmap_2d_publisher.h rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.h index a29727f068..306b33483a 100644 --- a/src/libs/costmap_2d/include/costmap_2d/costmap_2d_publisher.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.h @@ -35,16 +35,16 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_COSTMAP_2D_PUBLISHER_H_ -#define COSTMAP_2D_COSTMAP_2D_PUBLISHER_H_ +#ifndef nav2_costmap_2d_nav2_costmap_2d_PUBLISHER_H_ +#define nav2_costmap_2d_nav2_costmap_2d_PUBLISHER_H_ #include "rclcpp/rclcpp.hpp" -#include +#include #include #include #include -namespace costmap_2d +namespace nav2_costmap_2d { /** * @class Costmap2DPublisher @@ -109,5 +109,5 @@ class Costmap2DPublisher nav_msgs::msg::OccupancyGrid grid_; static char * cost_translation_table_; ///< Translate from 0-255 values in costmap to -1 to 100 values in message. }; -} // namespace costmap_2d -#endif // COSTMAP_2D_COSTMAP_2D_PUBLISHER_H +} // namespace nav2_costmap_2d +#endif // nav2_costmap_2d_nav2_costmap_2d_PUBLISHER_H diff --git a/src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.h similarity index 91% rename from src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.h index 74152cb636..53ec7d25ea 100644 --- a/src/libs/costmap_2d/include/costmap_2d/costmap_2d_ros.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.h @@ -35,14 +35,14 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_ -#define COSTMAP_2D_COSTMAP_2D_ROS_H_ - -#include -#include -#include -//#include -#include +#ifndef nav2_costmap_2d_nav2_costmap_2d_ROS_H_ +#define nav2_costmap_2d_nav2_costmap_2d_ROS_H_ + +#include +#include +#include +//#include +#include #include #include // TODO(bpwilcox): Resolve dynamic reconfigure dependencies @@ -75,7 +75,7 @@ class SuperValue : public XmlRpc::XmlRpcValue } }; -namespace costmap_2d +namespace nav2_costmap_2d { /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to @@ -179,7 +179,7 @@ class Costmap2DROS /** @brief Returns the current padded footprint as a geometry_msgs::msg::Polygon. */ geometry_msgs::msg::Polygon getRobotFootprintPolygon() { - return costmap_2d::toPolygon(padded_footprint_); + return nav2_costmap_2d::toPolygon(padded_footprint_); } /** @brief Return the current footprint of the robot as a vector of points. @@ -250,13 +250,13 @@ class Costmap2DROS * * If the values of footprint and robot_radius are the same in * new_config and old_config, nothing is changed. */ - //void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, - // const costmap_2d::Costmap2DConfig &old_config); + //void readFootprintFromConfig(const nav2_costmap_2d::Costmap2DConfig &new_config, + // const nav2_costmap_2d::Costmap2DConfig &old_config); void resetOldParameters(rclcpp::Node::SharedPtr nh); // TODO(bpwilcox): Resolve dynamic reconfigure dependencies - //void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level); + //void reconfigureCB(nav2_costmap_2d::Costmap2DConfig &config, uint32_t level); void movementCB(); void mapUpdateLoop(double frequency); @@ -271,8 +271,8 @@ class Costmap2DROS Costmap2DPublisher * publisher_; // TODO(bpwilcox): Resolve dynamic reconfigure dependencies - //dynamic_reconfigure::Server *dsrv_; - //costmap_2d::Costmap2DConfig old_config_; + //dynamic_reconfigure::Server *dsrv_; + //nav2_costmap_2d::Costmap2DConfig old_config_; std::recursive_mutex configuration_mutex_; @@ -284,6 +284,6 @@ class Costmap2DROS float footprint_padding_; }; // class Costmap2DROS -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_COSTMAP_2D_ROS_H +#endif // nav2_costmap_2d_nav2_costmap_2d_ROS_H diff --git a/src/libs/costmap_2d/include/costmap_2d/costmap_layer.h b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.h similarity index 89% rename from src/libs/costmap_2d/include/costmap_2d/costmap_layer.h rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.h index 2f7d7abf54..fc55145f65 100644 --- a/src/libs/costmap_2d/include/costmap_2d/costmap_layer.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.h @@ -35,14 +35,14 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_COSTMAP_LAYER_H_ -#define COSTMAP_2D_COSTMAP_LAYER_H_ +#ifndef nav2_costmap_2d_COSTMAP_LAYER_H_ +#define nav2_costmap_2d_COSTMAP_LAYER_H_ #include -#include -#include +#include +#include -namespace costmap_2d +namespace nav2_costmap_2d { class CostmapLayer : public Layer, public Costmap2D @@ -78,7 +78,7 @@ class CostmapLayer : public Layer, public Costmap2D * TrueOverwrite means every value from this layer * is written into the master grid. */ - void updateWithTrueOverwrite(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + void updateWithTrueOverwrite(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); /* @@ -88,7 +88,7 @@ class CostmapLayer : public Layer, public Costmap2D * Overwrite means every valid value from this layer * is written into the master grid (does not copy NO_INFORMATION) */ - void updateWithOverwrite(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + void updateWithOverwrite(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); /* @@ -100,7 +100,7 @@ class CostmapLayer : public Layer, public Costmap2D * it is overwritten. If the layer's value is NO_INFORMATION, * the master value does not change. */ - void updateWithMax(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + void updateWithMax(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); /* @@ -115,7 +115,7 @@ class CostmapLayer : public Layer, public Costmap2D * If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE, * the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1). */ - void updateWithAddition(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + void updateWithAddition(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); /** @@ -150,5 +150,5 @@ class CostmapLayer : public Layer, public Costmap2D double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; }; -} // namespace costmap_2d -#endif // COSTMAP_2D_COSTMAP_LAYER_H_ +} // namespace nav2_costmap_2d +#endif // nav2_costmap_2d_COSTMAP_LAYER_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/costmap_math.h b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.h similarity index 95% rename from src/libs/costmap_2d/include/costmap_2d/costmap_math.h rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.h index d893edbe61..8fee35cafa 100644 --- a/src/libs/costmap_2d/include/costmap_2d/costmap_math.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_math.h @@ -35,8 +35,8 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_COSTMAP_MATH_H_ -#define COSTMAP_2D_COSTMAP_MATH_H_ +#ifndef nav2_costmap_2d_COSTMAP_MATH_H_ +#define nav2_costmap_2d_COSTMAP_MATH_H_ #include #include @@ -67,4 +67,4 @@ bool intersects(std::vector & polygon, float testx, f bool intersects(std::vector & polygon1, std::vector & polygon2); -#endif // COSTMAP_2D_COSTMAP_MATH_H_ +#endif // nav2_costmap_2d_COSTMAP_MATH_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/footprint.h b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.h similarity index 97% rename from src/libs/costmap_2d/include/costmap_2d/footprint.h rename to nav2_costmap_2d/include/nav2_costmap_2d/footprint.h index eb87c7d582..7ed44b0b13 100644 --- a/src/libs/costmap_2d/include/costmap_2d/footprint.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint.h @@ -35,8 +35,8 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_FOOTPRINT_H -#define COSTMAP_2D_FOOTPRINT_H +#ifndef nav2_costmap_2d_FOOTPRINT_H +#define nav2_costmap_2d_FOOTPRINT_H #include "rclcpp/rclcpp.hpp" #include @@ -45,7 +45,7 @@ #include #include -namespace costmap_2d +namespace nav2_costmap_2d { /** @@ -148,6 +148,6 @@ std::vector makeFootprintFromXMLRPC( void writeFootprintToParam(rclcpp::Node::SharedPtr nh, const std::vector & footprint); -} // end namespace costmap_2d +} // end namespace nav2_costmap_2d -#endif // COSTMAP_2D_FOOTPRINT_H +#endif // nav2_costmap_2d_FOOTPRINT_H diff --git a/src/libs/costmap_2d/include/costmap_2d/inflation_layer.h b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.h similarity index 91% rename from src/libs/costmap_2d/include/costmap_2d/inflation_layer.h rename to nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.h index 71809c4603..f62171f975 100644 --- a/src/libs/costmap_2d/include/costmap_2d/inflation_layer.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.h @@ -35,17 +35,17 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_INFLATION_LAYER_H_ -#define COSTMAP_2D_INFLATION_LAYER_H_ +#ifndef nav2_costmap_2d_INFLATION_LAYER_H_ +#define nav2_costmap_2d_INFLATION_LAYER_H_ #include -#include -#include +#include +#include // TODO(bpwilcox): Resolve dynamic reconfigure dependencies -//#include +//#include //#include -namespace costmap_2d +namespace nav2_costmap_2d { /** * @class CellData @@ -90,7 +90,7 @@ class InflationLayer : public Layer double * min_y, double * max_x, double * max_y); - virtual void updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + virtual void updateCosts(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); virtual bool isDiscretized() { @@ -189,12 +189,12 @@ class InflationLayer : public Layer double last_min_x_, last_min_y_, last_max_x_, last_max_y_; // TODO(bpwilcox): Resolve dynamic reconfigure dependencies - //dynamic_reconfigure::Server *dsrv_; - //void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level); + //dynamic_reconfigure::Server *dsrv_; + //void reconfigureCB(nav2_costmap_2d::InflationPluginConfig &config, uint32_t level); bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. }; -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_INFLATION_LAYER_H_ +#endif // nav2_costmap_2d_INFLATION_LAYER_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/layer.h b/nav2_costmap_2d/include/nav2_costmap_2d/layer.h similarity index 94% rename from src/libs/costmap_2d/include/costmap_2d/layer.h rename to nav2_costmap_2d/include/nav2_costmap_2d/layer.h index a18930dd93..4d1d6271f2 100644 --- a/src/libs/costmap_2d/include/costmap_2d/layer.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.h @@ -34,15 +34,15 @@ * * Author: David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_LAYER_H_ -#define COSTMAP_2D_LAYER_H_ +#ifndef nav2_costmap_2d_LAYER_H_ +#define nav2_costmap_2d_LAYER_H_ -#include -#include +#include +#include #include #include -namespace costmap_2d +namespace nav2_costmap_2d { class LayeredCostmap; @@ -130,6 +130,6 @@ class Layer std::vector footprint_spec_; }; -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_LAYER_H_ +#endif // nav2_costmap_2d_LAYER_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/layered_costmap.h b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.h similarity index 92% rename from src/libs/costmap_2d/include/costmap_2d/layered_costmap.h rename to nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.h index 6fb412af2b..12d7faa82d 100644 --- a/src/libs/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layered_costmap.h @@ -35,16 +35,16 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_LAYERED_COSTMAP_H_ -#define COSTMAP_2D_LAYERED_COSTMAP_H_ +#ifndef nav2_costmap_2d_LAYERED_COSTMAP_H_ +#define nav2_costmap_2d_LAYERED_COSTMAP_H_ -#include -#include -#include +#include +#include +#include #include #include -namespace costmap_2d +namespace nav2_costmap_2d { class Layer; @@ -102,7 +102,7 @@ class LayeredCostmap bool isTrackingUnknown() { - return costmap_.getDefaultValue() == costmap_2d::NO_INFORMATION; + return costmap_.getDefaultValue() == nav2_costmap_2d::NO_INFORMATION; } std::vector > * getPlugins() @@ -173,6 +173,6 @@ class LayeredCostmap std::vector footprint_; }; -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_LAYERED_COSTMAP_H_ +#endif // nav2_costmap_2d_LAYERED_COSTMAP_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/observation.h b/nav2_costmap_2d/include/nav2_costmap_2d/observation.h similarity index 95% rename from src/libs/costmap_2d/include/costmap_2d/observation.h rename to nav2_costmap_2d/include/nav2_costmap_2d/observation.h index 8963c067b6..f1445bf4e0 100644 --- a/src/libs/costmap_2d/include/costmap_2d/observation.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation.h @@ -29,13 +29,13 @@ * Authors: Conor McGann */ -#ifndef COSTMAP_2D_OBSERVATION_H_ -#define COSTMAP_2D_OBSERVATION_H_ +#ifndef nav2_costmap_2d_OBSERVATION_H_ +#define nav2_costmap_2d_OBSERVATION_H_ #include #include -namespace costmap_2d +namespace nav2_costmap_2d { /** @@ -99,5 +99,5 @@ class Observation double obstacle_range_, raytrace_range_; }; -} // namespace costmap_2d -#endif // COSTMAP_2D_OBSERVATION_H_ +} // namespace nav2_costmap_2d +#endif // nav2_costmap_2d_OBSERVATION_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/observation_buffer.h b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.h similarity index 95% rename from src/libs/costmap_2d/include/costmap_2d/observation_buffer.h rename to nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.h index b7beebe8da..98d24f0e2e 100644 --- a/src/libs/costmap_2d/include/costmap_2d/observation_buffer.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.h @@ -34,20 +34,20 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#ifndef COSTMAP_2D_OBSERVATION_BUFFER_H_ -#define COSTMAP_2D_OBSERVATION_BUFFER_H_ +#ifndef nav2_costmap_2d_OBSERVATION_BUFFER_H_ +#define nav2_costmap_2d_OBSERVATION_BUFFER_H_ #include #include #include #include "rclcpp/time.hpp" -#include +#include #include #include -namespace costmap_2d +namespace nav2_costmap_2d { /** * @class ObservationBuffer @@ -151,5 +151,5 @@ class ObservationBuffer double obstacle_range_, raytrace_range_; double tf_tolerance_; }; -} // namespace costmap_2d -#endif // COSTMAP_2D_OBSERVATION_BUFFER_H_ +} // namespace nav2_costmap_2d +#endif // nav2_costmap_2d_OBSERVATION_BUFFER_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.h similarity index 76% rename from src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h rename to nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.h index 9b64ba1cb9..1b11d164f8 100644 --- a/src/libs/costmap_2d/include/costmap_2d/obstacle_layer.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.h @@ -35,13 +35,13 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_OBSTACLE_LAYER_H_ -#define COSTMAP_2D_OBSTACLE_LAYER_H_ +#ifndef nav2_costmap_2d_OBSTACLE_LAYER_H_ +#define nav2_costmap_2d_OBSTACLE_LAYER_H_ #include -#include -#include -#include +#include +#include +#include #include @@ -53,10 +53,10 @@ #include #include #include -#include -#include +#include +#include -namespace costmap_2d +namespace nav2_costmap_2d { class ObstacleLayer : public CostmapLayer @@ -73,7 +73,7 @@ class ObstacleLayer : public CostmapLayer double * min_y, double * max_x, double * max_y); - virtual void updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + virtual void updateCosts(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); virtual void activate(); @@ -86,7 +86,7 @@ class ObstacleLayer : public CostmapLayer * @param buffer A pointer to the observation buffer to update */ void laserScanCallback(const sensor_msgs::LaserScanConstPtr & message, - const std::shared_ptr & buffer); + const std::shared_ptr & buffer); /** * @brief A callback to handle buffering LaserScan messages which need filtering to turn Inf values into range_max. @@ -102,7 +102,7 @@ class ObstacleLayer : public CostmapLayer * @param buffer A pointer to the observation buffer to update */ void pointCloudCallback(const sensor_msgs::PointCloudConstPtr & message, - const std::shared_ptr & buffer); + const std::shared_ptr & buffer); /** * @brief A callback to handle buffering PointCloud2 messages @@ -110,10 +110,10 @@ class ObstacleLayer : public CostmapLayer * @param buffer A pointer to the observation buffer to update */ void pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr & message, - const std::shared_ptr & buffer); + const std::shared_ptr & buffer); // for testing purposes - void addStaticObservation(costmap_2d::Observation & obs, bool marking, bool clearing); + void addStaticObservation(nav2_costmap_2d::Observation & obs, bool marking, bool clearing); void clearStaticObservations(bool marking, bool clearing); protected: @@ -124,14 +124,14 @@ class ObstacleLayer : public CostmapLayer * @param marking_observations A reference to a vector that will be populated with the observations * @return True if all the observation buffers are current, false otherwise */ - bool getMarkingObservations(std::vector & marking_observations) const; + bool getMarkingObservations(std::vector & marking_observations) const; /** * @brief Get the observations used to clear space * @param clearing_observations A reference to a vector that will be populated with the observations * @return True if all the observation buffers are current, false otherwise */ - bool getClearingObservations(std::vector & clearing_observations) const; + bool getClearingObservations(std::vector & clearing_observations) const; /** * @brief Clear freespace based on one observation @@ -141,7 +141,7 @@ class ObstacleLayer : public CostmapLayer * @param max_x * @param max_y */ - virtual void raytraceFreespace(const costmap_2d::Observation & clearing_observation, + virtual void raytraceFreespace(const nav2_costmap_2d::Observation & clearing_observation, double * min_x, double * min_y, double * max_x, double * max_y); @@ -165,22 +165,22 @@ class ObstacleLayer : public CostmapLayer std::vector > observation_subscribers_; ///< @brief Used for the observation message filters std::vector > observation_notifiers_; ///< @brief Used to make sure that transforms are available for each sensor - std::vector > observation_buffers_; ///< @brief Used to store observations from various sensors - std::vector > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles - std::vector > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles + std::vector > observation_buffers_; ///< @brief Used to store observations from various sensors + std::vector > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstacles + std::vector > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles // Used only for testing purposes - std::vector static_clearing_observations_, static_marking_observations_; + std::vector static_clearing_observations_, static_marking_observations_; bool rolling_window_; - dynamic_reconfigure::Server * dsrv_; + dynamic_reconfigure::Server * dsrv_; int combination_method_; private: - void reconfigureCB(costmap_2d::ObstaclePluginConfig & config, uint32_t level); + void reconfigureCB(nav2_costmap_2d::ObstaclePluginConfig & config, uint32_t level); }; -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_OBSTACLE_LAYER_H_ +#endif // nav2_costmap_2d_OBSTACLE_LAYER_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/static_layer.h b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.h similarity index 86% rename from src/libs/costmap_2d/include/costmap_2d/static_layer.h rename to nav2_costmap_2d/include/nav2_costmap_2d/static_layer.h index f81f79960c..aacd2c48b0 100644 --- a/src/libs/costmap_2d/include/costmap_2d/static_layer.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.h @@ -35,19 +35,19 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_STATIC_LAYER_H_ -#define COSTMAP_2D_STATIC_LAYER_H_ +#ifndef nav2_costmap_2d_STATIC_LAYER_H_ +#define nav2_costmap_2d_STATIC_LAYER_H_ #include -#include -#include -//#include +#include +#include +//#include //#include #include #include #include -namespace costmap_2d +namespace nav2_costmap_2d { class StaticLayer : public CostmapLayer @@ -64,7 +64,7 @@ class StaticLayer : public CostmapLayer double * min_y, double * max_x, double * max_y); - virtual void updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, + virtual void updateCosts(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); virtual void matchSize(); @@ -80,7 +80,7 @@ class StaticLayer : public CostmapLayer void incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update); //TODO(bpwilcox): Replace dynamic_reconfigure functionality - //void reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level); + //void reconfigureCB(nav2_costmap_2d::GenericPluginConfig & config, uint32_t level); unsigned char interpretValue(unsigned char value); @@ -99,9 +99,9 @@ class StaticLayer : public CostmapLayer unsigned char lethal_threshold_, unknown_cost_value_; //TODO(bpwilcox): Replace dynamic_reconfigure functionality - //dynamic_reconfigure::Server * dsrv_; + //dynamic_reconfigure::Server * dsrv_; }; -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_STATIC_LAYER_H_ +#endif // nav2_costmap_2d_STATIC_LAYER_H_ diff --git a/src/libs/costmap_2d/include/costmap_2d/testing_helper.h b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.h similarity index 50% rename from src/libs/costmap_2d/include/costmap_2d/testing_helper.h rename to nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.h index 957808a821..759ec53691 100644 --- a/src/libs/costmap_2d/include/costmap_2d/testing_helper.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.h @@ -1,18 +1,18 @@ -#ifndef COSTMAP_2D_TESTING_HELPER_H -#define COSTMAP_2D_TESTING_HELPER_H +#ifndef nav2_costmap_2d_TESTING_HELPER_H +#define nav2_costmap_2d_TESTING_HELPER_H #include "rclcpp/rclcpp.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include const double MAX_Z(1.0); -void setValues(costmap_2d::Costmap2D & costmap, const unsigned char * map) +void setValues(nav2_costmap_2d::Costmap2D & costmap, const unsigned char * map) { int index = 0; for (int i = 0; i < costmap.getSizeInCellsY(); i++) { @@ -25,15 +25,15 @@ void setValues(costmap_2d::Costmap2D & costmap, const unsigned char * map) char printableCost(unsigned char cost) { switch (cost) { - case costmap_2d::NO_INFORMATION: return '?'; - case costmap_2d::LETHAL_OBSTACLE: return 'L'; - case costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I'; - case costmap_2d::FREE_SPACE: return '.'; + case nav2_costmap_2d::NO_INFORMATION: return '?'; + case nav2_costmap_2d::LETHAL_OBSTACLE: return 'L'; + case nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE: return 'I'; + case nav2_costmap_2d::FREE_SPACE: return '.'; default: return '0' + (unsigned char) (10 * cost / 255); } } -void printMap(costmap_2d::Costmap2D & costmap) +void printMap(nav2_costmap_2d::Costmap2D & costmap) { printf("map:\n"); for (int i = 0; i < costmap.getSizeInCellsY(); i++) { @@ -44,7 +44,7 @@ void printMap(costmap_2d::Costmap2D & costmap) } } -unsigned int countValues(costmap_2d::Costmap2D & costmap, unsigned char value, bool equal = true) +unsigned int countValues(nav2_costmap_2d::Costmap2D & costmap, unsigned char value, bool equal = true) { unsigned int count = 0; for (int i = 0; i < costmap.getSizeInCellsY(); i++) { @@ -58,23 +58,23 @@ unsigned int countValues(costmap_2d::Costmap2D & costmap, unsigned char value, b return count; } -void addStaticLayer(costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf) +void addStaticLayer(nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf) { - costmap_2d::StaticLayer * slayer = new costmap_2d::StaticLayer(); - layers.addPlugin(std::shared_ptr(slayer)); + nav2_costmap_2d::StaticLayer * slayer = new nav2_costmap_2d::StaticLayer(); + layers.addPlugin(std::shared_ptr(slayer)); slayer->initialize(&layers, "static", &tf); } -costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap & layers, +nav2_costmap_2d::ObstacleLayer * addObstacleLayer(nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf) { - costmap_2d::ObstacleLayer * olayer = new costmap_2d::ObstacleLayer(); + nav2_costmap_2d::ObstacleLayer * olayer = new nav2_costmap_2d::ObstacleLayer(); olayer->initialize(&layers, "obstacles", &tf); - layers.addPlugin(std::shared_ptr(olayer)); + layers.addPlugin(std::shared_ptr(olayer)); return olayer; } -void addObservation(costmap_2d::ObstacleLayer * olayer, double x, double y, double z = 0.0, +void addObservation(nav2_costmap_2d::ObstacleLayer * olayer, double x, double y, double z = 0.0, double ox = 0.0, double oy = 0.0, double oz = MAX_Z) { sensor_msgs::PointCloud2 cloud; @@ -93,19 +93,19 @@ void addObservation(costmap_2d::ObstacleLayer * olayer, double x, double y, doub p.y = oy; p.z = oz; - costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0 + nav2_costmap_2d::Observation obs(p, cloud, 100.0, 100.0); // obstacle range = raytrace range = 100.0 olayer->addStaticObservation(obs, true, true); } -costmap_2d::InflationLayer * addInflationLayer(costmap_2d::LayeredCostmap & layers, +nav2_costmap_2d::InflationLayer * addInflationLayer(nav2_costmap_2d::LayeredCostmap & layers, tf2_ros::Buffer & tf) { - costmap_2d::InflationLayer * ilayer = new costmap_2d::InflationLayer(); + nav2_costmap_2d::InflationLayer * ilayer = new nav2_costmap_2d::InflationLayer(); ilayer->initialize(&layers, "inflation", &tf); - std::shared_ptr ipointer(ilayer); + std::shared_ptr ipointer(ilayer); layers.addPlugin(ipointer); return ilayer; } -#endif // COSTMAP_2D_TESTING_HELPER_H +#endif // nav2_costmap_2d_TESTING_HELPER_H diff --git a/src/libs/costmap_2d/include/costmap_2d/voxel_layer.h b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.h similarity index 87% rename from src/libs/costmap_2d/include/costmap_2d/voxel_layer.h rename to nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.h index 7025768237..5b6a04ccc4 100644 --- a/src/libs/costmap_2d/include/costmap_2d/voxel_layer.h +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.h @@ -35,14 +35,14 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#ifndef COSTMAP_2D_VOXEL_LAYER_H_ -#define COSTMAP_2D_VOXEL_LAYER_H_ +#ifndef nav2_costmap_2d_VOXEL_LAYER_H_ +#define nav2_costmap_2d_VOXEL_LAYER_H_ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -51,11 +51,11 @@ #include #include #include -#include -#include +#include +#include #include -namespace costmap_2d +namespace nav2_costmap_2d { class VoxelLayer : public ObstacleLayer @@ -89,14 +89,14 @@ class VoxelLayer : public ObstacleLayer virtual void resetMaps(); private: - void reconfigureCB(costmap_2d::VoxelPluginConfig & config, uint32_t level); + void reconfigureCB(nav2_costmap_2d::VoxelPluginConfig & config, uint32_t level); void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info); - virtual void raytraceFreespace(const costmap_2d::Observation & clearing_observation, + virtual void raytraceFreespace(const nav2_costmap_2d::Observation & clearing_observation, double * min_x, double * min_y, double * max_x, double * max_y); - dynamic_reconfigure::Server * voxel_dsrv_; + dynamic_reconfigure::Server * voxel_dsrv_; bool publish_voxel_; ros::Publisher voxel_pub_; @@ -156,6 +156,6 @@ class VoxelLayer : public ObstacleLayer } }; -} // namespace costmap_2d +} // namespace nav2_costmap_2d -#endif // COSTMAP_2D_VOXEL_LAYER_H_ +#endif // nav2_costmap_2d_VOXEL_LAYER_H_ diff --git a/src/libs/costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch similarity index 67% rename from src/libs/costmap_2d/launch/example.launch rename to nav2_costmap_2d/launch/example.launch index 87cd2ea120..ac089abfba 100644 --- a/src/libs/costmap_2d/launch/example.launch +++ b/nav2_costmap_2d/launch/example.launch @@ -9,13 +9,13 @@ - + - - + + diff --git a/src/libs/costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml similarity index 100% rename from src/libs/costmap_2d/launch/example_params.yaml rename to nav2_costmap_2d/launch/example_params.yaml diff --git a/src/libs/costmap_2d/msg/VoxelGrid.msg b/nav2_costmap_2d/msg/VoxelGrid.msg similarity index 100% rename from src/libs/costmap_2d/msg/VoxelGrid.msg rename to nav2_costmap_2d/msg/VoxelGrid.msg diff --git a/src/libs/costmap_2d/package.xml b/nav2_costmap_2d/package.xml similarity index 82% rename from src/libs/costmap_2d/package.xml rename to nav2_costmap_2d/package.xml index 5f09f45cfd..6f0aa3f4ec 100644 --- a/src/libs/costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -1,7 +1,7 @@ - costmap_2d + nav2_costmap_2d 0.1.0 This package provides an implementation of a 2D costmap that takes in sensor @@ -12,17 +12,11 @@ costmap, rolling window based costmaps, and parameter based subscription to and configuration of sensor topics. - Eitan Marder-Eppstein - David V. Lu!! - Dave Hershberger - contradict@gmail.com David V. Lu!! Michael Ferguson Aaron Hoy - email="oregon.robotics.team@intel.com">Oregon Robotics Team - Oregon Robotics Team + Steve Macenski BSD - http://wiki.ros.org/costmap_2d ament_cmake rosidl_default_generators diff --git a/src/libs/costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp similarity index 91% rename from src/libs/costmap_2d/plugins/inflation_layer.cpp rename to nav2_costmap_2d/plugins/inflation_layer.cpp index ac58e749b1..a2cbbc3552 100644 --- a/src/libs/costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -36,18 +36,18 @@ * David V. Lu!! *********************************************************************/ #include -#include -#include -#include +#include +#include +#include #include -PLUGINLIB_EXPORT_CLASS(costmap_2d::InflationLayer, costmap_2d::Layer) +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::InflationLayer, nav2_costmap_2d::Layer) -using costmap_2d::LETHAL_OBSTACLE; -using costmap_2d::INSCRIBED_INFLATED_OBSTACLE; -using costmap_2d::NO_INFORMATION; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; -namespace costmap_2d +namespace nav2_costmap_2d { InflationLayer::InflationLayer() @@ -87,7 +87,7 @@ void InflationLayer::onInitialize() // TODO(bpwilcox): Resolve dynamic reconfigure dependencies /* - dynamic_reconfigure::Server::CallbackType cb = std::bind( + dynamic_reconfigure::Server::CallbackType cb = std::bind( &InflationLayer::reconfigureCB, this, _1, _2); if (dsrv_ != NULL){ @@ -96,7 +96,7 @@ void InflationLayer::onInitialize() } else { - dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name_)); + dsrv_ = new dynamic_reconfigure::Server(ros::NodeHandle("~/" + name_)); dsrv_->setCallback(cb); } */ } @@ -108,7 +108,7 @@ void InflationLayer::onInitialize() } // TODO(bpwilcox): Resolve dynamic reconfigure dependencies /* -void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level) +void InflationLayer::reconfigureCB(nav2_costmap_2d::InflationPluginConfig &config, uint32_t level) { setInflationParameters(config.inflation_radius, config.cost_scaling_factor); @@ -122,7 +122,7 @@ void InflationLayer::reconfigureCB(costmap_2d::InflationPluginConfig &config, ui void InflationLayer::matchSize() { std::unique_lock lock(*inflation_access_); - costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap(); + nav2_costmap_2d::Costmap2D * costmap = layered_costmap_->getCostmap(); resolution_ = costmap->getResolution(); cell_inflation_radius_ = cellDistance(inflation_radius_); computeCaches(); @@ -175,12 +175,12 @@ void InflationLayer::onFootprintChanged() need_reinflation_ = true; RCLCPP_DEBUG(rclcpp::get_logger( - "costmap_2d"), "InflationLayer::onFootprintChanged(): num footprint points: %lu," + "nav2_costmap_2d"), "InflationLayer::onFootprintChanged(): num footprint points: %lu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_); } -void InflationLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, +void InflationLayer::updateCosts(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { @@ -190,7 +190,7 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, } // make sure the inflation list is empty at the beginning of the cycle (should always be true) - RCLCPP_FATAL_EXPRESSION(rclcpp::get_logger("costmap_2d"), + RCLCPP_FATAL_EXPRESSION(rclcpp::get_logger("nav2_costmap_2d"), !inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation"); unsigned char * master_array = master_grid.getCharMap(); @@ -198,12 +198,12 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, if (seen_ == NULL) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), "InflationLayer::updateCosts(): seen_ array is NULL"); + "nav2_costmap_2d"), "InflationLayer::updateCosts(): seen_ array is NULL"); seen_size_ = size_x * size_y; seen_ = new bool[seen_size_]; } else if (seen_size_ != size_x * size_y) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), "InflationLayer::updateCosts(): seen_ array size is wrong"); + "nav2_costmap_2d"), "InflationLayer::updateCosts(): seen_ array size is wrong"); delete[] seen_; seen_size_ = size_x * size_y; seen_ = new bool[seen_size_]; @@ -388,4 +388,4 @@ void InflationLayer::setInflationParameters(double inflation_radius, double cost } } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp similarity index 95% rename from src/libs/costmap_2d/plugins/obstacle_layer.cpp rename to nav2_costmap_2d/plugins/obstacle_layer.cpp index bba20ef34e..6054e4c43d 100644 --- a/src/libs/costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -35,23 +35,23 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include -#include +#include +#include #include #include #include -PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer) +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::ObstacleLayer, nav2_costmap_2d::Layer) -using costmap_2d::NO_INFORMATION; -using costmap_2d::LETHAL_OBSTACLE; -using costmap_2d::FREE_SPACE; +using nav2_costmap_2d::NO_INFORMATION; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::FREE_SPACE; -using costmap_2d::ObservationBuffer; -using costmap_2d::Observation; +using nav2_costmap_2d::ObservationBuffer; +using nav2_costmap_2d::Observation; -namespace costmap_2d +namespace nav2_costmap_2d { void ObstacleLayer::onInitialize() @@ -220,8 +220,8 @@ void ObstacleLayer::onInitialize() void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle & nh) { - dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = std::bind( + dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = std::bind( &ObstacleLayer::reconfigureCB, this, _1, _2); dsrv_->setCallback(cb); } @@ -232,7 +232,7 @@ ObstacleLayer::~ObstacleLayer() delete dsrv_; } } -void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig & config, uint32_t level) +void ObstacleLayer::reconfigureCB(nav2_costmap_2d::ObstaclePluginConfig & config, uint32_t level) { enabled_ = config.enabled; footprint_clearing_enabled_ = config.footprint_clearing_enabled; @@ -412,7 +412,7 @@ void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot } } -void ObstacleLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, +void ObstacleLayer::updateCosts(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { @@ -421,7 +421,7 @@ void ObstacleLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, } if (footprint_clearing_enabled_) { - setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); + setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE); } switch (combination_method_) { @@ -436,7 +436,7 @@ void ObstacleLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, } } -void ObstacleLayer::addStaticObservation(costmap_2d::Observation & obs, bool marking, bool clearing) +void ObstacleLayer::addStaticObservation(nav2_costmap_2d::Observation & obs, bool marking, bool clearing) { if (marking) { static_marking_observations_.push_back(obs); @@ -610,4 +610,4 @@ void ObstacleLayer::reset() activate(); } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp similarity index 88% rename from src/libs/costmap_2d/plugins/static_layer.cpp rename to nav2_costmap_2d/plugins/static_layer.cpp index ccf2e5e021..b061327363 100644 --- a/src/libs/costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -36,20 +36,20 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include -#include +#include +#include #include #include #include -PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer) +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) -using costmap_2d::NO_INFORMATION; -using costmap_2d::LETHAL_OBSTACLE; -using costmap_2d::FREE_SPACE; +using nav2_costmap_2d::NO_INFORMATION; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::FREE_SPACE; -namespace costmap_2d +namespace nav2_costmap_2d { StaticLayer::StaticLayer() {enabled_ = true;} @@ -65,7 +65,7 @@ void StaticLayer::onInitialize() { auto nh = rclcpp::Node::make_shared(name_); rclcpp::Node::SharedPtr g_nh; - g_nh = rclcpp::Node::make_shared("costmap_2d_static"); + g_nh = rclcpp::Node::make_shared("nav2_costmap_2d_static"); auto parameters_client = std::make_shared(nh); current_ = true; @@ -94,7 +94,7 @@ void StaticLayer::onInitialize() //if (map_sub_.getTopic() != ros::names::resolve(map_topic)) { // we'll subscribe to the latched topic that the map server uses - RCLCPP_INFO(rclcpp::get_logger("costmap_2d"), "Requesting the map..."); + RCLCPP_INFO(rclcpp::get_logger("nav2_costmap_2d"), "Requesting the map..."); rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; custom_qos_profile.depth = 1; custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; @@ -110,11 +110,11 @@ void StaticLayer::onInitialize() } RCLCPP_INFO(rclcpp::get_logger( - "costmap_2d"), "Received a %d X %d map at %f m/pix", getSizeInCellsX(), + "nav2_costmap_2d"), "Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution()); if (subscribe_to_updates_) { - RCLCPP_INFO(rclcpp::get_logger("costmap_2d"), "Subscribing to updates"); + RCLCPP_INFO(rclcpp::get_logger("nav2_costmap_2d"), "Subscribing to updates"); map_update_sub_ = g_nh->create_subscription( map_topic + "_updates", std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1), custom_qos_profile); @@ -128,13 +128,13 @@ void StaticLayer::onInitialize() delete dsrv_; } */ - //dsrv_ = new dynamic_reconfigure::Server(nh); - //dynamic_reconfigure::Server::CallbackType cb = std::bind( + //dsrv_ = new dynamic_reconfigure::Server(nh); + //dynamic_reconfigure::Server::CallbackType cb = std::bind( // &StaticLayer::reconfigureCB, this, _1, _2); //dsrv_->setCallback(cb); } -/* void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig & config, uint32_t level) +/* void StaticLayer::reconfigureCB(nav2_costmap_2d::GenericPluginConfig & config, uint32_t level) { if (config.enabled != enabled_) { enabled_ = config.enabled; @@ -178,7 +178,7 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_ unsigned int size_x = new_map->info.width, size_y = new_map->info.height; RCLCPP_DEBUG(rclcpp::get_logger( - "costmap_2d"), "Received a %d X %d map at %f m/pix", size_x, size_y, + "nav2_costmap_2d"), "Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution); // resize costmap if size, resolution or origin do not match @@ -192,7 +192,7 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_ { // Update the size of the layered costmap (and all layers, including this one) RCLCPP_INFO(rclcpp::get_logger( - "costmap_2d"), "Resizing costmap to %d X %d at %f m/pix", size_x, size_y, + "nav2_costmap_2d"), "Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x, @@ -205,7 +205,7 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_ { // only update the size of the costmap stored locally in this layer RCLCPP_INFO(rclcpp::get_logger( - "costmap_2d"), "Resizing static layer to %d X %d at %f m/pix", size_x, size_y, + "nav2_costmap_2d"), "Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution); resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x, new_map->info.origin.position.y); @@ -234,7 +234,7 @@ void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_ // shutdown the map subscrber if firt_map_only_ flag is on if (first_map_only_) { RCLCPP_INFO(rclcpp::get_logger( - "costmap_2d"), "Shutting down the map subscriber. first_map_only flag is on"); + "nav2_costmap_2d"), "Shutting down the map subscriber. first_map_only flag is on"); // TODO(bpwilcox): Resolve shutdown of ros2 subscription //map_sub_.shutdown(); } @@ -307,7 +307,7 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, has_updated_data_ = false; } -void StaticLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, +void StaticLayer::updateCosts(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { if (!map_received_) { @@ -334,7 +334,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, in try { transform = tf_->lookupTransform(map_frame_, global_frame_, tf2_ros::fromMsg(rclcpp::Time())); } catch (tf2::TransformException ex) { - RCLCPP_ERROR(rclcpp::get_logger("costmap_2d"), "%s", ex.what()); + RCLCPP_ERROR(rclcpp::get_logger("nav2_costmap_2d"), "%s", ex.what()); return; } // Copy map data given proper transformations @@ -361,4 +361,4 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D & master_grid, int min_i, in } } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp similarity index 95% rename from src/libs/costmap_2d/plugins/voxel_layer.cpp rename to nav2_costmap_2d/plugins/voxel_layer.cpp index 5070a1b8ca..411ebc923b 100644 --- a/src/libs/costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -35,21 +35,21 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include +#include #include #include #define VOXEL_BITS 16 -PLUGINLIB_EXPORT_CLASS(costmap_2d::VoxelLayer, costmap_2d::Layer) +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::VoxelLayer, nav2_costmap_2d::Layer) -using costmap_2d::NO_INFORMATION; -using costmap_2d::LETHAL_OBSTACLE; -using costmap_2d::FREE_SPACE; +using nav2_costmap_2d::NO_INFORMATION; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::FREE_SPACE; -using costmap_2d::ObservationBuffer; -using costmap_2d::Observation; +using nav2_costmap_2d::ObservationBuffer; +using nav2_costmap_2d::Observation; -namespace costmap_2d +namespace nav2_costmap_2d { void VoxelLayer::onInitialize() @@ -59,7 +59,7 @@ void VoxelLayer::onInitialize() private_nh.param("publish_voxel_map", publish_voxel_, false); if (publish_voxel_) { - voxel_pub_ = private_nh.advertise("voxel_grid", 1); + voxel_pub_ = private_nh.advertise("voxel_grid", 1); } clearing_endpoints_pub_ = private_nh.advertise("clearing_endpoints", 1); @@ -67,8 +67,8 @@ void VoxelLayer::onInitialize() void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle & nh) { - voxel_dsrv_ = new dynamic_reconfigure::Server(nh); - dynamic_reconfigure::Server::CallbackType cb = std::bind( + voxel_dsrv_ = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = std::bind( &VoxelLayer::reconfigureCB, this, _1, _2); voxel_dsrv_->setCallback(cb); } @@ -80,7 +80,7 @@ VoxelLayer::~VoxelLayer() } } -void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig & config, uint32_t level) +void VoxelLayer::reconfigureCB(nav2_costmap_2d::VoxelPluginConfig & config, uint32_t level) { enabled_ = config.enabled; footprint_clearing_enabled_ = config.footprint_clearing_enabled; @@ -194,7 +194,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, } if (publish_voxel_) { - costmap_2d::VoxelGrid grid_msg; + nav2_costmap_2d::VoxelGrid grid_msg; unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); grid_msg.size_x = voxel_grid_.sizeX(); grid_msg.size_y = voxel_grid_.sizeY(); @@ -449,4 +449,4 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y) delete[] local_voxel_map; } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/array_parser.cpp b/nav2_costmap_2d/src/array_parser.cpp similarity index 98% rename from src/libs/costmap_2d/src/array_parser.cpp rename to nav2_costmap_2d/src/array_parser.cpp index 468e1670a4..4d5ef24bcf 100644 --- a/src/libs/costmap_2d/src/array_parser.cpp +++ b/nav2_costmap_2d/src/array_parser.cpp @@ -34,7 +34,7 @@ #include #include -namespace costmap_2d +namespace nav2_costmap_2d { /** @brief Parse a vector of vector of floats from a string. @@ -102,4 +102,4 @@ std::vector > parseVVF(const std::string & input, std::string return result; } -} // end namespace costmap_2d +} // end namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp similarity index 99% rename from src/libs/costmap_2d/src/costmap_2d.cpp rename to nav2_costmap_2d/src/costmap_2d.cpp index fa5156bbed..32e486c409 100644 --- a/src/libs/costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -35,12 +35,12 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include +#include #include using namespace std; -namespace costmap_2d +namespace nav2_costmap_2d { Costmap2D::Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, double resolution, double origin_x, double origin_y, unsigned char default_value) @@ -473,4 +473,4 @@ bool Costmap2D::saveMap(std::string file_name) return true; } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp similarity index 96% rename from src/libs/costmap_2d/src/costmap_2d_cloud.cpp rename to nav2_costmap_2d/src/costmap_2d_cloud.cpp index 09c9dfe836..ca4b770899 100644 --- a/src/libs/costmap_2d/src/costmap_2d_cloud.cpp +++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include static inline void mapToWorld3D(const unsigned int mx, const unsigned int my, const unsigned int mz, @@ -58,7 +58,7 @@ float g_colors_a[] = {0.0f, 0.5f, 1.0f}; V_Cell g_marked; V_Cell g_unknown; void voxelCallback(const ros::Publisher & pub_marked, const ros::Publisher & pub_unknown, - const costmap_2d::VoxelGridConstPtr & grid) + const nav2_costmap_2d::VoxelGridConstPtr & grid) { if (grid->data.empty()) { ROS_ERROR("Received empty voxel grid"); @@ -183,14 +183,14 @@ void voxelCallback(const ros::Publisher & pub_marked, const ros::Publisher & pub int main(int argc, char ** argv) { - ros::init(argc, argv, "costmap_2d_cloud"); + ros::init(argc, argv, "nav2_costmap_2d_cloud"); ros::NodeHandle n; ROS_DEBUG("Startup"); ros::Publisher pub_marked = n.advertise("voxel_marked_cloud", 2); ros::Publisher pub_unknown = n.advertise("voxel_unknown_cloud", 2); - ros::Subscriber sub = n.subscribe("voxel_grid", 1, std::bind(voxelCallback, pub_marked, pub_unknown, _1)); ros::spin(); diff --git a/src/libs/costmap_2d/src/costmap_2d_markers.cpp b/nav2_costmap_2d/src/costmap_2d_markers.cpp similarity index 94% rename from src/libs/costmap_2d/src/costmap_2d_markers.cpp rename to nav2_costmap_2d/src/costmap_2d_markers.cpp index 078be357a3..f57e71a309 100644 --- a/src/libs/costmap_2d/src/costmap_2d_markers.cpp +++ b/nav2_costmap_2d/src/costmap_2d_markers.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include struct Cell @@ -57,7 +57,7 @@ float g_colors_a[] = {0.0f, 0.5f, 1.0f}; std::string g_marker_ns; V_Cell g_cells; -void voxelCallback(const ros::Publisher & pub, const costmap_2d::VoxelGridConstPtr & grid) +void voxelCallback(const ros::Publisher & pub, const nav2_costmap_2d::VoxelGridConstPtr & grid) { if (grid->data.empty()) { ROS_ERROR("Received empty voxel grid"); @@ -135,14 +135,14 @@ void voxelCallback(const ros::Publisher & pub, const costmap_2d::VoxelGridConstP int main(int argc, char ** argv) { - ros::init(argc, argv, "costmap_2d_markers"); + ros::init(argc, argv, "nav2_costmap_2d_markers"); ros::NodeHandle n; ROS_DEBUG("Startup"); ros::Publisher pub = n.advertise("visualization_marker", 1); ros::Subscriber sub = - n.subscribe("voxel_grid", 1, std::bind(voxelCallback, pub, _1)); + n.subscribe("voxel_grid", 1, std::bind(voxelCallback, pub, _1)); g_marker_ns = n.resolveName("voxel_grid"); ros::spin(); diff --git a/src/libs/costmap_2d/src/costmap_2d_node.cpp b/nav2_costmap_2d/src/costmap_2d_node.cpp similarity index 95% rename from src/libs/costmap_2d/src/costmap_2d_node.cpp rename to nav2_costmap_2d/src/costmap_2d_node.cpp index 4f4a5a1797..cf90ff86bf 100644 --- a/src/libs/costmap_2d/src/costmap_2d_node.cpp +++ b/nav2_costmap_2d/src/costmap_2d_node.cpp @@ -36,7 +36,7 @@ * David V. Lu!! *********************************************************************/ #include -#include +#include #include int main(int argc, char ** argv) @@ -47,7 +47,7 @@ int main(int argc, char ** argv) std::string name = "costmap"; tf2_ros::Buffer buffer(tf2::durationFromSec(10)); tf2_ros::TransformListener tf(buffer); - costmap_2d::Costmap2DROS lcr(name, buffer); + nav2_costmap_2d::Costmap2DROS lcr(name, buffer); rclcpp::spin(node); diff --git a/src/libs/costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp similarity index 97% rename from src/libs/costmap_2d/src/costmap_2d_publisher.cpp rename to nav2_costmap_2d/src/costmap_2d_publisher.cpp index df426fb3fe..0b452c2f76 100644 --- a/src/libs/costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -35,10 +35,10 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include -#include +#include +#include -namespace costmap_2d +namespace nav2_costmap_2d { char * Costmap2DPublisher::cost_translation_table_ = NULL; @@ -170,4 +170,4 @@ void Costmap2DPublisher::publishCostmap() y0_ = costmap_->getSizeInCellsY(); } -} // end namespace costmap_2d +} // end namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp similarity index 92% rename from src/libs/costmap_2d/src/costmap_2d_ros.cpp rename to nav2_costmap_2d/src/costmap_2d_ros.cpp index 7530c44551..28e266238d 100644 --- a/src/libs/costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -35,8 +35,8 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include -#include +#include +#include #include #include #include @@ -46,7 +46,7 @@ using namespace std; -namespace costmap_2d +namespace nav2_costmap_2d { void move_parameter(rclcpp::Node::SharedPtr old_h, rclcpp::Node::SharedPtr new_h, std::string name, @@ -78,7 +78,7 @@ Costmap2DROS::Costmap2DROS(const std::string & name, tf2_ros::Buffer & tf) robot_stopped_(false), map_update_thread_(NULL), last_publish_(0), - plugin_loader_("costmap_2d", "costmap_2d::Layer"), + plugin_loader_("nav2_costmap_2d", "nav2_costmap_2d::Layer"), publisher_(NULL), publish_cycle_(1), footprint_padding_(0.0) @@ -109,7 +109,7 @@ Costmap2DROS::Costmap2DROS(const std::string & name, tf2_ros::Buffer & tf) rclcpp::spin_some(private_nh); if (last_error + rclcpp::Duration(5.0) < clock.now()) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s", robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); last_error = clock.now(); @@ -140,7 +140,7 @@ Costmap2DROS::Costmap2DROS(const std::string & name, tf2_ros::Buffer & tf) for (int32_t i = 0; i < my_list.size(); ++i) { std::string pname = static_cast(my_list[i]["name"]); std::string type = static_cast(my_list[i]["type"]); - RCLCPP_INFO(rclcpp::get_logger("costmap_2d"), "Using plugin \"%s\"", pname.c_str()); + RCLCPP_INFO(rclcpp::get_logger("nav2_costmap_2d"), "Using plugin \"%s\"", pname.c_str()); std::shared_ptr plugin = plugin_loader_.createSharedInstance(type); layered_costmap_->addPlugin(plugin); @@ -213,7 +213,7 @@ Costmap2DROS::~Costmap2DROS() void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh) { - RCLCPP_INFO(rclcpp::get_logger("costmap_2d"), "Loading from pre-hydro parameter style"); + RCLCPP_INFO(rclcpp::get_logger("nav2_costmap_2d"), "Loading from pre-hydro parameter style"); bool flag; std::string s; std::vector plugins; @@ -228,7 +228,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh) flag = parameters_client->get_parameter("static_map"); if (flag) { map["name"] = XmlRpc::XmlRpcValue("static_layer"); - map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer"); + map["type"] = XmlRpc::XmlRpcValue("nav2_costmap_2d::StaticLayer"); super_map.setStruct(&map); plugins.push_back(super_map); @@ -247,7 +247,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh) s = parameters_client->get_parameter("map_type"); if (s == "voxel") { map["name"] = XmlRpc::XmlRpcValue("obstacle_layer"); - map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer"); + map["type"] = XmlRpc::XmlRpcValue("nav2_costmap_2d::VoxelLayer"); super_map.setStruct(&map); plugins.push_back(super_map); @@ -260,7 +260,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh) } } else { map["name"] = XmlRpc::XmlRpcValue("obstacle_layer"); - map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer"); + map["type"] = XmlRpc::XmlRpcValue("nav2_costmap_2d::ObstacleLayer"); super_map.setStruct(&map); plugins.push_back(super_map); } @@ -284,7 +284,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh) move_parameter(nh, inflation, "cost_scaling_factor"); move_parameter(nh, inflation, "inflation_radius"); map["name"] = XmlRpc::XmlRpcValue("inflation_layer"); - map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer"); + map["type"] = XmlRpc::XmlRpcValue("nav2_costmap_2d::InflationLayer"); super_map.setStruct(&map); plugins.push_back(super_map); @@ -298,7 +298,7 @@ void Costmap2DROS::resetOldParameters(rclcpp::Node::SharedPtr nh) } // TODO(bpwilcox): resolve dynamic reconfigure dependencies /* -void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level) +void Costmap2DROS::reconfigureCB(nav2_costmap_2d::Costmap2DConfig &config, uint32_t level) { transform_tolerance_ = config.transform_tolerance; if (map_update_thread_ != NULL) @@ -344,8 +344,8 @@ void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t l // TODO(bpwilcox): resolve dynamic reconfigure dependencies /* -void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, - const costmap_2d::Costmap2DConfig &old_config) +void Costmap2DROS::readFootprintFromConfig(const nav2_costmap_2d::Costmap2DConfig &new_config, + const nav2_costmap_2d::Costmap2DConfig &old_config) { // Only change the footprint if footprint or robot_radius has // changed. Otherwise we might overwrite a footprint sent on a @@ -366,7 +366,7 @@ void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &ne } else { - RCLCPP_ERROR(rclcpp::get_logger("costmap_2d"),"Invalid footprint string from dynamic reconfigure"); + RCLCPP_ERROR(rclcpp::get_logger("nav2_costmap_2d"),"Invalid footprint string from dynamic reconfigure"); } } else @@ -396,7 +396,7 @@ void Costmap2DROS::movementCB() geometry_msgs::msg::PoseStamped new_pose; if (!getRobotPose(new_pose)) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), "Could not get robot pose, cancelling reconfiguration"); + "nav2_costmap_2d"), "Could not get robot pose, cancelling reconfiguration"); robot_stopped_ = false; } // make sure that the robot is not moving @@ -436,7 +436,7 @@ void Costmap2DROS::mapUpdateLoop(double frequency) start_t = start.tv_sec + double(start.tv_usec) / 1e6; end_t = end.tv_sec + double(end.tv_usec) / 1e6; t_diff = end_t - start_t; - RCLCPP_DEBUG(rclcpp::get_logger("costmap_2d"), "Map update time: %.9f", t_diff); + RCLCPP_DEBUG(rclcpp::get_logger("nav2_costmap_2d"), "Map update time: %.9f", t_diff); if (publish_cycle_.nanoseconds() > 0 && layered_costmap_->isInitialized()) { unsigned int x0, y0, xn, yn; layered_costmap_->getBounds(&x0, &xn, &y0, &yn); @@ -453,7 +453,7 @@ void Costmap2DROS::mapUpdateLoop(double frequency) if (r.period() > tf2::durationFromSec(1 / frequency)) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency, r.period()); @@ -570,15 +570,15 @@ bool Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) c tf_.transform(robot_pose, global_pose, global_frame_); } catch (tf2::LookupException & ex) { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), "No Transform available Error looking up robot pose: %s\n", ex.what()); + "nav2_costmap_2d"), "No Transform available Error looking up robot pose: %s\n", ex.what()); return false; } catch (tf2::ConnectivityException & ex) { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), "Connectivity Error looking up robot pose: %s\n", ex.what()); + "nav2_costmap_2d"), "Connectivity Error looking up robot pose: %s\n", ex.what()); return false; } catch (tf2::ExtrapolationException & ex) { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), "Extrapolation Error looking up robot pose: %s\n", ex.what()); + "nav2_costmap_2d"), "Extrapolation Error looking up robot pose: %s\n", ex.what()); return false; } // check global_pose timeout @@ -588,7 +588,7 @@ bool Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) c tf2::timeToSec(tf2_ros::fromMsg(global_pose.header.stamp)) > transform_tolerance_) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f", tf2::timeToSec(tf2_ros::fromMsg(current_time)), tf2::timeToSec(tf2_ros::fromMsg(global_pose.header.stamp)), transform_tolerance_); @@ -612,4 +612,4 @@ const padded_footprint_, oriented_footprint); } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp similarity index 84% rename from src/libs/costmap_2d/src/costmap_layer.cpp rename to nav2_costmap_2d/src/costmap_layer.cpp index 35c14c8497..7947b19a63 100644 --- a/src/libs/costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -1,6 +1,6 @@ -#include +#include -namespace costmap_2d +namespace nav2_costmap_2d { void CostmapLayer::touch(double x, double y, double * min_x, double * min_y, double * max_x, @@ -45,7 +45,7 @@ void CostmapLayer::useExtraBounds(double * min_x, double * min_y, double * max_x has_extra_bounds_ = false; } -void CostmapLayer::updateWithMax(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, +void CostmapLayer::updateWithMax(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { @@ -73,7 +73,7 @@ void CostmapLayer::updateWithMax(costmap_2d::Costmap2D & master_grid, int min_i, } } -void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D & master_grid, int min_i, +void CostmapLayer::updateWithTrueOverwrite(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) @@ -93,7 +93,7 @@ void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D & master_grid, } } -void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, +void CostmapLayer::updateWithOverwrite(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { @@ -114,7 +114,7 @@ void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D & master_grid, int } } -void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D & master_grid, int min_i, int min_j, +void CostmapLayer::updateWithAddition(nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j) { @@ -137,8 +137,8 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D & master_grid, int m master_array[it] = costmap_[it]; } else { int sum = old_cost + costmap_[it]; - if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { - master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; + if (sum >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { + master_array[it] = nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1; } else { master_array[it] = sum; } @@ -147,4 +147,4 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D & master_grid, int m } } } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/costmap_math.cpp b/nav2_costmap_2d/src/costmap_math.cpp similarity index 98% rename from src/libs/costmap_2d/src/costmap_math.cpp rename to nav2_costmap_2d/src/costmap_math.cpp index 9995d456ad..a9797e8f65 100644 --- a/src/libs/costmap_2d/src/costmap_math.cpp +++ b/nav2_costmap_2d/src/costmap_math.cpp @@ -27,7 +27,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1) { diff --git a/src/libs/costmap_2d/src/footprint.cpp b/nav2_costmap_2d/src/footprint.cpp similarity index 95% rename from src/libs/costmap_2d/src/footprint.cpp rename to nav2_costmap_2d/src/footprint.cpp index f7d7b88bb9..54e77f4c90 100644 --- a/src/libs/costmap_2d/src/footprint.cpp +++ b/nav2_costmap_2d/src/footprint.cpp @@ -28,12 +28,12 @@ */ #include -#include -#include -#include +#include +#include +#include #include -namespace costmap_2d +namespace nav2_costmap_2d { void calculateMinAndMaxDistances(const std::vector & footprint, @@ -170,16 +170,16 @@ bool makeFootprintFromString(const std::string & footprint_string, if (error != "") { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), "Error parsing footprint parameter: '%s'", error.c_str()); + "nav2_costmap_2d"), "Error parsing footprint parameter: '%s'", error.c_str()); RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), " Footprint string was '%s'.", footprint_string.c_str()); + "nav2_costmap_2d"), " Footprint string was '%s'.", footprint_string.c_str()); return false; } // convert vvf into points. if (vvf.size() < 3) { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "You must specify at least three points for the robot footprint, reverting to previous footprint."); return false; } @@ -193,7 +193,7 @@ bool makeFootprintFromString(const std::string & footprint_string, footprint.push_back(point); } else { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.", int(vvf[i].size())); return false; @@ -280,7 +280,7 @@ double getNumberFromXMLRPC(XmlRpc::XmlRpcValue & value, const std::string & full { std::string & value_string = value; RCLCPP_FATAL(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "Values in the footprint specification (param %s) must be numbers. Found value %s.", full_param_name.c_str(), value_string.c_str()); throw std::runtime_error("Values in the footprint specification must be numbers"); @@ -297,7 +297,7 @@ std::vector makeFootprintFromXMLRPC( footprint_xmlrpc.size() < 3) { RCLCPP_FATAL(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "The footprint must be specified as list of lists on the parameter server, %s was specified as %s", full_param_name.c_str(), std::string(footprint_xmlrpc).c_str()); throw std::runtime_error( @@ -315,7 +315,7 @@ std::vector makeFootprintFromXMLRPC( point.size() != 2) { RCLCPP_FATAL(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", full_param_name.c_str()); @@ -332,4 +332,4 @@ std::vector makeFootprintFromXMLRPC( return footprint; } -} // end namespace costmap_2d +} // end namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/layer.cpp b/nav2_costmap_2d/src/layer.cpp similarity index 95% rename from src/libs/costmap_2d/src/layer.cpp rename to nav2_costmap_2d/src/layer.cpp index 2aae263026..3f9f545a91 100644 --- a/src/libs/costmap_2d/src/layer.cpp +++ b/nav2_costmap_2d/src/layer.cpp @@ -27,9 +27,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "costmap_2d/layer.h" +#include "nav2_costmap_2d/layer.h" -namespace costmap_2d +namespace nav2_costmap_2d { Layer::Layer() @@ -53,4 +53,4 @@ const std::vector & Layer::getFootprint() const return layered_costmap_->getFootprint(); } -} // end namespace costmap_2d +} // end namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp similarity index 92% rename from src/libs/costmap_2d/src/layered_costmap.cpp rename to nav2_costmap_2d/src/layered_costmap.cpp index a6c9e89e3f..cb0a40cfe2 100644 --- a/src/libs/costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -35,8 +35,8 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ -#include -#include +#include +#include #include #include #include @@ -44,7 +44,7 @@ using std::vector; -namespace costmap_2d +namespace nav2_costmap_2d { LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown) @@ -124,7 +124,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_); if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " + "nav2_costmap_2d"), "Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but " "is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s", prev_minx, prev_miny, prev_maxx, prev_maxy, minx_, miny_, maxx_, maxy_, @@ -142,7 +142,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1); RCLCPP_DEBUG(rclcpp::get_logger( - "costmap_2d"), "Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); + "nav2_costmap_2d"), "Updating area x: [%d, %d] y: [%d, %d]", x0, xn, y0, yn); if (xn < x0 || yn < y0) { return; @@ -177,7 +177,7 @@ bool LayeredCostmap::isCurrent() void LayeredCostmap::setFootprint(const std::vector & footprint_spec) { footprint_ = footprint_spec; - costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); + nav2_costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_); for (vector >::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) @@ -186,4 +186,4 @@ void LayeredCostmap::setFootprint(const std::vector & } } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp similarity index 96% rename from src/libs/costmap_2d/src/observation_buffer.cpp rename to nav2_costmap_2d/src/observation_buffer.cpp index df16aa5117..c664bdd0a7 100644 --- a/src/libs/costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -34,7 +34,7 @@ * * Author: Eitan Marder-Eppstein *********************************************************************/ -#include +#include #include #include @@ -43,7 +43,7 @@ using namespace std; using namespace tf2; -namespace costmap_2d +namespace nav2_costmap_2d { ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate, @@ -75,7 +75,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) tf2::durationFromSec(tf_tolerance_), &tf_error)) { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), "Transform between %s and %s with tolerance %.2f failed: %s.", + "nav2_costmap_2d"), "Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(), global_frame_.c_str(), tf_tolerance_, tf_error.c_str()); return false; @@ -99,7 +99,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame); } catch (TransformException & ex) { RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), "TF Error attempting to transform an observation from %s to %s: %s", + "nav2_costmap_2d"), "TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(), new_global_frame.c_str(), ex.what()); return false; @@ -182,7 +182,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) // if an exception occurs, we need to remove the empty observation from the list observation_list_.pop_front(); RCLCPP_ERROR(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); @@ -240,7 +240,7 @@ bool ObservationBuffer::isCurrent() const bool current = (clock_.now() - last_updated_).toSec() <= expected_update_rate_.toSec(); if (!current) { RCLCPP_WARN(rclcpp::get_logger( - "costmap_2d"), + "nav2_costmap_2d"), "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.", topic_name_.c_str(), (clock_.now() - last_updated_).toSec(), expected_update_rate_.toSec()); @@ -252,4 +252,4 @@ void ObservationBuffer::resetLastUpdated() { last_updated_ = clock_.now(); } -} // namespace costmap_2d +} // namespace nav2_costmap_2d diff --git a/src/libs/costmap_2d/test/TenByTen.pgm b/nav2_costmap_2d/test/TenByTen.pgm similarity index 100% rename from src/libs/costmap_2d/test/TenByTen.pgm rename to nav2_costmap_2d/test/TenByTen.pgm diff --git a/src/libs/costmap_2d/test/TenByTen.yaml b/nav2_costmap_2d/test/TenByTen.yaml similarity index 100% rename from src/libs/costmap_2d/test/TenByTen.yaml rename to nav2_costmap_2d/test/TenByTen.yaml diff --git a/src/libs/costmap_2d/test/array_parser_test.cpp b/nav2_costmap_2d/test/array_parser_test.cpp similarity index 97% rename from src/libs/costmap_2d/test/array_parser_test.cpp rename to nav2_costmap_2d/test/array_parser_test.cpp index 7ac2a3900f..cec4f13635 100644 --- a/src/libs/costmap_2d/test/array_parser_test.cpp +++ b/nav2_costmap_2d/test/array_parser_test.cpp @@ -29,9 +29,9 @@ #include -#include "costmap_2d/array_parser.h" +#include "nav2_costmap_2d/array_parser.h" -using namespace costmap_2d; +using namespace nav2_costmap_2d; TEST(array_parser, basic_operation) { diff --git a/src/libs/costmap_2d/test/costmap_params.yaml b/nav2_costmap_2d/test/costmap_params.yaml similarity index 100% rename from src/libs/costmap_2d/test/costmap_params.yaml rename to nav2_costmap_2d/test/costmap_params.yaml diff --git a/src/libs/costmap_2d/test/costmap_tester.cpp b/nav2_costmap_2d/test/costmap_tester.cpp similarity index 84% rename from src/libs/costmap_2d/test/costmap_tester.cpp rename to nav2_costmap_2d/test/costmap_tester.cpp index c49a98c562..a334fc683d 100644 --- a/src/libs/costmap_2d/test/costmap_tester.cpp +++ b/nav2_costmap_2d/test/costmap_tester.cpp @@ -36,29 +36,29 @@ *********************************************************************/ #include #include -#include -#include +#include +#include #include -namespace costmap_2d { +namespace nav2_costmap_2d { class CostmapTester : public testing::Test { public: CostmapTester(tf2_ros::Buffer& tf); void checkConsistentCosts(); - void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y); - void compareCells(costmap_2d::Costmap2D& costmap, + void compareCellToNeighbors(nav2_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y); + void compareCells(nav2_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny); virtual void TestBody(){} private: - costmap_2d::Costmap2DROS costmap_ros_; + nav2_costmap_2d::Costmap2DROS costmap_ros_; }; CostmapTester::CostmapTester(tf2_ros::Buffer& tf): costmap_ros_("test_costmap", tf){} void CostmapTester::checkConsistentCosts(){ - costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap(); + nav2_costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap(); //get a copy of the costmap contained by our ros wrapper costmap->saveMap("costmap_test.pgm"); @@ -71,7 +71,7 @@ void CostmapTester::checkConsistentCosts(){ } } -void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){ +void CostmapTester::compareCellToNeighbors(nav2_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){ //we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable for(int offset_x = -1; offset_x <= 1; ++offset_x){ for(int offset_y = -1; offset_y <= 1; ++offset_y){ @@ -87,18 +87,18 @@ void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsig } //for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect -void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){ +void CostmapTester::compareCells(nav2_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){ double cell_distance = hypot(static_cast(x-nx), static_cast(y-ny)); unsigned char cell_cost = costmap.getCost(x, y); unsigned char neighbor_cost = costmap.getCost(nx, ny); - if(cell_cost == costmap_2d::LETHAL_OBSTACLE){ + if(cell_cost == nav2_costmap_2d::LETHAL_OBSTACLE){ //if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance); - EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE)); + EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == nav2_costmap_2d::FREE_SPACE)); } - else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + else if(cell_cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ //the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1; unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance); @@ -108,12 +108,12 @@ void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, ROS_ERROR("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny); costmap.saveMap("failing_costmap.pgm"); } - EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE)); + EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == nav2_costmap_2d::FREE_SPACE)); } } }; -costmap_2d::CostmapTester* map_tester = NULL; +nav2_costmap_2d::CostmapTester* map_tester = NULL; TEST(CostmapTester, checkConsistentCosts){ map_tester->checkConsistentCosts(); @@ -134,7 +134,7 @@ int main(int argc, char** argv){ tf2_ros::Buffer tf(ros::Duration(10)); tf2_ros::TransformListener tfl(tf); - map_tester = new costmap_2d::CostmapTester(tf); + map_tester = new nav2_costmap_2d::CostmapTester(tf); double wait_time; private_nh.param("wait_time", wait_time, 30.0); diff --git a/src/libs/costmap_2d/test/footprint_tests.cpp b/nav2_costmap_2d/test/footprint_tests.cpp similarity index 99% rename from src/libs/costmap_2d/test/footprint_tests.cpp rename to nav2_costmap_2d/test/footprint_tests.cpp index 356b9b671b..4c8ccfb5bb 100644 --- a/src/libs/costmap_2d/test/footprint_tests.cpp +++ b/nav2_costmap_2d/test/footprint_tests.cpp @@ -40,7 +40,7 @@ #include #include -using namespace costmap_2d; +using namespace nav2_costmap_2d; tf2_ros::TransformListener* tfl_; tf2_ros::Buffer* tf_; diff --git a/src/libs/costmap_2d/test/footprint_tests.launch b/nav2_costmap_2d/test/footprint_tests.launch similarity index 91% rename from src/libs/costmap_2d/test/footprint_tests.launch rename to nav2_costmap_2d/test/footprint_tests.launch index 30edbdd4d2..7a546e1c44 100644 --- a/src/libs/costmap_2d/test/footprint_tests.launch +++ b/nav2_costmap_2d/test/footprint_tests.launch @@ -1,6 +1,6 @@ - + diff --git a/src/libs/costmap_2d/test/inflation_tests.cpp b/nav2_costmap_2d/test/inflation_tests.cpp similarity index 90% rename from src/libs/costmap_2d/test/inflation_tests.cpp rename to nav2_costmap_2d/test/inflation_tests.cpp index cd3d8d057e..32a477ee07 100644 --- a/src/libs/costmap_2d/test/inflation_tests.cpp +++ b/nav2_costmap_2d/test/inflation_tests.cpp @@ -34,15 +34,15 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -using namespace costmap_2d; +using namespace nav2_costmap_2d; using geometry_msgs::Point; std::vector setRadii(LayeredCostmap& layers, double length, double width, double inflation_radius) @@ -202,17 +202,17 @@ TEST(costmap, testCostFunctionCorrectness){ for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){ // To the right - ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); // To the left - ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); // Down - ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); // Up - ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map->getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); } // Verify the normalized cost attenuates as expected @@ -237,7 +237,7 @@ TEST(costmap, testCostFunctionCorrectness){ for(unsigned int i = 0; i < 100; i++) for(unsigned int j = 0; j < 100; j++) - ASSERT_EQ(map->getCost(i, j), costmap_2d::FREE_SPACE);*/ + ASSERT_EQ(map->getCost(i, j), nav2_costmap_2d::FREE_SPACE);*/ } /** @@ -301,7 +301,7 @@ TEST(costmap, testInflation){ unsigned int x, y; map.indexToCells(ind, x, y); ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true); - ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(x, y) == nav2_costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); }*/ addObservation(olayer, 0, 0, 0.4); @@ -360,8 +360,8 @@ TEST(costmap, testInflation2){ Costmap2D* costmap = layers.getCostmap(); //printMap(*costmap); - ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); - ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(costmap->getCost(2, 3), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(costmap->getCost(3, 3), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); } /** diff --git a/src/libs/costmap_2d/test/inflation_tests.launch b/nav2_costmap_2d/test/inflation_tests.launch similarity index 52% rename from src/libs/costmap_2d/test/inflation_tests.launch rename to nav2_costmap_2d/test/inflation_tests.launch index 1c2b2fe731..d08b67fbf2 100644 --- a/src/libs/costmap_2d/test/inflation_tests.launch +++ b/nav2_costmap_2d/test/inflation_tests.launch @@ -1,6 +1,6 @@ - - + + diff --git a/src/libs/costmap_2d/test/module_tests.cpp b/nav2_costmap_2d/test/module_tests.cpp similarity index 91% rename from src/libs/costmap_2d/test/module_tests.cpp rename to nav2_costmap_2d/test/module_tests.cpp index 52a7df941c..c537b5a98b 100644 --- a/src/libs/costmap_2d/test/module_tests.cpp +++ b/nav2_costmap_2d/test/module_tests.cpp @@ -32,12 +32,12 @@ * Test harness for Costmap2D */ -#include -#include +#include +#include #include #include -using namespace costmap_2d; +using namespace nav2_costmap_2d; const unsigned char MAP_10_BY_10_CHAR[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -92,7 +92,7 @@ TEST(costmap, testResetForStaticMap){ std::vector staticMap; for(unsigned int i=0; i<10; i++){ for(unsigned int j=0; j<10; j++){ - staticMap.push_back(costmap_2d::LETHAL_OBSTACLE); + staticMap.push_back(nav2_costmap_2d::LETHAL_OBSTACLE); } } @@ -148,7 +148,7 @@ TEST(costmap, testResetForStaticMap){ int hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ hitCount++; } } @@ -159,7 +159,7 @@ TEST(costmap, testResetForStaticMap){ hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE) + if(map.getCost(i, j) != nav2_costmap_2d::LETHAL_OBSTACLE) hitCount++; } } @@ -172,7 +172,7 @@ TEST(costmap, testResetForStaticMap){ hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE) hitCount++; } } @@ -211,17 +211,17 @@ TEST(costmap, testCostFunctionCorrectness){ for(unsigned int i = 0; i <= (unsigned int)ceil(ROBOT_RADIUS * 5.0); i++){ // To the right - ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map.getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50 + i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); // To the left - ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map.getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50 - i, 50) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); // Down - ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map.getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50, 50 + i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); // Up - ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); - ASSERT_EQ(map.getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(50, 50 - i) >= nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); } // Verify the normalized cost attenuates as expected @@ -246,7 +246,7 @@ TEST(costmap, testCostFunctionCorrectness){ for(unsigned int i = 0; i < 100; i++) for(unsigned int j = 0; j < 100; j++) - ASSERT_EQ(map.getCost(i, j), costmap_2d::FREE_SPACE); + ASSERT_EQ(map.getCost(i, j), nav2_costmap_2d::FREE_SPACE); } char printableCost( unsigned char cost ) @@ -299,7 +299,7 @@ TEST(costmap, testWaveInterference){ printf("map:\n"); for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) != costmap_2d::FREE_SPACE){ + if(map.getCost(i, j) != nav2_costmap_2d::FREE_SPACE){ update_count++; } printf("%c", printableCost( map.getCost( i, j ))); @@ -452,7 +452,7 @@ TEST(costmap, testRaytracing){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ lethal_count++; } } @@ -514,7 +514,7 @@ TEST(costmap, testInflationShouldNotCreateUnknowns){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::NO_INFORMATION){ + if(map.getCost(i, j) == nav2_costmap_2d::NO_INFORMATION){ unknown_count++; } } @@ -546,7 +546,7 @@ TEST(costmap, testStaticMap){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ occupiedCells.push_back(map.getIndex(i, j)); } } @@ -634,7 +634,7 @@ TEST(costmap, testDynamicObstacles){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ ids.push_back(map.getIndex(i, j)); } } @@ -676,7 +676,7 @@ TEST(costmap, testMultipleAdditions){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ ids.push_back(map.getIndex(i, j)); } } @@ -717,7 +717,7 @@ TEST(costmap, testZThreshold){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ ids.push_back(map.getIndex(i, j)); } } @@ -739,7 +739,7 @@ TEST(costmap, testInflation){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ occupiedCells.push_back(map.getIndex(i, j)); } } @@ -759,7 +759,7 @@ TEST(costmap, testInflation){ unsigned int x, y; map.indexToCells(ind, x, y); ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true); - ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); + ASSERT_EQ(map.getCost(x, y) == nav2_costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true); } // Set an obstacle at the origin and observe insertions for it and its neighbors @@ -783,7 +783,7 @@ TEST(costmap, testInflation){ occupiedCells.clear(); for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ occupiedCells.push_back(map.getIndex(i, j)); } } @@ -814,7 +814,7 @@ TEST(costmap, testInflation){ occupiedCells.clear(); for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ occupiedCells.push_back(map.getIndex(i, j)); } } @@ -844,9 +844,9 @@ TEST(costmap, testInflation){ map.updateWorld(0, 0, obsBuf2, empty); - ASSERT_EQ(map.getCost(1, 9), costmap_2d::LETHAL_OBSTACLE); - ASSERT_EQ(map.getCost(0, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); - ASSERT_EQ(map.getCost(2, 9), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(map.getCost(1, 9), nav2_costmap_2d::LETHAL_OBSTACLE); + ASSERT_EQ(map.getCost(0, 9), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(map.getCost(2, 9), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); // Add an obstacle and verify that it over-writes its inflated status pcl::PointCloud c3; @@ -866,7 +866,7 @@ TEST(costmap, testInflation){ map.updateWorld(0, 0, obsBuf3, empty); - ASSERT_EQ(map.getCost(0, 9), costmap_2d::LETHAL_OBSTACLE); + ASSERT_EQ(map.getCost(0, 9), nav2_costmap_2d::LETHAL_OBSTACLE); } /** @@ -900,8 +900,8 @@ TEST(costmap, testInflation2){ map.updateWorld(0, 0, obsBuf, obsBuf); - ASSERT_EQ(map.getCost(3, 2), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); - ASSERT_EQ(map.getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(map.getCost(3, 2), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); + ASSERT_EQ(map.getCost(3, 3), nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE); } /** @@ -923,7 +923,7 @@ TEST(costmap, testInflation3){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ ids.push_back(map.getIndex(i, j)); } } @@ -951,7 +951,7 @@ TEST(costmap, testInflation3){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) != costmap_2d::FREE_SPACE){ + if(map.getCost(i, j) != nav2_costmap_2d::FREE_SPACE){ ids.push_back(map.getIndex(i, j)); } } @@ -962,7 +962,7 @@ TEST(costmap, testInflation3){ ids.clear(); for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE || map.getCost(i, j) == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){ ids.push_back(map.getIndex(i, j)); } } @@ -976,7 +976,7 @@ TEST(costmap, testInflation3){ ids.clear(); for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) != costmap_2d::FREE_SPACE){ + if(map.getCost(i, j) != nav2_costmap_2d::FREE_SPACE){ ids.push_back(map.getIndex(i, j)); } } @@ -1014,7 +1014,7 @@ TEST(costmap, testRaytracing2){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ obstacles.push_back(map.getIndex(i, j)); } } @@ -1027,7 +1027,7 @@ TEST(costmap, testRaytracing2){ obstacles.clear(); for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ obstacles.push_back(map.getIndex(i, j)); } } diff --git a/src/libs/costmap_2d/test/obstacle_tests.cpp b/nav2_costmap_2d/test/obstacle_tests.cpp similarity index 90% rename from src/libs/costmap_2d/test/obstacle_tests.cpp rename to nav2_costmap_2d/test/obstacle_tests.cpp index 2b2fe376b9..bd26bcb35c 100644 --- a/src/libs/costmap_2d/test/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/obstacle_tests.cpp @@ -32,14 +32,14 @@ * Test harness for ObstacleLayer for Costmap2D */ -#include -#include -#include -#include +#include +#include +#include +#include #include #include -using namespace costmap_2d; +using namespace nav2_costmap_2d; /* * For reference, the static map looks like this: @@ -143,7 +143,7 @@ TEST(costmap, testRaytracing2){ // Should thus be the same ASSERT_EQ(with_static, obs_after); // If 21 are filled, 79 should be free - ASSERT_EQ(79, countValues(*(layers.getCostmap()), costmap_2d::FREE_SPACE)); + ASSERT_EQ(79, countValues(*(layers.getCostmap()), nav2_costmap_2d::FREE_SPACE)); } /** @@ -170,9 +170,9 @@ TEST(costmap, testWaveInterference){ // 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free // <0,0> is footprint and is free //printMap(*costmap); - ASSERT_EQ(3, countValues(*costmap, costmap_2d::LETHAL_OBSTACLE)); - ASSERT_EQ(92, countValues(*costmap, costmap_2d::NO_INFORMATION)); - ASSERT_EQ(5, countValues(*costmap, costmap_2d::FREE_SPACE)); + ASSERT_EQ(3, countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE)); + ASSERT_EQ(92, countValues(*costmap, nav2_costmap_2d::NO_INFORMATION)); + ASSERT_EQ(5, countValues(*costmap, nav2_costmap_2d::FREE_SPACE)); } /** @@ -193,7 +193,7 @@ TEST(costmap, testZThreshold){ layers.updateMap(0,0,0); Costmap2D* costmap = layers.getCostmap(); - ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 1); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 1); } @@ -216,10 +216,10 @@ TEST(costmap, testDynamicObstacles){ Costmap2D* costmap = layers.getCostmap(); // Should now have 1 insertion and no deletions - ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); // Repeating the call - we should see no insertions or deletions - ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 21); } @@ -239,7 +239,7 @@ TEST(costmap, testMultipleAdditions){ Costmap2D* costmap = layers.getCostmap(); //printMap(*costmap); - ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 20); + ASSERT_EQ(countValues(*costmap, nav2_costmap_2d::LETHAL_OBSTACLE), 20); } diff --git a/nav2_costmap_2d/test/obstacle_tests.launch b/nav2_costmap_2d/test/obstacle_tests.launch new file mode 100644 index 0000000000..4ddac24c75 --- /dev/null +++ b/nav2_costmap_2d/test/obstacle_tests.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/nav2_costmap_2d/test/simple_driving_test.xml b/nav2_costmap_2d/test/simple_driving_test.xml new file mode 100644 index 0000000000..5351f51069 --- /dev/null +++ b/nav2_costmap_2d/test/simple_driving_test.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/src/libs/costmap_2d/test/static_tests.cpp b/nav2_costmap_2d/test/static_tests.cpp similarity index 94% rename from src/libs/costmap_2d/test/static_tests.cpp rename to nav2_costmap_2d/test/static_tests.cpp index 714ceba8ec..8bde38c7a5 100644 --- a/src/libs/costmap_2d/test/static_tests.cpp +++ b/nav2_costmap_2d/test/static_tests.cpp @@ -32,16 +32,16 @@ * Test harness for StaticMap Layer for Costmap2D */ -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -using namespace costmap_2d; +using namespace nav2_costmap_2d; /** @@ -52,7 +52,7 @@ TEST(costmap, testResetForStaticMap){ std::vector staticMap; for(unsigned int i=0; i<10; i++){ for(unsigned int j=0; j<10; j++){ - staticMap.push_back(costmap_2d::LETHAL_OBSTACLE); + staticMap.push_back(nav2_costmap_2d::LETHAL_OBSTACLE); } } @@ -108,7 +108,7 @@ TEST(costmap, testResetForStaticMap){ int hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ hitCount++; } } @@ -119,7 +119,7 @@ TEST(costmap, testResetForStaticMap){ hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE) + if(map.getCost(i, j) != nav2_costmap_2d::LETHAL_OBSTACLE) hitCount++; } } @@ -132,7 +132,7 @@ TEST(costmap, testResetForStaticMap){ hitCount = 0; for(unsigned int i=0; i < 10; ++i){ for(unsigned int j=0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE) hitCount++; } } @@ -266,7 +266,7 @@ TEST(costmap, testStaticMap){ for(unsigned int i = 0; i < 10; ++i){ for(unsigned int j = 0; j < 10; ++j){ - if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){ + if(map.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE){ occupiedCells.push_back(map.getIndex(i, j)); } } diff --git a/nav2_costmap_2d/test/static_tests.launch b/nav2_costmap_2d/test/static_tests.launch new file mode 100644 index 0000000000..f3f458918b --- /dev/null +++ b/nav2_costmap_2d/test/static_tests.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/src/mission_execution/nav2_mission_executor/CHANGELOG.rst b/nav2_costmap_world_model/CHANGELOG.rst similarity index 100% rename from src/mission_execution/nav2_mission_executor/CHANGELOG.rst rename to nav2_costmap_world_model/CHANGELOG.rst diff --git a/src/world_model/nav2_costmap_world_model/CMakeLists.txt b/nav2_costmap_world_model/CMakeLists.txt similarity index 92% rename from src/world_model/nav2_costmap_world_model/CMakeLists.txt rename to nav2_costmap_world_model/CMakeLists.txt index 6dbbbde877..b582518b38 100644 --- a/src/world_model/nav2_costmap_world_model/CMakeLists.txt +++ b/nav2_costmap_world_model/CMakeLists.txt @@ -16,8 +16,7 @@ find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_tasks) find_package(nav2_util) -find_package(nav2_libs_msgs) -find_package(nav2_world_model_msgs) +find_package(nav2_msgs) find_package(nav_msgs) include_directories( @@ -41,8 +40,7 @@ set(dependencies std_msgs nav2_tasks nav2_util - nav2_libs_msgs - nav2_world_model_msgs + nav2_msgs tf2_ros tf2 nav_msgs diff --git a/src/mission_execution/nav2_mission_execution_msgs/README.md b/nav2_costmap_world_model/README.md similarity index 100% rename from src/mission_execution/nav2_mission_execution_msgs/README.md rename to nav2_costmap_world_model/README.md diff --git a/src/mission_execution/nav2_mission_executor/README.md b/nav2_costmap_world_model/doc/.gitignore similarity index 100% rename from src/mission_execution/nav2_mission_executor/README.md rename to nav2_costmap_world_model/doc/.gitignore diff --git a/src/world_model/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp b/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp similarity index 79% rename from src/world_model/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp rename to nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp index 6338f67a1d..848a74d099 100644 --- a/src/world_model/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp +++ b/nav2_costmap_world_model/include/nav2_costmap_world_model/costmap_world_model.hpp @@ -20,8 +20,8 @@ #include #include "rclcpp/rclcpp.hpp" #include "nav2_util/costmap.hpp" -#include "nav2_libs_msgs/msg/costmap.hpp" -#include "nav2_world_model_msgs/srv/get_costmap.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/srv/get_costmap.hpp" #include "nav2_tasks/map_service_client.hpp" namespace nav2_costmap_world_model @@ -35,16 +35,16 @@ class CostmapWorldModel : public rclcpp::Node private: // Server for providing a costmap - rclcpp::Service::SharedPtr costmapServer_; + rclcpp::Service::SharedPtr costmapServer_; // TODO(orduno): Define a server for scoring trajectories - // rclcpp::Service::SharedPtr scoringServer_; + // rclcpp::Service::SharedPtr scoringServer_; // TODO(orduno): Define a client for getting the static map - // rclcpp::Client::SharedPtr mapClient_; + // rclcpp::Client::SharedPtr mapClient_; // TODO(orduno): Alternatively, obtain from a latched topic - // rclcpp::Subscription::SharedPtr mapSub_; + // rclcpp::Subscription::SharedPtr mapSub_; // TODO(orduno): Define a task for handling trajectory scoring // std::unique_ptr scorer; diff --git a/src/world_model/nav2_costmap_world_model/package.xml b/nav2_costmap_world_model/package.xml similarity index 81% rename from src/world_model/nav2_costmap_world_model/package.xml rename to nav2_costmap_world_model/package.xml index c390d5606a..d67f6716bd 100644 --- a/src/world_model/nav2_costmap_world_model/package.xml +++ b/nav2_costmap_world_model/package.xml @@ -13,14 +13,12 @@ nav2_tasks nav2_tasks nav2_util - nav2_util_msgs - nav2_world_model_msgs + nav2_msgs rclcpp nav2_tasks + nav2_msgs nav2_tasks - nav2_util_msgs - nav2_world_model_msgs ament_lint_common ament_lint_auto diff --git a/src/world_model/nav2_costmap_world_model/src/costmap_world_model.cpp b/nav2_costmap_world_model/src/costmap_world_model.cpp similarity index 87% rename from src/world_model/nav2_costmap_world_model/src/costmap_world_model.cpp rename to nav2_costmap_world_model/src/costmap_world_model.cpp index 3283d0b573..55abe7121c 100644 --- a/src/world_model/nav2_costmap_world_model/src/costmap_world_model.cpp +++ b/nav2_costmap_world_model/src/costmap_world_model.cpp @@ -31,8 +31,8 @@ CostmapWorldModel::CostmapWorldModel(const string & name) auto costmap_service_callback = [this]( const std::shared_ptr/*request_header*/, - const std::shared_ptr request, - const std::shared_ptr response) -> void + const std::shared_ptr request, + const std::shared_ptr response) -> void { RCLCPP_INFO( this->get_logger(), "CostmapWorldModel::CostmapWorldModel:Incoming costmap request"); @@ -40,7 +40,7 @@ CostmapWorldModel::CostmapWorldModel(const string & name) }; // Create a service that will use the callback function to handle requests. - costmapServer_ = create_service(name + "_GetCostmap", + costmapServer_ = create_service(name + "_GetCostmap", costmap_service_callback); // Get the current map from the map server diff --git a/src/world_model/nav2_costmap_world_model/src/main.cpp b/nav2_costmap_world_model/src/main.cpp similarity index 100% rename from src/world_model/nav2_costmap_world_model/src/main.cpp rename to nav2_costmap_world_model/src/main.cpp diff --git a/src/navigation/nav2_bt_navigator/CHANGELOG.rst b/nav2_costmap_world_model/test/.gitignore similarity index 100% rename from src/navigation/nav2_bt_navigator/CHANGELOG.rst rename to nav2_costmap_world_model/test/.gitignore diff --git a/src/navigation/nav2_simple_navigator/CHANGELOG.rst b/nav2_dijkstra_planner/CHANGELOG.rst similarity index 100% rename from src/navigation/nav2_simple_navigator/CHANGELOG.rst rename to nav2_dijkstra_planner/CHANGELOG.rst diff --git a/src/planning/nav2_dijkstra_planner/CMakeLists.txt b/nav2_dijkstra_planner/CMakeLists.txt similarity index 88% rename from src/planning/nav2_dijkstra_planner/CMakeLists.txt rename to nav2_dijkstra_planner/CMakeLists.txt index c5dd732b36..70d7c3850e 100644 --- a/src/planning/nav2_dijkstra_planner/CMakeLists.txt +++ b/nav2_dijkstra_planner/CMakeLists.txt @@ -16,9 +16,7 @@ find_package(std_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(nav2_tasks REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav2_planning_msgs REQUIRED) -find_package(nav2_libs_msgs REQUIRED) -find_package(nav2_world_model_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) include_directories( @@ -44,9 +42,7 @@ set(dependencies visualization_msgs nav2_tasks nav2_util - nav2_planning_msgs - nav2_libs_msgs - nav2_world_model_msgs + nav2_msgs nav_msgs ) diff --git a/src/navigation/nav2_bt_navigator/README.md b/nav2_dijkstra_planner/README.md similarity index 100% rename from src/navigation/nav2_bt_navigator/README.md rename to nav2_dijkstra_planner/README.md diff --git a/src/planning/nav2_dijkstra_planner/doc/..gitignore b/nav2_dijkstra_planner/doc/..gitignore similarity index 100% rename from src/planning/nav2_dijkstra_planner/doc/..gitignore rename to nav2_dijkstra_planner/doc/..gitignore diff --git a/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp b/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp similarity index 91% rename from src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp rename to nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp index 41923f36ad..fe6e1f0f0a 100644 --- a/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp +++ b/nav2_dijkstra_planner/include/nav2_dijkstra_planner/dijkstra_planner.hpp @@ -21,8 +21,8 @@ #include #include "nav2_tasks/compute_path_to_pose_task.hpp" +#include "nav2_msgs/msg/costmap.hpp" #include "nav2_tasks/costmap_service_client.hpp" -#include "nav2_libs_msgs/msg/costmap.hpp" #include "nav2_dijkstra_planner/navfn.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/point.hpp" @@ -46,7 +46,7 @@ class DijkstraPlanner : public nav2_tasks::ComputePathToPoseTaskServer bool makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav2_planning_msgs::msg::Path & plan); + nav2_msgs::msg::Path & plan); // Compute the navigation function given a seed point in the world to start from bool computePotential(const geometry_msgs::msg::Point & world_point); @@ -54,7 +54,7 @@ class DijkstraPlanner : public nav2_tasks::ComputePathToPoseTaskServer // Compute a plan to a goal from a potential - must call computePotential first bool getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav2_planning_msgs::msg::Path & plan); + nav2_msgs::msg::Path & plan); // Compute the potential, or navigation cost, at a given point in the world // - must call computePotential first @@ -86,14 +86,14 @@ class DijkstraPlanner : public nav2_tasks::ComputePathToPoseTaskServer // Request costmap from world model void getCostmap( - nav2_libs_msgs::msg::Costmap & costmap, const std::string layer = "master", + nav2_msgs::msg::Costmap & costmap, const std::string layer = "master", const std::chrono::milliseconds waitTime = std::chrono::milliseconds(100)); // Print costmap to terminal - void printCostmap(const nav2_libs_msgs::msg::Costmap & costmap); + void printCostmap(const nav2_msgs::msg::Costmap & costmap); // Publish a path for visualization purposes - void publishPlan(const nav2_planning_msgs::msg::Path & path); + void publishPlan(const nav2_msgs::msg::Path & path); void publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::SharedPtr & endpoints); // Planner based on ROS1 NavFn algorithm @@ -107,7 +107,7 @@ class DijkstraPlanner : public nav2_tasks::ComputePathToPoseTaskServer rclcpp::Publisher::SharedPtr plan_marker_publisher_; // The costmap to use - nav2_libs_msgs::msg::Costmap costmap_; + nav2_msgs::msg::Costmap costmap_; // The global frame of the costmap std::string global_frame_; diff --git a/src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/navfn.hpp b/nav2_dijkstra_planner/include/nav2_dijkstra_planner/navfn.hpp similarity index 100% rename from src/planning/nav2_dijkstra_planner/include/nav2_dijkstra_planner/navfn.hpp rename to nav2_dijkstra_planner/include/nav2_dijkstra_planner/navfn.hpp diff --git a/src/planning/nav2_dijkstra_planner/package.xml b/nav2_dijkstra_planner/package.xml similarity index 81% rename from src/planning/nav2_dijkstra_planner/package.xml rename to nav2_dijkstra_planner/package.xml index 6c1fd1b463..c0ee56513f 100644 --- a/src/planning/nav2_dijkstra_planner/package.xml +++ b/nav2_dijkstra_planner/package.xml @@ -13,17 +13,13 @@ visualization_msgs nav2_tasks nav2_util - nav2_planning_msgs - nav2_util_msgs - nav2_world_model_msgs + nav2_msgs nav_msgs rclcpp visualization_msgs nav2_tasks - nav2_planning_msgs - nav2_util_msgs - nav2_world_model_msgs + nav2_msgs nav_msgs ament_cmake_cppcheck diff --git a/src/planning/nav2_dijkstra_planner/src/dijkstra_planner.cpp b/nav2_dijkstra_planner/src/dijkstra_planner.cpp similarity index 97% rename from src/planning/nav2_dijkstra_planner/src/dijkstra_planner.cpp rename to nav2_dijkstra_planner/src/dijkstra_planner.cpp index ab9385d6b8..dc8498eedc 100644 --- a/src/planning/nav2_dijkstra_planner/src/dijkstra_planner.cpp +++ b/nav2_dijkstra_planner/src/dijkstra_planner.cpp @@ -30,12 +30,12 @@ #include "nav2_dijkstra_planner/dijkstra_planner.hpp" #include "nav2_dijkstra_planner/navfn.hpp" #include "nav2_util/costmap.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/srv/get_costmap.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "visualization_msgs/msg/marker.hpp" #include "builtin_interfaces/msg/duration.hpp" -#include "nav2_libs_msgs/msg/costmap.hpp" -#include "nav2_world_model_msgs/srv/get_costmap.hpp" #include "nav_msgs/msg/path.hpp" using namespace std::chrono_literals; @@ -135,7 +135,7 @@ bool DijkstraPlanner::makePlan( const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal, double tolerance, - nav2_planning_msgs::msg::Path & plan) + nav2_msgs::msg::Path & plan) { // clear the plan, just in case plan.poses.clear(); @@ -269,7 +269,7 @@ DijkstraPlanner::computePotential(const geometry_msgs::msg::Point & world_point) bool DijkstraPlanner::getPlanFromPotential( const geometry_msgs::msg::Pose & goal, - nav2_planning_msgs::msg::Path & plan) + nav2_msgs::msg::Path & plan) { // clear the plan, just in case plan.poses.clear(); @@ -404,8 +404,8 @@ DijkstraPlanner::clearRobotCell(unsigned int mx, unsigned int my) void DijkstraPlanner::getCostmap( - nav2_libs_msgs::msg::Costmap & costmap, const std::string /*layer*/, - const std::chrono::milliseconds /*waitTime*/) + nav2_msgs::msg::Costmap & costmap, const std::string /*layer*/, + const std::chrono::milliseconds /*waitTime*/) { // TODO(orduno): explicitly provide specifications for costmap using the costmap on the request, // including master (aggreate) layer @@ -418,7 +418,7 @@ DijkstraPlanner::getCostmap( } void -DijkstraPlanner::printCostmap(const nav2_libs_msgs::msg::Costmap & costmap) +DijkstraPlanner::printCostmap(const nav2_msgs::msg::Costmap & costmap) { std::cout << "Costmap" << std::endl; std::cout << " size: " << @@ -512,7 +512,7 @@ DijkstraPlanner::publishEndpoints(const nav2_tasks::ComputePathToPoseCommand::Sh } void -DijkstraPlanner::publishPlan(const nav2_planning_msgs::msg::Path & path) +DijkstraPlanner::publishPlan(const nav2_msgs::msg::Path & path) { // Publish as a nav1 path msg nav_msgs::msg::Path rviz_path; diff --git a/src/planning/nav2_dijkstra_planner/src/main.cpp b/nav2_dijkstra_planner/src/main.cpp similarity index 100% rename from src/planning/nav2_dijkstra_planner/src/main.cpp rename to nav2_dijkstra_planner/src/main.cpp diff --git a/src/planning/nav2_dijkstra_planner/src/navfn.cpp b/nav2_dijkstra_planner/src/navfn.cpp similarity index 100% rename from src/planning/nav2_dijkstra_planner/src/navfn.cpp rename to nav2_dijkstra_planner/src/navfn.cpp diff --git a/src/navigation/nav2_simple_navigator/README.md b/nav2_dijkstra_planner/test/.gitignore similarity index 100% rename from src/navigation/nav2_simple_navigator/README.md rename to nav2_dijkstra_planner/test/.gitignore diff --git a/src/planning/nav2_astar_planner/CHANGELOG.rst b/nav2_gazebo_localizer/.gitignore similarity index 100% rename from src/planning/nav2_astar_planner/CHANGELOG.rst rename to nav2_gazebo_localizer/.gitignore diff --git a/src/planning/nav2_astar_planner/README.md b/nav2_map_server/.gitignore similarity index 100% rename from src/planning/nav2_astar_planner/README.md rename to nav2_map_server/.gitignore diff --git a/src/planning/nav2_dijkstra_planner/CHANGELOG.rst b/nav2_map_server/CHANGELOG.rst similarity index 100% rename from src/planning/nav2_dijkstra_planner/CHANGELOG.rst rename to nav2_map_server/CHANGELOG.rst diff --git a/src/mapping/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt similarity index 100% rename from src/mapping/nav2_map_server/CMakeLists.txt rename to nav2_map_server/CMakeLists.txt diff --git a/src/planning/nav2_dijkstra_planner/README.md b/nav2_map_server/README.md similarity index 100% rename from src/planning/nav2_dijkstra_planner/README.md rename to nav2_map_server/README.md diff --git a/src/mapping/nav2_map_server/include/nav2_map_server/base_map_server.hpp b/nav2_map_server/include/nav2_map_server/base_map_server.hpp similarity index 100% rename from src/mapping/nav2_map_server/include/nav2_map_server/base_map_server.hpp rename to nav2_map_server/include/nav2_map_server/base_map_server.hpp diff --git a/src/mapping/nav2_map_server/include/nav2_map_server/map_factory.hpp b/nav2_map_server/include/nav2_map_server/map_factory.hpp similarity index 100% rename from src/mapping/nav2_map_server/include/nav2_map_server/map_factory.hpp rename to nav2_map_server/include/nav2_map_server/map_factory.hpp diff --git a/src/mapping/nav2_map_server/include/nav2_map_server/map_reps/map_reps.hpp b/nav2_map_server/include/nav2_map_server/map_reps/map_reps.hpp similarity index 100% rename from src/mapping/nav2_map_server/include/nav2_map_server/map_reps/map_reps.hpp rename to nav2_map_server/include/nav2_map_server/map_reps/map_reps.hpp diff --git a/src/mapping/nav2_map_server/include/nav2_map_server/map_reps/occ_grid_server.hpp b/nav2_map_server/include/nav2_map_server/map_reps/occ_grid_server.hpp similarity index 100% rename from src/mapping/nav2_map_server/include/nav2_map_server/map_reps/occ_grid_server.hpp rename to nav2_map_server/include/nav2_map_server/map_reps/occ_grid_server.hpp diff --git a/src/mapping/nav2_map_server/include/nav2_map_server/map_server_ros.hpp b/nav2_map_server/include/nav2_map_server/map_server_ros.hpp similarity index 100% rename from src/mapping/nav2_map_server/include/nav2_map_server/map_server_ros.hpp rename to nav2_map_server/include/nav2_map_server/map_server_ros.hpp diff --git a/src/mapping/nav2_map_server/package.xml b/nav2_map_server/package.xml similarity index 100% rename from src/mapping/nav2_map_server/package.xml rename to nav2_map_server/package.xml diff --git a/src/mapping/nav2_map_server/src/main.cpp b/nav2_map_server/src/main.cpp similarity index 100% rename from src/mapping/nav2_map_server/src/main.cpp rename to nav2_map_server/src/main.cpp diff --git a/src/mapping/nav2_map_server/src/map_reps/map_factory.cpp b/nav2_map_server/src/map_reps/map_factory.cpp similarity index 100% rename from src/mapping/nav2_map_server/src/map_reps/map_factory.cpp rename to nav2_map_server/src/map_reps/map_factory.cpp diff --git a/src/mapping/nav2_map_server/src/map_reps/occ_grid_server.cpp b/nav2_map_server/src/map_reps/occ_grid_server.cpp similarity index 100% rename from src/mapping/nav2_map_server/src/map_reps/occ_grid_server.cpp rename to nav2_map_server/src/map_reps/occ_grid_server.cpp diff --git a/src/mapping/nav2_map_server/src/map_server_ros.cpp b/nav2_map_server/src/map_server_ros.cpp similarity index 100% rename from src/mapping/nav2_map_server/src/map_server_ros.cpp rename to nav2_map_server/src/map_server_ros.cpp diff --git a/src/mapping/nav2_map_server/test/CMakeLists.txt b/nav2_map_server/test/CMakeLists.txt similarity index 100% rename from src/mapping/nav2_map_server/test/CMakeLists.txt rename to nav2_map_server/test/CMakeLists.txt diff --git a/src/mapping/nav2_map_server/test/component/CMakeLists.txt b/nav2_map_server/test/component/CMakeLists.txt similarity index 100% rename from src/mapping/nav2_map_server/test/component/CMakeLists.txt rename to nav2_map_server/test/component/CMakeLists.txt diff --git a/src/mapping/nav2_map_server/test/component/test_occ_grid_launch.py b/nav2_map_server/test/component/test_occ_grid_launch.py similarity index 100% rename from src/mapping/nav2_map_server/test/component/test_occ_grid_launch.py rename to nav2_map_server/test/component/test_occ_grid_launch.py diff --git a/src/mapping/nav2_map_server/test/component/test_occ_grid_node.cpp b/nav2_map_server/test/component/test_occ_grid_node.cpp similarity index 100% rename from src/mapping/nav2_map_server/test/component/test_occ_grid_node.cpp rename to nav2_map_server/test/component/test_occ_grid_node.cpp diff --git a/src/mapping/nav2_map_server/test/test_constants.cpp b/nav2_map_server/test/test_constants.cpp similarity index 100% rename from src/mapping/nav2_map_server/test/test_constants.cpp rename to nav2_map_server/test/test_constants.cpp diff --git a/src/mapping/nav2_map_server/test/test_constants/test_constants.h b/nav2_map_server/test/test_constants/test_constants.h similarity index 100% rename from src/mapping/nav2_map_server/test/test_constants/test_constants.h rename to nav2_map_server/test/test_constants/test_constants.h diff --git a/nav2_map_server/test/test_launch_files/__pycache__/map_server_node.launch.cpython-35.pyc b/nav2_map_server/test/test_launch_files/__pycache__/map_server_node.launch.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8f7613a6efc28e050edab898dee1f1e709868968 GIT binary patch literal 659 zcmYjOy^ho{5FR_(WOtS2I0`6u0k+vlS5Y8@#8I4tw&1P`MP%Y3n~*q>?W{Q3y|=h0 z;0bsTI=5830u|%zo^p;onZIv5pXd2%wS0B|>+|mb;14`nob-2e%xe;oHiHHrJL()7 z1}%q-13Se6$U=|-$Pn%*Z{`8l>)BsI!*4#3^s&%26Z$lF~~U_C~W};4kd#FgkJjlms=P2qWC6C%+8e9 zU*xUmbAx)1njD?P9KT~RSn;L7Y&iv8)D5;Dtc$2$MCBGjmqUxnnrwda;rb@Oy!!Mb zv1lxTHn*Z})^qJ2%`KaeX|6ltVo`XiCJVQ8m*eCLdtrAjth-9Ns6>lu{}c3zkfZol zr@OAjw5^Jrz$P`iNU8Cv6u!{KXw!{-rAqIAOepo;j3Pg^@U?2tq!X*$Gu|A7oBr0l zEK#8r79Rj|8qW(q4L=rO0ZUjke)^2;b)*;cbJ4NT$U1vAvEH0W>zJ}e#6<6?J@N4Q Uo3@ifgKy8tG7*V{KV?bqAMWt3h5!Hn literal 0 HcmV?d00001 diff --git a/src/mapping/nav2_map_server/test/test_launch_files/map_server_node.launch.py b/nav2_map_server/test/test_launch_files/map_server_node.launch.py similarity index 100% rename from src/mapping/nav2_map_server/test/test_launch_files/map_server_node.launch.py rename to nav2_map_server/test/test_launch_files/map_server_node.launch.py diff --git a/src/mapping/nav2_map_server/test/testmap.bmp b/nav2_map_server/test/testmap.bmp similarity index 100% rename from src/mapping/nav2_map_server/test/testmap.bmp rename to nav2_map_server/test/testmap.bmp diff --git a/src/mapping/nav2_map_server/test/testmap.png b/nav2_map_server/test/testmap.png similarity index 100% rename from src/mapping/nav2_map_server/test/testmap.png rename to nav2_map_server/test/testmap.png diff --git a/src/mapping/nav2_map_server/test/testmap.yaml b/nav2_map_server/test/testmap.yaml similarity index 100% rename from src/mapping/nav2_map_server/test/testmap.yaml rename to nav2_map_server/test/testmap.yaml diff --git a/src/mapping/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt similarity index 100% rename from src/mapping/nav2_map_server/test/unit/CMakeLists.txt rename to nav2_map_server/test/unit/CMakeLists.txt diff --git a/src/mapping/nav2_map_server/test/unit/test_occ_grid.cpp b/nav2_map_server/test/unit/test_occ_grid.cpp similarity index 100% rename from src/mapping/nav2_map_server/test/unit/test_occ_grid.cpp rename to nav2_map_server/test/unit/test_occ_grid.cpp diff --git a/src/planning/nav2_planning_msgs/CHANGELOG.rst b/nav2_mission_executor/CHANGELOG.rst similarity index 100% rename from src/planning/nav2_planning_msgs/CHANGELOG.rst rename to nav2_mission_executor/CHANGELOG.rst diff --git a/src/mission_execution/nav2_mission_executor/CMakeLists.txt b/nav2_mission_executor/CMakeLists.txt similarity index 93% rename from src/mission_execution/nav2_mission_executor/CMakeLists.txt rename to nav2_mission_executor/CMakeLists.txt index 31fe8e4c8c..a00de15913 100644 --- a/src/mission_execution/nav2_mission_executor/CMakeLists.txt +++ b/nav2_mission_executor/CMakeLists.txt @@ -13,7 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) -find_package(nav2_mission_execution_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav2_tasks REQUIRED) include_directories( @@ -36,7 +36,7 @@ set(dependencies rclcpp std_msgs nav2_tasks - nav2_mission_execution_msgs + nav2_msgs ) ament_target_dependencies(${executable_name} diff --git a/src/planning/nav2_planning_msgs/README.md b/nav2_mission_executor/README.md similarity index 100% rename from src/planning/nav2_planning_msgs/README.md rename to nav2_mission_executor/README.md diff --git a/src/world_model/nav2_costmap_world_model/CHANGELOG.rst b/nav2_mission_executor/doc/.gitignore similarity index 100% rename from src/world_model/nav2_costmap_world_model/CHANGELOG.rst rename to nav2_mission_executor/doc/.gitignore diff --git a/src/mission_execution/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp b/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp similarity index 94% rename from src/mission_execution/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp rename to nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp index 66d6342d8d..d2de8b8174 100644 --- a/src/mission_execution/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp +++ b/nav2_mission_executor/include/nav2_mission_executor/mission_executor.hpp @@ -17,6 +17,7 @@ #include #include +#include "nav2_tasks/task_status.hpp" #include "nav2_tasks/execute_mission_task.hpp" #include "nav2_tasks/navigate_to_pose_task.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -43,7 +44,7 @@ class MissionExecutor : public nav2_tasks::ExecuteMissionTaskServer geometry_msgs::msg::PoseStamped::SharedPtr goal_pose_; // Also, for now, publish a mission plan when receiving a goal pose from rviz - rclcpp::Publisher::SharedPtr plan_pub_; + rclcpp::Publisher::SharedPtr plan_pub_; }; } // namespace nav2_mission_execution diff --git a/src/mission_execution/nav2_mission_executor/package.xml b/nav2_mission_executor/package.xml similarity index 87% rename from src/mission_execution/nav2_mission_executor/package.xml rename to nav2_mission_executor/package.xml index fad0ff7535..680d1beaed 100644 --- a/src/mission_execution/nav2_mission_executor/package.xml +++ b/nav2_mission_executor/package.xml @@ -12,11 +12,11 @@ rclcpp std_msgs nav2_tasks - nav2_mission_execution_msgs + nav2_msgs rclcpp nav2_tasks - nav2_mission_execution_msgs + nav2_msgs ament_lint_common ament_lint_auto diff --git a/src/mission_execution/nav2_mission_executor/src/main.cpp b/nav2_mission_executor/src/main.cpp similarity index 100% rename from src/mission_execution/nav2_mission_executor/src/main.cpp rename to nav2_mission_executor/src/main.cpp diff --git a/src/mission_execution/nav2_mission_executor/src/mission_executor.cpp b/nav2_mission_executor/src/mission_executor.cpp similarity index 93% rename from src/mission_execution/nav2_mission_executor/src/mission_executor.cpp rename to nav2_mission_executor/src/mission_executor.cpp index 4072eb55ee..07c5f43f4c 100644 --- a/src/mission_execution/nav2_mission_executor/src/mission_executor.cpp +++ b/nav2_mission_executor/src/mission_executor.cpp @@ -37,7 +37,7 @@ MissionExecutor::MissionExecutor() goal_sub_ = create_subscription("move_base_simple/goal", std::bind(&MissionExecutor::onGoalPoseReceived, this, std::placeholders::_1)); - plan_pub_ = create_publisher( + plan_pub_ = create_publisher( "ExecuteMissionTask_command"); } @@ -52,7 +52,7 @@ MissionExecutor::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::Share RCLCPP_INFO(get_logger(), "MissionExecutor::onGoalPoseReceived"); goal_pose_ = msg; - auto message = nav2_mission_execution_msgs::msg::MissionPlan(); + auto message = nav2_msgs::msg::MissionPlan(); message.mission_plan = "Hello, world!"; RCLCPP_INFO(this->get_logger(), "MissionExecutor::onGoalPoseReceived: publishing a mission plan"); diff --git a/src/world_model/nav2_costmap_world_model/README.md b/nav2_mission_executor/test/.gitignore similarity index 100% rename from src/world_model/nav2_costmap_world_model/README.md rename to nav2_mission_executor/test/.gitignore diff --git a/nav2_msgs/CHANGELOG.rst b/nav2_msgs/CHANGELOG.rst new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_libs_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt similarity index 83% rename from src/libs/nav2_libs_msgs/CMakeLists.txt rename to nav2_msgs/CMakeLists.txt index 7139505603..2056dc9678 100644 --- a/src/libs/nav2_libs_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_libs_msgs) +project(nav2_msgs) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) @@ -19,6 +19,11 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Costmap.msg" "msg/CostmapMetaData.msg" + "msg/Path.msg" + "msg/PathEndPoints.msg" + "msg/MissionPlan.msg" + "msg/TaskStatus.msg" + "srv/GetCostmap.srv" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs ) diff --git a/nav2_msgs/README.md b/nav2_msgs/README.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_libs_msgs/msg/Costmap.msg b/nav2_msgs/msg/Costmap.msg similarity index 100% rename from src/libs/nav2_libs_msgs/msg/Costmap.msg rename to nav2_msgs/msg/Costmap.msg diff --git a/src/libs/nav2_libs_msgs/msg/CostmapMetaData.msg b/nav2_msgs/msg/CostmapMetaData.msg similarity index 100% rename from src/libs/nav2_libs_msgs/msg/CostmapMetaData.msg rename to nav2_msgs/msg/CostmapMetaData.msg diff --git a/src/mission_execution/nav2_mission_execution_msgs/msg/MissionPlan.msg b/nav2_msgs/msg/MissionPlan.msg similarity index 100% rename from src/mission_execution/nav2_mission_execution_msgs/msg/MissionPlan.msg rename to nav2_msgs/msg/MissionPlan.msg diff --git a/src/planning/nav2_planning_msgs/msg/Path.msg b/nav2_msgs/msg/Path.msg similarity index 76% rename from src/planning/nav2_planning_msgs/msg/Path.msg rename to nav2_msgs/msg/Path.msg index ce8ca16e79..5a096b9156 100644 --- a/src/planning/nav2_planning_msgs/msg/Path.msg +++ b/nav2_msgs/msg/Path.msg @@ -1,4 +1,4 @@ std_msgs/Header header # An array of poses that represents a Path for a robot to follow -geometry_msgs/Pose[] poses +geometry_msgs/Pose[] poses \ No newline at end of file diff --git a/src/planning/nav2_planning_msgs/msg/PathEndPoints.msg b/nav2_msgs/msg/PathEndPoints.msg similarity index 93% rename from src/planning/nav2_planning_msgs/msg/PathEndPoints.msg rename to nav2_msgs/msg/PathEndPoints.msg index ebb268ae59..7561cec396 100644 --- a/src/planning/nav2_planning_msgs/msg/PathEndPoints.msg +++ b/nav2_msgs/msg/PathEndPoints.msg @@ -8,4 +8,4 @@ geometry_msgs/Pose goal # If the goal is obstructed, how many meters the planner can # relax the constraint in x and y before failing. -float32 tolerance +float32 tolerance \ No newline at end of file diff --git a/nav2_msgs/msg/TaskStatus.msg b/nav2_msgs/msg/TaskStatus.msg new file mode 100644 index 0000000000..dcac36487e --- /dev/null +++ b/nav2_msgs/msg/TaskStatus.msg @@ -0,0 +1,5 @@ +uint8 SUCCEEDED=0 +uint8 FAILED=1 +uint8 RUNNING=2 +uint8 CANCELED=3 +uint8 result \ No newline at end of file diff --git a/src/planning/nav2_planning_msgs/package.xml b/nav2_msgs/package.xml similarity index 83% rename from src/planning/nav2_planning_msgs/package.xml rename to nav2_msgs/package.xml index 41277c5065..4740c769ad 100644 --- a/src/planning/nav2_planning_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -1,10 +1,11 @@ - nav2_planning_msgs + nav2_msgs 0.1.0 - TODO + Messages and service files for the navigation2 stack Michael Jeronimo + Steve Macenski Apache License 2.0 ament_cmake diff --git a/src/world_model/nav2_world_model_msgs/srv/GetCostmap.srv b/nav2_msgs/srv/GetCostmap.srv similarity index 50% rename from src/world_model/nav2_world_model_msgs/srv/GetCostmap.srv rename to nav2_msgs/srv/GetCostmap.srv index a583755f44..8dddc64a1c 100644 --- a/src/world_model/nav2_world_model_msgs/srv/GetCostmap.srv +++ b/nav2_msgs/srv/GetCostmap.srv @@ -1,6 +1,6 @@ # Get the costmap # Specifications for the requested costmap -nav2_libs_msgs/CostmapMetaData specs +nav2_msgs/CostmapMetaData specs --- -nav2_libs_msgs/Costmap map +nav2_msgs/Costmap map diff --git a/src/planning/nav2_planning_tests/CMakeLists.txt b/nav2_planning_tests/CMakeLists.txt similarity index 87% rename from src/planning/nav2_planning_tests/CMakeLists.txt rename to nav2_planning_tests/CMakeLists.txt index 181b38c9c4..941c75682c 100644 --- a/src/planning/nav2_planning_tests/CMakeLists.txt +++ b/nav2_planning_tests/CMakeLists.txt @@ -17,9 +17,7 @@ if(BUILD_TESTING) find_package(rclcpp REQUIRED) find_package(nav2_tasks REQUIRED) find_package(nav2_util REQUIRED) -find_package(nav2_libs_msgs REQUIRED) -find_package(nav2_planning_msgs REQUIRED) -find_package(nav2_world_model_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(ament_cmake_gtest REQUIRED) @@ -37,9 +35,7 @@ ament_target_dependencies(test_planner_node rclcpp nav2_tasks nav2_util - nav2_libs_msgs - nav2_planning_msgs - nav2_world_model_msgs + nav2_msgs nav_msgs visualization_msgs ) diff --git a/src/planning/nav2_planning_tests/example_result.png b/nav2_planning_tests/example_result.png similarity index 100% rename from src/planning/nav2_planning_tests/example_result.png rename to nav2_planning_tests/example_result.png diff --git a/src/planning/nav2_planning_tests/maps/map.pgm b/nav2_planning_tests/maps/map.pgm similarity index 100% rename from src/planning/nav2_planning_tests/maps/map.pgm rename to nav2_planning_tests/maps/map.pgm diff --git a/src/planning/nav2_planning_tests/maps/map.xcf b/nav2_planning_tests/maps/map.xcf similarity index 100% rename from src/planning/nav2_planning_tests/maps/map.xcf rename to nav2_planning_tests/maps/map.xcf diff --git a/src/planning/nav2_planning_tests/maps/map_circular.pgm b/nav2_planning_tests/maps/map_circular.pgm similarity index 100% rename from src/planning/nav2_planning_tests/maps/map_circular.pgm rename to nav2_planning_tests/maps/map_circular.pgm diff --git a/src/planning/nav2_planning_tests/maps/map_circular.yaml b/nav2_planning_tests/maps/map_circular.yaml similarity index 100% rename from src/planning/nav2_planning_tests/maps/map_circular.yaml rename to nav2_planning_tests/maps/map_circular.yaml diff --git a/src/planning/nav2_planning_tests/package.xml b/nav2_planning_tests/package.xml similarity index 77% rename from src/planning/nav2_planning_tests/package.xml rename to nav2_planning_tests/package.xml index 62653423d9..494d2e1e83 100644 --- a/src/planning/nav2_planning_tests/package.xml +++ b/nav2_planning_tests/package.xml @@ -12,18 +12,14 @@ rclcpp nav2_tasks nav2_util - nav2_libs_msgs - nav2_planning_msgs - nav2_world_model_msgs + nav2_msgs nav_msgs visualization_msgs rclcpp nav2_tasks nav2_util - nav2_libs_msgs - nav2_planning_msgs - nav2_world_model_msgs + nav2_msgs nav_msgs visualization_msgs diff --git a/src/planning/nav2_planning_tests/planner_tester.cpp b/nav2_planning_tests/planner_tester.cpp similarity index 97% rename from src/planning/nav2_planning_tests/planner_tester.cpp rename to nav2_planning_tests/planner_tester.cpp index 3ae30eebb5..458d9c63de 100644 --- a/src/planning/nav2_planning_tests/planner_tester.cpp +++ b/nav2_planning_tests/planner_tester.cpp @@ -22,7 +22,7 @@ #include "planner_tester.hpp" #include "geometry_msgs/msg/twist.hpp" #include "nav2_util/map_loader/map_loader.hpp" -#include "nav2_libs_msgs/msg/costmap_meta_data.hpp" +#include "nav2_msgs/msg/costmap_meta_data.hpp" using namespace std::chrono_literals; using nav2_tasks::TaskStatus; @@ -209,8 +209,8 @@ void PlannerTester::startCostmapServer(std::string serviceName) auto costmap_service_callback = [this]( const std::shared_ptr/*request_header*/, - const std::shared_ptr request, - std::shared_ptr response) -> void + const std::shared_ptr request, + std::shared_ptr response) -> void { RCLCPP_INFO(this->get_logger(), "PlannerTester: Incoming costmap request"); response->map = costmap_->getCostmap(request->specs); @@ -219,7 +219,7 @@ void PlannerTester::startCostmapServer(std::string serviceName) // TODO(orduno): Enable parameter server and get costmap service name from there // Create a service that will use the callback function to handle requests. - costmap_server_ = create_service( + costmap_server_ = create_service( serviceName, costmap_service_callback); costmap_server_running_ = true; diff --git a/src/planning/nav2_planning_tests/planner_tester.hpp b/nav2_planning_tests/planner_tester.hpp similarity index 95% rename from src/planning/nav2_planning_tests/planner_tester.hpp rename to nav2_planning_tests/planner_tester.hpp index 8f7e6b2461..7f16313872 100644 --- a/src/planning/nav2_planning_tests/planner_tester.hpp +++ b/nav2_planning_tests/planner_tester.hpp @@ -23,8 +23,8 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_tasks/compute_path_to_pose_task.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav2_libs_msgs/msg/costmap.hpp" -#include "nav2_world_model_msgs/srv/get_costmap.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/srv/get_costmap.hpp" #include "visualization_msgs/msg/marker.hpp" #include "nav2_util/costmap.hpp" @@ -95,7 +95,7 @@ class PlannerTester : public rclcpp::Node, public ::testing::Test std::string plannerName_; // Server for providing a costmap - rclcpp::Service::SharedPtr costmap_server_; + rclcpp::Service::SharedPtr costmap_server_; // Occupancy grid publisher for visualization rclcpp::Publisher::SharedPtr map_publisher_; diff --git a/src/planning/nav2_planning_tests/test_planner_launch.py b/nav2_planning_tests/test_planner_launch.py similarity index 100% rename from src/planning/nav2_planning_tests/test_planner_launch.py rename to nav2_planning_tests/test_planner_launch.py diff --git a/src/planning/nav2_planning_tests/test_planner_node.cpp b/nav2_planning_tests/test_planner_node.cpp similarity index 100% rename from src/planning/nav2_planning_tests/test_planner_node.cpp rename to nav2_planning_tests/test_planner_node.cpp diff --git a/nav2_robot/CHANGELOG.rst b/nav2_robot/CHANGELOG.rst new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_robot/CMakeLists.txt b/nav2_robot/CMakeLists.txt similarity index 100% rename from src/libs/nav2_robot/CMakeLists.txt rename to nav2_robot/CMakeLists.txt diff --git a/nav2_robot/README.md b/nav2_robot/README.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_robot/doc/.gitignore b/nav2_robot/doc/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_robot/include/nav2_robot/robot.hpp b/nav2_robot/include/nav2_robot/robot.hpp similarity index 100% rename from src/libs/nav2_robot/include/nav2_robot/robot.hpp rename to nav2_robot/include/nav2_robot/robot.hpp diff --git a/src/libs/nav2_robot/include/nav2_robot/ros_robot.hpp b/nav2_robot/include/nav2_robot/ros_robot.hpp similarity index 100% rename from src/libs/nav2_robot/include/nav2_robot/ros_robot.hpp rename to nav2_robot/include/nav2_robot/ros_robot.hpp diff --git a/src/libs/nav2_robot/package.xml b/nav2_robot/package.xml similarity index 100% rename from src/libs/nav2_robot/package.xml rename to nav2_robot/package.xml diff --git a/src/libs/nav2_robot/src/ros_robot.cpp b/nav2_robot/src/ros_robot.cpp similarity index 100% rename from src/libs/nav2_robot/src/ros_robot.cpp rename to nav2_robot/src/ros_robot.cpp diff --git a/nav2_robot/test/.gitignore b/nav2_robot/test/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_simple_navigator/CHANGELOG.rst b/nav2_simple_navigator/CHANGELOG.rst new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/navigation/nav2_simple_navigator/CMakeLists.txt b/nav2_simple_navigator/CMakeLists.txt similarity index 95% rename from src/navigation/nav2_simple_navigator/CMakeLists.txt rename to nav2_simple_navigator/CMakeLists.txt index db952a6d12..14f4377369 100644 --- a/src/navigation/nav2_simple_navigator/CMakeLists.txt +++ b/nav2_simple_navigator/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_tasks REQUIRED) -find_package(nav2_planning_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(nav2_robot REQUIRED) include_directories( @@ -37,7 +37,7 @@ set(dependencies rclcpp std_msgs nav2_tasks - nav2_planning_msgs + nav2_msgs nav2_robot ) diff --git a/nav2_simple_navigator/README.md b/nav2_simple_navigator/README.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_simple_navigator/doc/.gitignore b/nav2_simple_navigator/doc/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/navigation/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp b/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp similarity index 97% rename from src/navigation/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp rename to nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp index 1e11a8053b..c1b42e93cf 100644 --- a/src/navigation/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp +++ b/nav2_simple_navigator/include/nav2_simple_navigator/simple_navigator.hpp @@ -17,6 +17,7 @@ #include #include +#include "nav2_tasks/task_status.hpp" #include "nav2_tasks/navigate_to_pose_task.hpp" #include "nav2_tasks/compute_path_to_pose_task.hpp" #include "nav2_tasks/follow_path_task.hpp" diff --git a/src/navigation/nav2_simple_navigator/package.xml b/nav2_simple_navigator/package.xml similarity index 89% rename from src/navigation/nav2_simple_navigator/package.xml rename to nav2_simple_navigator/package.xml index 5d485dd698..28dea23672 100644 --- a/src/navigation/nav2_simple_navigator/package.xml +++ b/nav2_simple_navigator/package.xml @@ -11,12 +11,12 @@ rclcpp nav2_tasks - nav2_planning_msgs + nav2_msgs nav2_robot rclcpp nav2_tasks - nav2_planning_msgs + nav2_msgs nav2_robot ament_lint_common diff --git a/src/navigation/nav2_simple_navigator/src/main.cpp b/nav2_simple_navigator/src/main.cpp similarity index 100% rename from src/navigation/nav2_simple_navigator/src/main.cpp rename to nav2_simple_navigator/src/main.cpp diff --git a/src/navigation/nav2_simple_navigator/src/simple_navigator.cpp b/nav2_simple_navigator/src/simple_navigator.cpp similarity index 100% rename from src/navigation/nav2_simple_navigator/src/simple_navigator.cpp rename to nav2_simple_navigator/src/simple_navigator.cpp diff --git a/nav2_simple_navigator/test/.gitignore b/nav2_simple_navigator/test/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_tasks/CHANGELOG.rst b/nav2_tasks/CHANGELOG.rst new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_tasks/CMakeLists.txt b/nav2_tasks/CMakeLists.txt similarity index 87% rename from src/libs/nav2_tasks/CMakeLists.txt rename to nav2_tasks/CMakeLists.txt index ce30e5eb00..231c97306c 100644 --- a/src/libs/nav2_tasks/CMakeLists.txt +++ b/nav2_tasks/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(std_msgs REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) include_directories( include @@ -25,9 +26,8 @@ install(DIRECTORY include/ DESTINATION include/ ) -rosidl_generate_interfaces(nav2_tasks - "msg/TaskStatus.msg" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs +set(dependencies + nav2_msgs ) if(BUILD_TESTING) diff --git a/nav2_tasks/README.md b/nav2_tasks/README.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_tasks/doc/.gitignore b/nav2_tasks/doc/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp similarity index 84% rename from src/libs/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp rename to nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp index 391e39725a..8b3dfafbea 100644 --- a/src/libs/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp +++ b/nav2_tasks/include/nav2_tasks/compute_path_to_pose_task.hpp @@ -17,14 +17,14 @@ #include "nav2_tasks/task_client.hpp" #include "nav2_tasks/task_server.hpp" -#include "nav2_planning_msgs/msg/path_end_points.hpp" -#include "nav2_planning_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path_end_points.hpp" +#include "nav2_msgs/msg/path.hpp" namespace nav2_tasks { -using ComputePathToPoseCommand = nav2_planning_msgs::msg::PathEndPoints; -using ComputePathToPoseResult = nav2_planning_msgs::msg::Path; +using ComputePathToPoseCommand = nav2_msgs::msg::PathEndPoints; +using ComputePathToPoseResult = nav2_msgs::msg::Path; using ComputePathToPoseTaskClient = TaskClient; diff --git a/src/libs/nav2_tasks/include/nav2_tasks/costmap_service_client.hpp b/nav2_tasks/include/nav2_tasks/costmap_service_client.hpp similarity index 72% rename from src/libs/nav2_tasks/include/nav2_tasks/costmap_service_client.hpp rename to nav2_tasks/include/nav2_tasks/costmap_service_client.hpp index 9666eeeade..fc881819e9 100644 --- a/src/libs/nav2_tasks/include/nav2_tasks/costmap_service_client.hpp +++ b/nav2_tasks/include/nav2_tasks/costmap_service_client.hpp @@ -16,23 +16,23 @@ #define NAV2_TASKS__COSTMAP_SERVICE_CLIENT_HPP_ #include "nav2_tasks/service_client.hpp" -#include "nav2_world_model_msgs/srv/get_costmap.hpp" +#include "nav2_msgs/srv/get_costmap.hpp" namespace nav2_tasks { -class CostmapServiceClient : public ServiceClient +class CostmapServiceClient : public ServiceClient { public: CostmapServiceClient() - : ServiceClient("WorldModel_GetCostmap") + : ServiceClient("WorldModel_GetCostmap") { } using CostmapServiceRequest = - ServiceClient::RequestType; + ServiceClient::RequestType; using CostmapServiceResponse = - ServiceClient::ResponseType; + ServiceClient::ResponseType; }; } // namespace nav2_tasks diff --git a/src/libs/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp b/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp similarity index 90% rename from src/libs/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp rename to nav2_tasks/include/nav2_tasks/execute_mission_task.hpp index 74e528be82..e127e50dd4 100644 --- a/src/libs/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp +++ b/nav2_tasks/include/nav2_tasks/execute_mission_task.hpp @@ -17,13 +17,13 @@ #include "nav2_tasks/task_client.hpp" #include "nav2_tasks/task_server.hpp" -#include "nav2_mission_execution_msgs/msg/mission_plan.hpp" +#include "nav2_msgs/msg/mission_plan.hpp" #include "std_msgs/msg/empty.hpp" namespace nav2_tasks { -using ExecuteMissionCommand = nav2_mission_execution_msgs::msg::MissionPlan; +using ExecuteMissionCommand = nav2_msgs::msg::MissionPlan; using ExecuteMissionResult = std_msgs::msg::Empty; using ExecuteMissionTaskClient = TaskClient; diff --git a/src/libs/nav2_tasks/include/nav2_tasks/follow_path_task.hpp b/nav2_tasks/include/nav2_tasks/follow_path_task.hpp similarity index 92% rename from src/libs/nav2_tasks/include/nav2_tasks/follow_path_task.hpp rename to nav2_tasks/include/nav2_tasks/follow_path_task.hpp index 7ea2b2cc04..c7589b380a 100644 --- a/src/libs/nav2_tasks/include/nav2_tasks/follow_path_task.hpp +++ b/nav2_tasks/include/nav2_tasks/follow_path_task.hpp @@ -17,13 +17,13 @@ #include "nav2_tasks/task_client.hpp" #include "nav2_tasks/task_server.hpp" -#include "nav2_planning_msgs/msg/path.hpp" +#include "nav2_msgs/msg/path.hpp" #include "std_msgs/msg/empty.hpp" namespace nav2_tasks { -using FollowPathCommand = nav2_planning_msgs::msg::Path; +using FollowPathCommand = nav2_msgs::msg::Path; using FollowPathResult = std_msgs::msg::Empty; using FollowPathTaskClient = TaskClient; diff --git a/src/libs/nav2_tasks/include/nav2_tasks/map_service_client.hpp b/nav2_tasks/include/nav2_tasks/map_service_client.hpp similarity index 100% rename from src/libs/nav2_tasks/include/nav2_tasks/map_service_client.hpp rename to nav2_tasks/include/nav2_tasks/map_service_client.hpp diff --git a/src/libs/nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp b/nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp similarity index 100% rename from src/libs/nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp rename to nav2_tasks/include/nav2_tasks/navigate_to_pose_task.hpp diff --git a/src/libs/nav2_tasks/include/nav2_tasks/service_client.hpp b/nav2_tasks/include/nav2_tasks/service_client.hpp similarity index 100% rename from src/libs/nav2_tasks/include/nav2_tasks/service_client.hpp rename to nav2_tasks/include/nav2_tasks/service_client.hpp diff --git a/src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp b/nav2_tasks/include/nav2_tasks/task_client.hpp similarity index 96% rename from src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp rename to nav2_tasks/include/nav2_tasks/task_client.hpp index 3819a1a977..0dc19a54fe 100644 --- a/src/libs/nav2_tasks/include/nav2_tasks/task_client.hpp +++ b/nav2_tasks/include/nav2_tasks/task_client.hpp @@ -114,11 +114,11 @@ class TaskClient // If the task has failed or has been canceled, no result message is forthcoming and we // can propagate the status code, using the TaskStatus type rather than the message-level // implementation type - case nav2_tasks::msg::TaskStatus::FAILED: - case nav2_tasks::msg::TaskStatus::CANCELED: + case nav2_msgs::msg::TaskStatus::FAILED: + case nav2_msgs::msg::TaskStatus::CANCELED: return static_cast(statusMsg_->result); - case nav2_tasks::msg::TaskStatus::SUCCEEDED: + case nav2_msgs::msg::TaskStatus::SUCCEEDED: { // The result message may be here already or it may come *after* the status // message. If it's here, the wait will be satisfied immediately. Otherwise @@ -150,7 +150,7 @@ class TaskClient // These messages are internal to the TaskClient implementation typedef std_msgs::msg::Empty CancelMsg; - typedef nav2_tasks::msg::TaskStatus StatusMsg; + typedef nav2_msgs::msg::TaskStatus StatusMsg; StatusMsg::SharedPtr statusMsg_; // Variables to handle the communication of the status message to the waitForResult thread diff --git a/src/libs/nav2_tasks/include/nav2_tasks/task_server.hpp b/nav2_tasks/include/nav2_tasks/task_server.hpp similarity index 94% rename from src/libs/nav2_tasks/include/nav2_tasks/task_server.hpp rename to nav2_tasks/include/nav2_tasks/task_server.hpp index c696cb6b39..035b60e131 100644 --- a/src/libs/nav2_tasks/include/nav2_tasks/task_server.hpp +++ b/nav2_tasks/include/nav2_tasks/task_server.hpp @@ -81,7 +81,7 @@ class TaskServer : public rclcpp::Node // These messages are internal to the TaskClient implementation typedef std_msgs::msg::Empty CancelMsg; - typedef nav2_tasks::msg::TaskStatus StatusMsg; + typedef nav2_msgs::msg::TaskStatus StatusMsg; // The pointer to our private worker thread std::thread * workerThread_; @@ -100,7 +100,7 @@ class TaskServer : public rclcpp::Node // Reset the execution flag now that we've executed the task commandReceived_ = false; - nav2_tasks::msg::TaskStatus statusMsg; + nav2_msgs::msg::TaskStatus statusMsg; // Check the result of the user's function and send the // appropriate message @@ -110,15 +110,15 @@ class TaskServer : public rclcpp::Node resultPub_->publish(resultMsg_); // Then send the success code - statusMsg.result = nav2_tasks::msg::TaskStatus::SUCCEEDED; + statusMsg.result = nav2_msgs::msg::TaskStatus::SUCCEEDED; statusPub_->publish(statusMsg); } else if (status == TaskStatus::FAILED) { // Otherwise, send the failure code - statusMsg.result = nav2_tasks::msg::TaskStatus::FAILED; + statusMsg.result = nav2_msgs::msg::TaskStatus::FAILED; statusPub_->publish(statusMsg); } else if (status == TaskStatus::CANCELED) { // Or the canceled code - statusMsg.result = nav2_tasks::msg::TaskStatus::CANCELED; + statusMsg.result = nav2_msgs::msg::TaskStatus::CANCELED; statusPub_->publish(statusMsg); } else { throw std::logic_error("Unexpected status return from task"); diff --git a/src/libs/nav2_tasks/include/nav2_tasks/task_status.hpp b/nav2_tasks/include/nav2_tasks/task_status.hpp similarity index 76% rename from src/libs/nav2_tasks/include/nav2_tasks/task_status.hpp rename to nav2_tasks/include/nav2_tasks/task_status.hpp index bf637c6754..46b1883681 100644 --- a/src/libs/nav2_tasks/include/nav2_tasks/task_status.hpp +++ b/nav2_tasks/include/nav2_tasks/task_status.hpp @@ -15,17 +15,17 @@ #ifndef NAV2_TASKS__TASK_STATUS_HPP_ #define NAV2_TASKS__TASK_STATUS_HPP_ -#include "nav2_tasks/msg/task_status.hpp" +#include "nav2_msgs/msg/task_status.hpp" namespace nav2_tasks { typedef enum { - SUCCEEDED = msg::TaskStatus::SUCCEEDED, - FAILED = msg::TaskStatus::FAILED, - RUNNING = msg::TaskStatus::RUNNING, - CANCELED = msg::TaskStatus::CANCELED + SUCCEEDED = nav2_msgs::msg::TaskStatus::SUCCEEDED, + FAILED = nav2_msgs::msg::TaskStatus::FAILED, + RUNNING = nav2_msgs::msg::TaskStatus::RUNNING, + CANCELED = nav2_msgs::msg::TaskStatus::CANCELED } TaskStatus; } // namespace nav2_tasks diff --git a/src/libs/nav2_tasks/msg/TaskStatus.msg b/nav2_tasks/msg/TaskStatus.msg similarity index 100% rename from src/libs/nav2_tasks/msg/TaskStatus.msg rename to nav2_tasks/msg/TaskStatus.msg diff --git a/src/libs/nav2_tasks/package.xml b/nav2_tasks/package.xml similarity index 93% rename from src/libs/nav2_tasks/package.xml rename to nav2_tasks/package.xml index 59a20cffaf..b7acbc6be9 100644 --- a/src/libs/nav2_tasks/package.xml +++ b/nav2_tasks/package.xml @@ -14,12 +14,14 @@ builtin_interfaces rosidl_default_generators geometry_msgs + nav2_msgs rclcpp std_msgs builtin_interfaces rosidl_default_runtime geometry_msgs + nav2_msgs ament_lint_common ament_lint_auto diff --git a/nav2_tasks/test/.gitignore b/nav2_tasks/test/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_util/CHANGELOG.rst b/nav2_util/CHANGELOG.rst new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt similarity index 96% rename from src/libs/nav2_util/CMakeLists.txt rename to nav2_util/CMakeLists.txt index 316cb1993e..651f725835 100644 --- a/src/libs/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -11,7 +11,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(nav2_libs_msgs REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(geometry_msgs REQUIRED) @@ -27,7 +27,7 @@ add_library(costmap_lib SHARED ament_target_dependencies(costmap_lib rclcpp - nav2_libs_msgs + nav2_msgs tf2 nav_msgs ) diff --git a/nav2_util/README.md b/nav2_util/README.md new file mode 100644 index 0000000000..e69de29bb2 diff --git a/nav2_util/doc/.gitignore b/nav2_util/doc/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/libs/nav2_util/include/nav2_util/costmap.hpp b/nav2_util/include/nav2_util/costmap.hpp similarity index 87% rename from src/libs/nav2_util/include/nav2_util/costmap.hpp rename to nav2_util/include/nav2_util/costmap.hpp index 664894e30f..b3925d804b 100644 --- a/src/libs/nav2_util/include/nav2_util/costmap.hpp +++ b/nav2_util/include/nav2_util/costmap.hpp @@ -19,8 +19,8 @@ #include #include "rclcpp/rclcpp.hpp" -#include "nav2_libs_msgs/msg/costmap.hpp" -#include "nav2_libs_msgs/msg/costmap_meta_data.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_msgs/msg/costmap_meta_data.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" namespace nav2_util @@ -53,9 +53,9 @@ class Costmap void setTestCostmap(const TestCostmap & testCostmapType); - nav2_libs_msgs::msg::Costmap getCostmap(const nav2_libs_msgs::msg::CostmapMetaData & specifications); + nav2_msgs::msg::Costmap getCostmap(const nav2_msgs::msg::CostmapMetaData & specifications); - nav2_libs_msgs::msg::CostmapMetaData getProperties() {return costmap_properties_;} + nav2_msgs::msg::CostmapMetaData getProperties() {return costmap_properties_;} bool isFree(const unsigned int x_coordinate, const unsigned int y_coordinate) const; bool isFree(const unsigned int index) const; @@ -76,7 +76,7 @@ class Costmap rclcpp::Node * node_; // TODO(orduno): For now, only holding costs from static map - nav2_libs_msgs::msg::CostmapMetaData costmap_properties_; + nav2_msgs::msg::CostmapMetaData costmap_properties_; std::vector costs_; // Static layer parameters diff --git a/src/libs/nav2_util/include/nav2_util/map/map.h b/nav2_util/include/nav2_util/map/map.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/map/map.h rename to nav2_util/include/nav2_util/map/map.h diff --git a/src/libs/nav2_util/include/nav2_util/map_loader/map_loader.hpp b/nav2_util/include/nav2_util/map_loader/map_loader.hpp similarity index 100% rename from src/libs/nav2_util/include/nav2_util/map_loader/map_loader.hpp rename to nav2_util/include/nav2_util/map_loader/map_loader.hpp diff --git a/src/libs/nav2_util/include/nav2_util/pf/eig3.h b/nav2_util/include/nav2_util/pf/eig3.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/pf/eig3.h rename to nav2_util/include/nav2_util/pf/eig3.h diff --git a/src/libs/nav2_util/include/nav2_util/pf/pf.h b/nav2_util/include/nav2_util/pf/pf.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/pf/pf.h rename to nav2_util/include/nav2_util/pf/pf.h diff --git a/src/libs/nav2_util/include/nav2_util/pf/pf_kdtree.h b/nav2_util/include/nav2_util/pf/pf_kdtree.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/pf/pf_kdtree.h rename to nav2_util/include/nav2_util/pf/pf_kdtree.h diff --git a/src/libs/nav2_util/include/nav2_util/pf/pf_pdf.h b/nav2_util/include/nav2_util/pf/pf_pdf.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/pf/pf_pdf.h rename to nav2_util/include/nav2_util/pf/pf_pdf.h diff --git a/src/libs/nav2_util/include/nav2_util/pf/pf_vector.h b/nav2_util/include/nav2_util/pf/pf_vector.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/pf/pf_vector.h rename to nav2_util/include/nav2_util/pf/pf_vector.h diff --git a/src/libs/nav2_util/include/nav2_util/sensors/laser.h b/nav2_util/include/nav2_util/sensors/laser.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/sensors/laser.h rename to nav2_util/include/nav2_util/sensors/laser.h diff --git a/src/libs/nav2_util/include/nav2_util/sensors/odom.h b/nav2_util/include/nav2_util/sensors/odom.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/sensors/odom.h rename to nav2_util/include/nav2_util/sensors/odom.h diff --git a/src/libs/nav2_util/include/nav2_util/sensors/sensor.h b/nav2_util/include/nav2_util/sensors/sensor.h similarity index 100% rename from src/libs/nav2_util/include/nav2_util/sensors/sensor.h rename to nav2_util/include/nav2_util/sensors/sensor.h diff --git a/src/libs/nav2_util/include/nav2_util/strutils.hpp b/nav2_util/include/nav2_util/strutils.hpp similarity index 100% rename from src/libs/nav2_util/include/nav2_util/strutils.hpp rename to nav2_util/include/nav2_util/strutils.hpp diff --git a/src/libs/nav2_util/package.xml b/nav2_util/package.xml similarity index 92% rename from src/libs/nav2_util/package.xml rename to nav2_util/package.xml index 9a3ccdb724..158038eb62 100644 --- a/src/libs/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -11,11 +11,11 @@ ament_cmake rclcpp - nav2_libs_msgs + nav2_msgs nav_msgs rclcpp - nav2_libs_msgs + nav2_msgs nav_msgs ament_cmake_cppcheck diff --git a/src/libs/nav2_util/src/costmap.cpp b/nav2_util/src/costmap.cpp similarity index 98% rename from src/libs/nav2_util/src/costmap.cpp rename to nav2_util/src/costmap.cpp index 55ef181f90..115af10b19 100644 --- a/src/libs/nav2_util/src/costmap.cpp +++ b/nav2_util/src/costmap.cpp @@ -104,8 +104,8 @@ void Costmap::setTestCostmap(const TestCostmap & testCostmapType) using_test_map_ = true; } -nav2_libs_msgs::msg::Costmap Costmap::getCostmap( - const nav2_libs_msgs::msg::CostmapMetaData & /*specifications*/) +nav2_msgs::msg::Costmap Costmap::getCostmap( + const nav2_msgs::msg::CostmapMetaData & /*specifications*/) { if (!map_provided_ && !using_test_map_) { throw std::runtime_error("Costmap has not been set."); @@ -114,7 +114,7 @@ nav2_libs_msgs::msg::Costmap Costmap::getCostmap( // TODO(orduno): build a costmap given the specifications // for now using the specs of the static map - nav2_libs_msgs::msg::Costmap costmap; + nav2_msgs::msg::Costmap costmap; costmap.header.stamp = node_->now(); costmap.header.frame_id = "map"; diff --git a/src/libs/nav2_util/src/map/map.c b/nav2_util/src/map/map.c similarity index 100% rename from src/libs/nav2_util/src/map/map.c rename to nav2_util/src/map/map.c diff --git a/src/libs/nav2_util/src/map/map_cspace.cpp b/nav2_util/src/map/map_cspace.cpp similarity index 100% rename from src/libs/nav2_util/src/map/map_cspace.cpp rename to nav2_util/src/map/map_cspace.cpp diff --git a/src/libs/nav2_util/src/map/map_draw.c b/nav2_util/src/map/map_draw.c similarity index 100% rename from src/libs/nav2_util/src/map/map_draw.c rename to nav2_util/src/map/map_draw.c diff --git a/src/libs/nav2_util/src/map/map_range.c b/nav2_util/src/map/map_range.c similarity index 100% rename from src/libs/nav2_util/src/map/map_range.c rename to nav2_util/src/map/map_range.c diff --git a/src/libs/nav2_util/src/map/map_store.c b/nav2_util/src/map/map_store.c similarity index 100% rename from src/libs/nav2_util/src/map/map_store.c rename to nav2_util/src/map/map_store.c diff --git a/src/libs/nav2_util/src/map_loader/map_loader.cpp b/nav2_util/src/map_loader/map_loader.cpp similarity index 100% rename from src/libs/nav2_util/src/map_loader/map_loader.cpp rename to nav2_util/src/map_loader/map_loader.cpp diff --git a/src/libs/nav2_util/src/pf/eig3.c b/nav2_util/src/pf/eig3.c similarity index 100% rename from src/libs/nav2_util/src/pf/eig3.c rename to nav2_util/src/pf/eig3.c diff --git a/src/libs/nav2_util/src/pf/pf.c b/nav2_util/src/pf/pf.c similarity index 100% rename from src/libs/nav2_util/src/pf/pf.c rename to nav2_util/src/pf/pf.c diff --git a/src/libs/nav2_util/src/pf/pf_draw.c b/nav2_util/src/pf/pf_draw.c similarity index 100% rename from src/libs/nav2_util/src/pf/pf_draw.c rename to nav2_util/src/pf/pf_draw.c diff --git a/src/libs/nav2_util/src/pf/pf_kdtree.c b/nav2_util/src/pf/pf_kdtree.c similarity index 100% rename from src/libs/nav2_util/src/pf/pf_kdtree.c rename to nav2_util/src/pf/pf_kdtree.c diff --git a/src/libs/nav2_util/src/pf/pf_pdf.c b/nav2_util/src/pf/pf_pdf.c similarity index 100% rename from src/libs/nav2_util/src/pf/pf_pdf.c rename to nav2_util/src/pf/pf_pdf.c diff --git a/src/libs/nav2_util/src/pf/pf_vector.c b/nav2_util/src/pf/pf_vector.c similarity index 100% rename from src/libs/nav2_util/src/pf/pf_vector.c rename to nav2_util/src/pf/pf_vector.c diff --git a/src/libs/nav2_util/src/sensors/laser.cpp b/nav2_util/src/sensors/laser.cpp similarity index 100% rename from src/libs/nav2_util/src/sensors/laser.cpp rename to nav2_util/src/sensors/laser.cpp diff --git a/src/libs/nav2_util/src/sensors/odom.cpp b/nav2_util/src/sensors/odom.cpp similarity index 100% rename from src/libs/nav2_util/src/sensors/odom.cpp rename to nav2_util/src/sensors/odom.cpp diff --git a/src/libs/nav2_util/src/sensors/sensor.cpp b/nav2_util/src/sensors/sensor.cpp similarity index 100% rename from src/libs/nav2_util/src/sensors/sensor.cpp rename to nav2_util/src/sensors/sensor.cpp diff --git a/nav2_util/test/.gitignore b/nav2_util/test/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/navigation2/CHANGELOG.rst b/navigation2/CHANGELOG.rst new file mode 100644 index 0000000000..e69de29bb2 diff --git a/navigation2/CMakeLists.txt b/navigation2/CMakeLists.txt new file mode 100644 index 0000000000..59ac3f63ca --- /dev/null +++ b/navigation2/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.5) +project(navigation2) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +find_package(ament_cmake REQUIRED) + +ament_package() diff --git a/navigation2/package.xml b/navigation2/package.xml new file mode 100644 index 0000000000..edfea61c14 --- /dev/null +++ b/navigation2/package.xml @@ -0,0 +1,35 @@ + + + + navigation2 + 0.0.1 + + ROS2 Navigation Stack + + Oregon Robotics Team + Steve Macenski + Apache License 2.0 + + ament_cmake + + nav2_amcl + nav2_astar_planner + nav2_bt_navigator + nav2_costmap_2d + nav2_costmap_world_model + nav2_dijkstra_planner + nav2_gazebo_localizer + nav2_map_server + nav2_mission_executor + nav2_msgs + nav2_planning_tests + nav2_robot + nav2_simple_navigator + nav2_tasks + nav2_util + + + ament_cmake + + + diff --git a/src/libs/costmap_2d/CHANGELOG.rst b/src/libs/costmap_2d/CHANGELOG.rst deleted file mode 100644 index 59899de15f..0000000000 --- a/src/libs/costmap_2d/CHANGELOG.rst +++ /dev/null @@ -1,245 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package costmap_2d -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.16.2 (2018-07-31) -------------------- -* Merge pull request `#773 `_ from ros-planning/packaging_fixes - packaging fixes -* update costmap_2d cmake - * explicit dependency on tf2 - * remove old PCL disable crap -* Contributors: Michael Ferguson - -1.16.1 (2018-07-28) -------------------- -* Merge pull request `#770 `_ from ros-planning/fix_debians - Fix debian builds (closes `#769 `_) -* add tf2_geometry_msgs depend to costmap_2d -* Contributors: Michael Ferguson - -1.16.0 (2018-07-25) -------------------- -* Switch to TF2 `#755 `_ -* unify combination_method dynamic reconfig, closes `#402 `_ -* Merge pull request `#723 `_ from moriarty/melodic-buildfarm-errors - Melodic buildfarm errors -* [costmap_2d/test] set empty transform to Identity -* fix test: abs(unsigned int) is ambiguous - Instead, compare values and subtract smaller from larger to find - the dx and dy. -* fixes pluginlib deprecated header warnings -* Merge pull request `#694 `_ from ros-planning/lunar_691 - costmap variable init & cleanup (forward port of `#691 `_) -* remove unused got_footprint\_ -* initialize all costmap variables -* Merge pull request `#686 `_ from ros-planning/lunar_675 - Fixed race condition with costmaps in LayeredCostmap::resizeMap() -* Fixed race condition with costmaps in LayeredCostmap::resizeMap() - LayeredCostmap::updateMap() and LayeredCostmap::resizeMap() write to the master grid costmap. - And these two functions can be called by different threads at the same time. - One example of these cases is a race condition between subscriber callback thread - dealing with dynamically-size-changing static_layer and periodical updateMap() calls from Costmap2DROS thread. - Under the situation the master grid costmap is not thread-safe. - LayeredCostmap::updateMap() already used the master grid costmap's lock. -* Contributors: Alexander Moriarty, David V. Lu, Jaeyoung Lee, Michael Ferguson, Vincent Rabaud - -1.15.2 (2018-03-22) -------------------- -* Merge pull request `#673 `_ from ros-planning/email_update_lunar - update maintainer email (lunar) -* Merge pull request `#670 `_ from DLu/fix206_lunar - Fixes `#206 `_ for Lunar -* fix 'enable' for static_layer with rolling window (`#659 `_) (`#665 `_) -* Merge pull request `#649 `_ from aaronhoy/lunar_add_ahoy - Add myself as a maintainer. -* Contributors: Aaron Hoy, David V. Lu!!, Jannik Abbenseth, Michael Ferguson - -1.15.1 (2017-08-14) -------------------- - -1.15.0 (2017-08-07) -------------------- -* Added parameter for allowing inflation in unknown cells (`#564 `_) -* Inflation Layer protected members and virtual computeCost [ABI BREAKING] -* Fix for `#517 `_: create a getRobotPose method on move_base instead of using that on the costmaps -* don't update costs if inflation radius is zero -* rebase fixups -* convert packages to format2 -* Speedup (~60%) inflation layer update (`#525 `_) -* Fix CMakeLists + package.xmls (`#548 `_) -* add missing deps on libpcl -* import only PCL common -* pcl proagate -lQt5::Widgets flag so we need to find_package Qt5Widgets (`#578 `_) -* Added deps to amcl costmap_2d move_base (`#512 `_) -* remove GCC warnings -* Fix CMake warnings -* renamed targets for message generation (gencpp -> generate_messages_cpp) in order to avoid warnings for non-existing target dependencies -* Fixed race condition with costmaps -* Merge pull request `#491 `_ from alexhenning/kinetic-inflation-fix -* Fixed sign error in inflation layer -* Adds warning when a layer shrinks the bounds -* Fixed bug with inflation layer that caused underinflation -* Fixed bug with artifacts when not current -* Fix bug with inflation artifacts being left behind -* Fixes issue with costmaps shearing -* Made costmap publishing truly lazy -* Contributors: Alex Henning, Alexander Reimann, Hidde Wieringa, Jorge Santos, Jorge Santos Simón, Martin Günther, Michael Ferguson, Mikael Arguedas, Stephan Opfer, Vincent Rabaud, mryellow - -1.14.0 (2016-05-20) -------------------- -* Reordered initializer list to match order of declarations. - This avoids compiler warning with some compilers. -* Made update map threadsafe - This is necessary for some plugins (e.g. VoxelLayer) that implement a - thread unsafe updateBounds() function. -* Fix bug with resetting static layer - If we don't have a new topic, consider our old data as if it were new. -* fix resource locations to fix tests -* Increase time-limit on failing test -* Merge pull request `#388 `_ from yujinrobot/jade_inflation_ghost_fix - No more ghosts in the inflation layer -* Fixes the dynamic reconfigure segfault - Doing a dynamic reconfigure of the inflation radius recreates - the cached cost values without first locking a mutex, which causes - a segfault. This breaks the reconfigure of inflation parameters into - a separate function and adds a mutex lock. -* Merge pull request `#415 `_ from alexhenning/jade-fix-multiple-static-layers - Fixes an issue with having multiple static layers -* Fixes an issue with having multiple static layers - If you have a static layer in both the local and global costmaps that - use the same map topic, there is a race condition that can cause the - static layer to get stuck after printing `Requesting map....`. This race - condition seems to be due to the call to shutdown in deactivate and how - the NodeHandle handles multiple subscribers under the hood. - This issue appears to happen about 1 in 1000 times in the setup I was - testing. This fix has never failed in over 1000000 tests. Instead of - calling activate and deactivate, the publisher is only recreated if the - topic has changed. Otherwise, it reuses the old setup. -* fix related to issue `#408 `_ - With Rolling Window on, costmap_2d not properly updating bounds and costs in the static layer -* No more ghosts in the inflation layer - Previous bounds would fit the sensor measurements, and the inflation layer would clear - out to these, but leave 'ghosts' behind. These ghosts are from two sources - 1) the - inflation radius and 2) whole obstacles left behind as the robot has moved from the last point. - The modifications here remember the last bounds and set the new bounds so that a box at least - large enough to incorporate the old bounds plus the inflation radius is generated. -* Contributors: Alex Henning, Daniel Stonier, Levon Avagyan, Michael Ferguson, palmieri - -1.13.1 (2015-10-29) -------------------- -* Remove excessive canTransform spam. -* Fix for `#382 `_ -* Republish costmap if origin changes -* Remove Footprint Layer -* Remove extra sign definition and use proper one when padding footprint -* fix plugin warnings on throw, closes `#205 `_ -* initialize publisher variables -* Look for robot_radius when footprint is not set. `#206 `_ -* Add a first_map_only parameter so we keep reusing the first received static map -* Merge pull request `#331 `_ from mikeferguson/static_layer_any_frame -* support rolling static map in any frame -* fix destructor of Costmap2D -* proper locking during costmap update -* do not resize static map when rolling -* Static layer works with rolling window now -* Contributors: Daniel Stonier, David Lu, Jihoon Lee, Michael Ferguson, Rein Appeldoorn, commaster90 - -1.13.0 (2015-03-17) -------------------- -* fixed issue with voxel_layer and obstacle_layer both deleting the same dynamic_reconfigure::Server and causing segfaults -* Fixing various memory freeing operations -* static_layer: Fix indexing error in OccupancyGridUpdate callback function. -* Contributors: Alex Bencz, David V. Lu!!, James Servos, Julse, Kaijen Hsiao - -1.12.0 (2015-02-04) -------------------- -* update maintainer email -* Contributors: Michael Ferguson - -1.11.15 (2015-02-03) --------------------- -* Add ARCHIVE_DESTINATION for static builds -* Contributors: Gary Servin - -1.11.14 (2014-12-05) --------------------- -* added waitForTransform to bufferCloud to solve extrapolation into the future exception -* deallocate costmap_ before reallocating -* prevent div by zero in raytraceLine -* only prefix sensor_frame when it's not empty -* tf_prefix support in obstacle_layer -* remove undefined function updateUsingPlugins -* remove unused cell_data.h -* numerous style fixes -* Contributors: Andrzej Pronobis, David Lu, Jeremie Deray, Mani Monajjemi, Michael Ferguson, enriquefernandez - -1.11.13 (2014-10-02) --------------------- - -1.11.12 (2014-10-01) --------------------- -* costmap_2d: export library layers -* Merge pull request `#198 `_ from kmhallen/hydro-devel - Fixed costmap_2d clearing from service /move_base/clear_costmaps -* Costmap Layer comments -* Add destructors for all of the layers to remove the dynamic parameter clients -* Add method for removing static observations (for testing) -* Move testing_helper -* Initial Clearing Costmap parameter change -* Fixed costmap_2d clearing from service /move_base/clear_costmaps -* Contributors: David Lu!!, Kevin Hallenbeck, Michael Ferguson - -1.11.11 (2014-07-23) --------------------- -* removes trailing spaces and empty lines -* Contributors: Enrique Fernández Perdomo - -1.11.10 (2014-06-25) --------------------- -* Remove unnecessary colons -* Remove unused robot_radius parameter from dynamic_reconfigure -* Contributors: Daniel Stonier, David Lu!! - -1.11.9 (2014-06-10) -------------------- -* fix hypot issues, add comments to tests from tracking this down -* dynamically reconfigure the previously uninitialised variable 'combination_method', closes `#187 `_. -* uses ::hypot(x, y) instead of sqrt(x*x, y*y) -* Contributors: Daniel Stonier, Michael Ferguson, Enrique Fernández Perdomo - -1.11.8 (2014-05-21) -------------------- - -1.11.7 (2014-05-21) -------------------- -* uses %u instead of %d for unsigned int -* update build to find eigen using cmake_modules -* inflation_layer: place .top() & .pop() calls together -* add parameter to configure whether full costmap is published each time -* Contributors: Michael Ferguson, Siegfried-A. Gevatter Pujals, agentx3r, enriquefernandez - -1.11.5 (2014-01-30) -------------------- -* Better threading in inflation layer -* don't set initialized until updateMap is called -* check whether costmap is initalized before publishing -* New Overwrite Methods - updateMap method - Fix for `#68 `_ - Fix for inflation memory problems - InfIsValid `#128 `_ - Static layer can recieve updates and accept non-lethal values - Obstacle layer uses track_unknown_space parameter - Footprint layer is not longer created as top-level layer (used as part of obstacle layer instead) -* Download test data from download.ros.org instead of willow -* Change maintainer from Hersh to Lu - -1.11.4 (2013-09-27) -------------------- -* Improve bounds checking -* Reimplement Clear Costmaps Service by implementing reset functions in each layer -* Package URL Updates -* Additional static layer functionality for receiving updates -* Misc. Pointcloud fixes -* Improved eigen alignment problem on 32-bit arch. -* fixed costmap_2d tests diff --git a/src/libs/costmap_2d/test/obstacle_tests.launch b/src/libs/costmap_2d/test/obstacle_tests.launch deleted file mode 100644 index d94cfab200..0000000000 --- a/src/libs/costmap_2d/test/obstacle_tests.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/src/libs/costmap_2d/test/simple_driving_test.xml b/src/libs/costmap_2d/test/simple_driving_test.xml deleted file mode 100644 index 6bf507d273..0000000000 --- a/src/libs/costmap_2d/test/simple_driving_test.xml +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - diff --git a/src/libs/costmap_2d/test/static_tests.launch b/src/libs/costmap_2d/test/static_tests.launch deleted file mode 100644 index 23a4ba9319..0000000000 --- a/src/libs/costmap_2d/test/static_tests.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/src/libs/nav2_util/msg/Costmap.msg b/src/libs/nav2_util/msg/Costmap.msg deleted file mode 100644 index 4c3e90ca70..0000000000 --- a/src/libs/nav2_util/msg/Costmap.msg +++ /dev/null @@ -1,9 +0,0 @@ -# This represents a 2-D grid map, in which each cell has an associated cost - -std_msgs/Header header - -# MetaData for the map -CostmapMetaData metadata - -# The cost data, in row-major order, starting with (0,0). -uint8[] data diff --git a/src/libs/nav2_util/msg/CostmapMetaData.msg b/src/libs/nav2_util/msg/CostmapMetaData.msg deleted file mode 100644 index 6338795dad..0000000000 --- a/src/libs/nav2_util/msg/CostmapMetaData.msg +++ /dev/null @@ -1,23 +0,0 @@ -# This hold basic information about the characterists of the Costmap - -# The time at which the static map was loaded -builtin_interfaces/Time map_load_time - -# The time of the last update to costmap -builtin_interfaces/Time update_time - -# The corresponding layer name -string layer - -# The map resolution [m/cell] -float32 resolution - -# Number of cells in the horizontal direction -uint32 size_x - -# Number of cells in the vertical direction -uint32 size_y - -# The origin of the costmap [m, m, rad]. -# This is the real-world pose of the cell (0,0) in the map. -geometry_msgs/Pose origin diff --git a/src/mission_execution/nav2_mission_execution_msgs/package.xml b/src/mission_execution/nav2_mission_execution_msgs/package.xml deleted file mode 100644 index 9c74cd4046..0000000000 --- a/src/mission_execution/nav2_mission_execution_msgs/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - - nav2_mission_execution_msgs - 0.1.0 - TODO - Michael Jeronimo - Apache License 2.0 - - ament_cmake - - rclcpp - std_msgs - builtin_interfaces - rosidl_default_generators - geometry_msgs - - rclcpp - std_msgs - builtin_interfaces - rosidl_default_runtime - geometry_msgs - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/planning/nav2_planning_msgs/CMakeLists.txt b/src/planning/nav2_planning_msgs/CMakeLists.txt deleted file mode 100644 index 7370dd2a6e..0000000000 --- a/src/planning/nav2_planning_msgs/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(nav2_planning_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 -Werror) -endif() - -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Path.msg" - "msg/PathEndPoints.msg" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs -) - -ament_export_dependencies(rosidl_default_runtime) - -ament_package() diff --git a/src/world_model/nav2_world_model_msgs/CMakeLists.txt b/src/world_model/nav2_world_model_msgs/CMakeLists.txt deleted file mode 100644 index f7777a1b18..0000000000 --- a/src/world_model/nav2_world_model_msgs/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(nav2_world_model_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 -Werror) -endif() - -find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav2_libs_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/GetCostmap.srv" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs nav2_libs_msgs -) - -ament_export_dependencies(rosidl_default_runtime) - -ament_package() diff --git a/src/world_model/nav2_world_model_msgs/package.xml b/src/world_model/nav2_world_model_msgs/package.xml deleted file mode 100644 index 809400028b..0000000000 --- a/src/world_model/nav2_world_model_msgs/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - nav2_world_model_msgs - 0.1.0 - TODO - Michael Jeronimo - Apache License 2.0 - - ament_cmake - - rclcpp - std_msgs - builtin_interfaces - rosidl_default_generators - geometry_msgs - nav2_libs_msgs - - rclcpp - std_msgs - builtin_interfaces - rosidl_default_runtime - geometry_msgs - nav2_libs_msgs - - rosidl_interface_packages - - - ament_cmake - - diff --git a/tools/build_all.sh b/tools/build_all.sh index a6057bbf50..48a48e12a1 100755 --- a/tools/build_all.sh +++ b/tools/build_all.sh @@ -60,7 +60,7 @@ cd $CWD/navstack_dependencies_ws colcon build --symlink-install) # Build our code -cd $CWD/navigation2 +cd $CWD/navigation2_ws (. $ROS2_SETUP_FILE && . $CWD/navstack_dependencies_ws/install/setup.bash && colcon build --symlink-install) @@ -68,7 +68,7 @@ cd $CWD/navigation2 if test "$ENABLE_ROS1" = true && test "$ENABLE_ROS2" = true ; then cd $CWD . ros1_dependencies_ws/devel/setup.bash - . navigation2/install/setup.bash + . navigation2_ws/install/setup.bash cd $CWD/ros2_ws colcon build --symlink-install --packages-select ros1_bridge --cmake-force-configure fi diff --git a/tools/initial_ros_setup.sh b/tools/initial_ros_setup.sh index 01be9e524c..5ec3ab4bd5 100755 --- a/tools/initial_ros_setup.sh +++ b/tools/initial_ros_setup.sh @@ -4,43 +4,43 @@ ENABLE_ROS1=true ENABLE_BUILD=true ENABLE_ROS2=true if [ "$ROS1_DISTRO" = "" ]; then - export ROS1_DISTRO=kinetic + export ROS1_DISTRO=kinetic fi if [ "$ROS2_DISTRO" = "" ]; then - export ROS2_DISTRO=bouncy + export ROS2_DISTRO=bouncy fi if test "$ROS1_DISTRO" != "kinetic" && test "$ROS1_DISTRO" != "melodic" ; then - echo "ROS1_DISTRO variable must be set to either kinetic or melodic" - exit 1 + echo "ROS1_DISTRO variable must be set to either kinetic or melodic" + exit 1 fi if [ "$ROS2_DISTRO" != "bouncy" ]; then - echo "ROS2_DISTRO variable must be set to bouncy" - exit 1 + echo "ROS2_DISTRO variable must be set to bouncy" + exit 1 fi for opt in "$@" ; do - case $opt in - --no-ros1) - ENABLE_ROS1=false - shift - ;; - --no-ros2) - ENABLE_ROS2=false - shift - ;; - --download-only) - ENABLE_BUILD=false - shift - ;; - *) - echo "Invalid option: $opt" - echo "Valid options:" - echo "--no-ros1 Disables downloading and building ROS1 dependencies and ROS1 bridge" - echo "--no-ros2 Uses the binary distribution of ROS2 bouncy" - echo "--download-only Skips the build step and only downloads the code" - exit 1 - ;; - esac + case $opt in + --no-ros1) + ENABLE_ROS1=false + shift + ;; + --no-ros2) + ENABLE_ROS2=false + shift + ;; + --download-only) + ENABLE_BUILD=false + shift + ;; + *) + echo "Invalid option: $opt" + echo "Valid options:" + echo "--no-ros1 Disables downloading and building ROS1 dependencies and ROS1 bridge" + echo "--no-ros2 Uses the binary distribution of ROS2 bouncy" + echo "--download-only Skips the build step and only downloads the code" + exit 1 + ;; + esac done set -e @@ -48,62 +48,65 @@ CHECKPOINT_FILES='' CWD=`pwd` return_to_root_dir() { - cd $CWD + cd $CWD } download_navstack() { - echo "Downloading the ROS 2 navstack" - if [ -f "custom_nav2.repos" ]; then #override default location for testing - vcs-import < custom_nav2.repos - else - git clone https://github.com/ros-planning/navigation2.git - fi - return_to_root_dir + echo "Downloading the ROS 2 navstack" + mkdir -p navigation2_ws/src + cd navigation2_ws + if [ -f "custom_nav2.repos" ]; then #override default location for testing + vcs import src < custom_nav2.repos + else + cd src + git clone https://github.com/ros-planning/navigation2.git + fi + return_to_root_dir } download_ros2() { - echo "Downloading ROS 2 Release Latest" - mkdir -p ros2_ws/src - cd ros2_ws - wget https://raw.githubusercontent.com/ros2/ros2/release-latest/ros2.repos - vcs import src < ros2.repos - return_to_root_dir + echo "Downloading ROS 2 Release Latest" + mkdir -p ros2_ws/src + cd ros2_ws + wget https://raw.githubusercontent.com/ros2/ros2/release-latest/ros2.repos + vcs import src < ros2.repos + return_to_root_dir } download_ros2_dependencies() { - echo "Downloading the dependencies workspace" - mkdir -p navstack_dependencies_ws/src - cd navstack_dependencies_ws - vcs import src < ${CWD}/navigation2/tools/ros2_dependencies.repos - return_to_root_dir + echo "Downloading the dependencies workspace" + mkdir -p navstack_dependencies_ws/src + cd navstack_dependencies_ws + vcs import src < ${CWD}/navigation2_ws/src/navigation2/tools/ros2_dependencies.repos + return_to_root_dir } download_ros1_dependencies() { - echo "Downloading the ROS 1 dependencies workspace" - mkdir -p ros1_dependencies_ws/src - cd ros1_dependencies_ws - vcs import src < ${CWD}/navigation2/tools/ros1_dependencies.repos.${ROS1_DISTRO} - return_to_root_dir + echo "Downloading the ROS 1 dependencies workspace" + mkdir -p ros1_dependencies_ws/src + cd ros1_dependencies_ws + vcs import src < ${CWD}/navigation2_ws/src/navigation2/tools/ros1_dependencies.repos.${ROS1_DISTRO} + return_to_root_dir } checkpoint() { - local CHECKPOINT_FILE_NAME=.INITIAL_SETUP_$1 - CHECKPOINT_FILES="${CHECKPOINT_FILES} ${CHECKPOINT_FILE_NAME}" - if [ ! -f ${CHECKPOINT_FILE_NAME} ]; then - $1 - touch ${CHECKPOINT_FILE_NAME} - fi + local CHECKPOINT_FILE_NAME=.INITIAL_SETUP_$1 + CHECKPOINT_FILES="${CHECKPOINT_FILES} ${CHECKPOINT_FILE_NAME}" + if [ ! -f ${CHECKPOINT_FILE_NAME} ]; then + $1 + touch ${CHECKPOINT_FILE_NAME} + fi } download_all() { - checkpoint download_navstack - checkpoint download_ros2_dependencies - if [ "$ENABLE_ROS2" = true ]; then - checkpoint download_ros2 - fi - if [ "$ENABLE_ROS1" = true ]; then - checkpoint download_ros1_dependencies - fi + checkpoint download_navstack + checkpoint download_ros2_dependencies + if [ "$ENABLE_ROS2" = true ]; then + checkpoint download_ros2 + fi + if [ "$ENABLE_ROS1" = true ]; then + checkpoint download_ros1_dependencies + fi } echo "This script will download the ROS 2 latest release workspace, the" @@ -117,20 +120,20 @@ echo "Are you sure you want to continue? [yN]" read -r REPLY echo if [ "$REPLY" = "y" ]; then - download_all - if [ "$ENABLE_BUILD" = true ]; then - $CWD/navigation2/tools/build_all.sh - fi + download_all + if [ "$ENABLE_BUILD" = true ]; then + $CWD/navigation2_ws/src/navigation2/tools/build_all.sh + fi - cd ${CWD} - rm ${CHECKPOINT_FILES} - echo - echo "Everything downloaded and built successfully." - echo "To use the navstack source the setup.bash in the install folder" - echo - echo "> source navigation2/install/setup.bash" - echo - echo "To build the navstack you can either" - echo "1. Run 'colcon build --symlink-install' from the navigation2 folder" - echo "2. or run 'make' from navigation2/build/ folder" + cd ${CWD} + rm ${CHECKPOINT_FILES} + echo + echo "Everything downloaded and built successfully." + echo "To use the navstack source the setup.bash in the install folder" + echo + echo "> source navigation2/install/setup.bash" + echo + echo "To build the navstack you can either" + echo "1. Run 'colcon build --symlink-install' from the navigation2 folder" + echo "2. or run 'make' from navigation2/build/ folder" fi