├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── custom.md │ └── feature_request.md └── workflows │ └── workflow.yml ├── .gitignore ├── LICENSE.txt ├── README.md ├── algorithm_manager ├── CMakeLists.txt ├── config │ ├── AutoDock.toml │ ├── Executor.toml │ ├── Task.toml │ ├── UwbTracking.toml │ └── VisionTracking.toml ├── include │ └── algorithm_manager │ │ ├── algorithm_task_manager.hpp │ │ ├── executor_ab_navigation.hpp │ │ ├── executor_auto_dock.hpp │ │ ├── executor_base.hpp │ │ ├── executor_factory.hpp │ │ ├── executor_laser_localization.hpp │ │ ├── executor_laser_mapping.hpp │ │ ├── executor_poses_through_navigation.hpp │ │ ├── executor_reset_nav.hpp │ │ ├── executor_uwb_tracking.hpp │ │ ├── executor_vision_localization.hpp │ │ ├── executor_vision_mapping.hpp │ │ ├── executor_vision_tracking.hpp │ │ ├── feedcode_type.hpp │ │ ├── timer.hpp │ │ └── tracking_indication.hpp ├── package.xml ├── params │ └── nav2_application_params.yaml ├── src │ ├── algorithm_task_manager.cpp │ ├── executor_ab_navigation.cpp │ ├── executor_auto_dock.cpp │ ├── executor_base.cpp │ ├── executor_laser_localization.cpp │ ├── executor_laser_mapping.cpp │ ├── executor_poses_through_navigation.cpp │ ├── executor_uwb_tracking.cpp │ ├── executor_vision_localization.cpp │ ├── executor_vision_mapping.cpp │ ├── executor_vision_tracking.cpp │ ├── main.cpp │ ├── timer.cpp │ └── tracking_indication.cpp └── test │ ├── AMENT_IGNORE │ ├── fake_action.cpp │ └── fake_nav_send_goal_action_test.cpp ├── behavior_manager ├── CMakeLists.txt ├── README ├── config │ ├── auto_tracking.toml │ ├── detect.toml │ └── parameter.toml ├── include │ └── behavior_manager │ │ ├── behavior_manager.hpp │ │ ├── executor_auto_tracking.hpp │ │ ├── executor_stair_jumping.hpp │ │ └── mode_detector.hpp ├── package.xml ├── src │ └── main.cpp └── test │ ├── AMENT_IGNORE │ ├── client_test.cpp │ ├── main.cpp │ └── targetstatic_test.cpp ├── cyberdog_emergency_stop ├── CMakeLists.txt ├── config │ └── configuration.toml ├── include │ └── cyberdog_emergency_stop │ │ ├── emergency_stop.hpp │ │ └── thread_pool.hpp ├── package.xml └── src │ ├── emergency_stop.cpp │ └── main.cpp ├── cyberdog_path_checker └── src │ └── cyberdog_controller │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ └── cyberdog_controller │ │ └── cyberdog_trajectory_checker.hpp │ ├── launch │ └── trajectory_checker.launch.py │ ├── package.xml │ ├── params │ └── params.yaml │ └── src │ ├── cyberdog_trajectory_checker.cpp │ └── main.cpp ├── cyberdog_rosbag_recorder ├── CMakeLists.txt ├── config │ └── topics.toml ├── include │ └── cyberdog_rosbag_recorder │ │ └── rosbag_record.hpp ├── package.xml └── src │ ├── main.cpp │ └── rosbag_record.cpp ├── lidar_obstacle_layer ├── CMakeLists.txt ├── include │ └── lidar_obstacle_layer │ │ └── lidar_obstacle_layer.hpp ├── lidar_obstacle_layer_plugin.xml ├── package.xml └── src │ └── lidar_obstacle_layer.cpp ├── map_label_server ├── CMakeLists.txt ├── include │ └── map_label_server │ │ ├── label_store.hpp │ │ └── labelserver_node.hpp ├── package.xml └── src │ ├── label_store.cpp │ ├── labelserver.cpp │ └── labelserver_node.cpp ├── navigation2 ├── .circleci │ ├── config.yml │ └── defaults.yaml ├── .dockerhub │ ├── debug │ │ └── Dockerfile │ ├── distro.Dockerfile │ ├── release │ │ └── Dockerfile │ └── source.Dockerfile ├── .dockerignore ├── .github │ ├── ISSUE_TEMPLATE.md │ ├── PULL_REQUEST_TEMPLATE.md │ └── mergify.yml ├── .gitignore ├── Dockerfile ├── Doxyfile ├── LICENSE ├── README.md ├── codecov.yml ├── nav2_amcl │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_amcl │ │ │ ├── amcl_node.hpp │ │ │ ├── angleutils.hpp │ │ │ ├── map │ │ │ └── map.hpp │ │ │ ├── motion_model │ │ │ └── motion_model.hpp │ │ │ ├── pf │ │ │ ├── eig3.hpp │ │ │ ├── pf.hpp │ │ │ ├── pf_kdtree.hpp │ │ │ ├── pf_pdf.hpp │ │ │ └── pf_vector.hpp │ │ │ └── sensors │ │ │ └── laser │ │ │ └── laser.hpp │ ├── package.xml │ └── src │ │ ├── amcl_node.cpp │ │ ├── include │ │ └── portable_utils.h │ │ ├── main.cpp │ │ ├── map │ │ ├── CMakeLists.txt │ │ ├── map.c │ │ ├── map_cspace.cpp │ │ ├── map_draw.c │ │ └── map_range.c │ │ ├── motion_model │ │ ├── CMakeLists.txt │ │ ├── differential_motion_model.cpp │ │ ├── motion_model.cpp │ │ └── omni_motion_model.cpp │ │ ├── pf │ │ ├── CMakeLists.txt │ │ ├── eig3.c │ │ ├── pf.c │ │ ├── pf_draw.c │ │ ├── pf_kdtree.c │ │ ├── pf_pdf.c │ │ └── pf_vector.c │ │ └── sensors │ │ ├── CMakeLists.txt │ │ └── laser │ │ ├── beam_model.cpp │ │ ├── laser.cpp │ │ ├── likelihood_field_model.cpp │ │ └── likelihood_field_model_prob.cpp ├── nav2_behavior_tree │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ │ ├── hierarchy.odg │ │ └── hierarchy.svg │ ├── groot_instructions.md │ ├── include │ │ └── nav2_behavior_tree │ │ │ ├── behavior_tree_engine.hpp │ │ │ ├── bt_action_node.hpp │ │ │ ├── bt_action_server.hpp │ │ │ ├── bt_action_server_impl.hpp │ │ │ ├── bt_conversions.hpp │ │ │ ├── bt_service_node.hpp │ │ │ ├── plugins │ │ │ ├── action │ │ │ │ ├── back_up_action.hpp │ │ │ │ ├── clear_costmap_service.hpp │ │ │ │ ├── compute_path_through_poses_action.hpp │ │ │ │ ├── compute_path_to_pose_action.hpp │ │ │ │ ├── controller_selector_node.hpp │ │ │ │ ├── follow_path_action.hpp │ │ │ │ ├── goal_checker_selector_node.hpp │ │ │ │ ├── navigate_through_poses_action.hpp │ │ │ │ ├── navigate_to_pose_action.hpp │ │ │ │ ├── planner_selector_node.hpp │ │ │ │ ├── reinitialize_global_localization_service.hpp │ │ │ │ ├── remove_passed_goals_action.hpp │ │ │ │ ├── spin_action.hpp │ │ │ │ ├── truncate_path_action.hpp │ │ │ │ └── wait_action.hpp │ │ │ ├── condition │ │ │ │ ├── distance_traveled_condition.hpp │ │ │ │ ├── goal_reached_condition.hpp │ │ │ │ ├── goal_updated_condition.hpp │ │ │ │ ├── initial_pose_received_condition.hpp │ │ │ │ ├── is_battery_low_condition.hpp │ │ │ │ ├── is_stuck_condition.hpp │ │ │ │ ├── time_expired_condition.hpp │ │ │ │ └── transform_available_condition.hpp │ │ │ ├── control │ │ │ │ ├── pipeline_sequence.hpp │ │ │ │ ├── recovery_node.hpp │ │ │ │ └── round_robin_node.hpp │ │ │ └── decorator │ │ │ │ ├── distance_controller.hpp │ │ │ │ ├── goal_updater_node.hpp │ │ │ │ ├── rate_controller.hpp │ │ │ │ ├── single_trigger_node.hpp │ │ │ │ └── speed_controller.hpp │ │ │ └── ros_topic_logger.hpp │ ├── nav2_tree_nodes.xml │ ├── package.xml │ ├── plugins │ │ ├── action │ │ │ ├── back_up_action.cpp │ │ │ ├── clear_costmap_service.cpp │ │ │ ├── compute_path_through_poses_action.cpp │ │ │ ├── compute_path_to_pose_action.cpp │ │ │ ├── controller_selector_node.cpp │ │ │ ├── follow_path_action.cpp │ │ │ ├── goal_checker_selector_node.cpp │ │ │ ├── navigate_through_poses_action.cpp │ │ │ ├── navigate_to_pose_action.cpp │ │ │ ├── planner_selector_node.cpp │ │ │ ├── reinitialize_global_localization_service.cpp │ │ │ ├── remove_passed_goals_action.cpp │ │ │ ├── spin_action.cpp │ │ │ ├── truncate_path_action.cpp │ │ │ └── wait_action.cpp │ │ ├── condition │ │ │ ├── distance_traveled_condition.cpp │ │ │ ├── goal_reached_condition.cpp │ │ │ ├── goal_updated_condition.cpp │ │ │ ├── initial_pose_received_condition.cpp │ │ │ ├── is_battery_low_condition.cpp │ │ │ ├── is_stuck_condition.cpp │ │ │ ├── time_expired_condition.cpp │ │ │ └── transform_available_condition.cpp │ │ ├── control │ │ │ ├── pipeline_sequence.cpp │ │ │ ├── recovery_node.cpp │ │ │ └── round_robin_node.cpp │ │ └── decorator │ │ │ ├── distance_controller.cpp │ │ │ ├── goal_updater_node.cpp │ │ │ ├── rate_controller.cpp │ │ │ ├── single_trigger_node.cpp │ │ │ └── speed_controller.cpp │ ├── src │ │ └── behavior_tree_engine.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── plugins │ │ ├── action │ │ │ ├── CMakeLists.txt │ │ │ ├── test_back_up_action.cpp │ │ │ ├── test_bt_action_node.cpp │ │ │ ├── test_clear_costmap_service.cpp │ │ │ ├── test_compute_path_through_poses_action.cpp │ │ │ ├── test_compute_path_to_pose_action.cpp │ │ │ ├── test_controller_selector_node.cpp │ │ │ ├── test_follow_path_action.cpp │ │ │ ├── test_goal_checker_selector_node.cpp │ │ │ ├── test_navigate_through_poses_action.cpp │ │ │ ├── test_navigate_to_pose_action.cpp │ │ │ ├── test_planner_selector_node.cpp │ │ │ ├── test_reinitialize_global_localization_service.cpp │ │ │ ├── test_remove_passed_goals_action.cpp │ │ │ ├── test_spin_action.cpp │ │ │ ├── test_truncate_path_action.cpp │ │ │ └── test_wait_action.cpp │ │ ├── condition │ │ │ ├── CMakeLists.txt │ │ │ ├── test_distance_traveled.cpp │ │ │ ├── test_goal_reached.cpp │ │ │ ├── test_goal_updated.cpp │ │ │ ├── test_initial_pose_received.cpp │ │ │ ├── test_is_battery_low.cpp │ │ │ ├── test_is_stuck.cpp │ │ │ ├── test_time_expired.cpp │ │ │ └── test_transform_available.cpp │ │ ├── control │ │ │ ├── CMakeLists.txt │ │ │ ├── test_pipeline_sequence.cpp │ │ │ ├── test_recovery_node.cpp │ │ │ └── test_round_robin_node.cpp │ │ └── decorator │ │ │ ├── CMakeLists.txt │ │ │ ├── test_distance_controller.cpp │ │ │ ├── test_goal_updater_node.cpp │ │ │ ├── test_rate_controller.cpp │ │ │ ├── test_single_trigger_node.cpp │ │ │ └── test_speed_controller.cpp │ │ ├── test_action_server.hpp │ │ ├── test_behavior_tree_fixture.hpp │ │ ├── test_bt_conversions.cpp │ │ ├── test_dummy_tree_node.hpp │ │ ├── test_service.hpp │ │ └── test_transform_handler.hpp ├── nav2_bringup │ └── bringup │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── launch │ │ ├── bringup_launch.py │ │ ├── localization_launch.py │ │ ├── navigation_launch.py │ │ └── slam_launch.py │ │ ├── package.xml │ │ └── params │ │ ├── nav2_multirobot_params_1.yaml │ │ ├── nav2_multirobot_params_2.yaml │ │ └── nav2_params.yaml ├── nav2_bt_navigator │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── behavior_trees │ │ ├── follow_point.xml │ │ ├── navigate_through_poses_w_replanning_and_recovery.xml │ │ ├── navigate_to_pose_w_replanning_and_recovery.xml │ │ ├── navigate_w_replanning_distance.xml │ │ ├── navigate_w_replanning_speed.xml │ │ └── navigate_w_replanning_time.xml │ ├── doc │ │ ├── follow_point.png │ │ ├── legend.png │ │ ├── navigate_w_replanning_distance.png │ │ ├── navigate_w_replanning_time.png │ │ ├── parallel_w_recovery.png │ │ ├── recovery_node.png │ │ └── recovery_w_goal_updated.png │ ├── include │ │ └── nav2_bt_navigator │ │ │ ├── bt_navigator.hpp │ │ │ ├── navigator.hpp │ │ │ └── navigators │ │ │ ├── automatic_recharge.hpp │ │ │ ├── navigate_through_poses.hpp │ │ │ ├── navigate_to_pose.hpp │ │ │ └── target_tracking.hpp │ ├── package.xml │ └── src │ │ ├── bt_navigator.cpp │ │ ├── main.cpp │ │ └── navigators │ │ ├── automatic_recharge.cpp │ │ ├── navigate_through_poses.cpp │ │ ├── navigate_to_pose.cpp │ │ └── target_tracking.cpp ├── nav2_common │ ├── CMakeLists.txt │ ├── cmake │ │ └── nav2_package.cmake │ ├── nav2_common-extras.cmake │ ├── nav2_common │ │ ├── __init__.py │ │ └── launch │ │ │ ├── __init__.py │ │ │ ├── has_node_params.py │ │ │ ├── replace_string.py │ │ │ └── rewritten_yaml.py │ └── package.xml ├── nav2_controller │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_controller │ │ │ ├── nav2_controller.hpp │ │ │ └── plugins │ │ │ ├── simple_goal_checker.hpp │ │ │ ├── simple_progress_checker.hpp │ │ │ └── stopped_goal_checker.hpp │ ├── package.xml │ ├── plugins.xml │ ├── plugins │ │ ├── simple_goal_checker.cpp │ │ ├── simple_progress_checker.cpp │ │ ├── stopped_goal_checker.cpp │ │ └── test │ │ │ ├── CMakeLists.txt │ │ │ ├── goal_checker.cpp │ │ │ └── progress_checker.cpp │ └── src │ │ ├── main.cpp │ │ └── nav2_controller.cpp ├── nav2_core │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_core │ │ │ ├── controller.hpp │ │ │ ├── exceptions.hpp │ │ │ ├── global_planner.hpp │ │ │ ├── goal_checker.hpp │ │ │ ├── progress_checker.hpp │ │ │ ├── recovery.hpp │ │ │ └── waypoint_task_executor.hpp │ └── package.xml ├── nav2_costmap_2d │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── cfg │ │ ├── Costmap2D.cfg │ │ ├── GenericPlugin.cfg │ │ ├── InflationPlugin.cfg │ │ ├── ObstaclePlugin.cfg │ │ └── VoxelPlugin.cfg │ ├── costmap_plugins.xml │ ├── include │ │ └── nav2_costmap_2d │ │ │ ├── array_parser.hpp │ │ │ ├── clear_costmap_service.hpp │ │ │ ├── cost_values.hpp │ │ │ ├── costmap_2d.hpp │ │ │ ├── costmap_2d_publisher.hpp │ │ │ ├── costmap_2d_ros.hpp │ │ │ ├── costmap_filters │ │ │ ├── costmap_filter.hpp │ │ │ ├── filter_values.hpp │ │ │ ├── keepout_filter.hpp │ │ │ └── speed_filter.hpp │ │ │ ├── costmap_layer.hpp │ │ │ ├── costmap_math.hpp │ │ │ ├── costmap_subscriber.hpp │ │ │ ├── costmap_topic_collision_checker.hpp │ │ │ ├── exceptions.hpp │ │ │ ├── footprint.hpp │ │ │ ├── footprint_collision_checker.hpp │ │ │ ├── footprint_subscriber.hpp │ │ │ ├── inflation_layer.hpp │ │ │ ├── layer.hpp │ │ │ ├── layered_costmap.hpp │ │ │ ├── observation.hpp │ │ │ ├── observation_buffer.hpp │ │ │ ├── obstacle_layer.hpp │ │ │ ├── range_sensor_layer.hpp │ │ │ ├── static_layer.hpp │ │ │ └── voxel_layer.hpp │ ├── launch │ │ ├── example.launch │ │ └── example_params.yaml │ ├── package.xml │ ├── plugins │ │ ├── costmap_filters │ │ │ ├── costmap_filter.cpp │ │ │ ├── keepout_filter.cpp │ │ │ └── speed_filter.cpp │ │ ├── inflation_layer.cpp │ │ ├── obstacle_layer.cpp │ │ ├── range_sensor_layer.cpp │ │ ├── static_layer.cpp │ │ └── voxel_layer.cpp │ ├── src │ │ ├── array_parser.cpp │ │ ├── clear_costmap_service.cpp │ │ ├── costmap_2d.cpp │ │ ├── costmap_2d_cloud.cpp │ │ ├── costmap_2d_markers.cpp │ │ ├── costmap_2d_node.cpp │ │ ├── costmap_2d_publisher.cpp │ │ ├── costmap_2d_ros.cpp │ │ ├── costmap_layer.cpp │ │ ├── costmap_math.cpp │ │ ├── costmap_subscriber.cpp │ │ ├── costmap_topic_collision_checker.cpp │ │ ├── footprint.cpp │ │ ├── footprint_collision_checker.cpp │ │ ├── footprint_subscriber.cpp │ │ ├── layer.cpp │ │ ├── layered_costmap.cpp │ │ └── observation_buffer.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── costmap_params.yaml │ │ ├── integration │ │ ├── CMakeLists.txt │ │ ├── costmap_tester.cpp │ │ ├── costmap_tests_launch.py │ │ ├── footprint_tests.cpp │ │ ├── inflation_tests.cpp │ │ ├── obstacle_tests.cpp │ │ ├── range_tests.cpp │ │ └── test_costmap_topic_collision_checker.cpp │ │ ├── map │ │ ├── TenByTen.pgm │ │ └── TenByTen.yaml │ │ ├── module_tests.cpp │ │ ├── simple_driving_test.xml │ │ ├── test_launch_files │ │ └── costmap_map_server.launch.py │ │ ├── testing_helper.hpp │ │ └── unit │ │ ├── CMakeLists.txt │ │ ├── array_parser_test.cpp │ │ ├── copy_window_test.cpp │ │ ├── costmap_conversion_test.cpp │ │ ├── declare_parameter_test.cpp │ │ ├── footprint_collision_checker_test.cpp │ │ ├── keepout_filter_test.cpp │ │ ├── keepout_filter_test.jpg │ │ └── speed_filter_test.cpp ├── nav2_dwb_controller │ ├── README.md │ ├── costmap_queue │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── costmap_queue │ │ │ │ ├── costmap_queue.hpp │ │ │ │ ├── limited_costmap_queue.hpp │ │ │ │ └── map_based_queue.hpp │ │ ├── package.xml │ │ ├── src │ │ │ ├── costmap_queue.cpp │ │ │ └── limited_costmap_queue.cpp │ │ └── test │ │ │ ├── mbq_test.cpp │ │ │ └── utest.cpp │ ├── dwb_core │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── dwb_core │ │ │ │ ├── dwb_local_planner.hpp │ │ │ │ ├── exceptions.hpp │ │ │ │ ├── illegal_trajectory_tracker.hpp │ │ │ │ ├── publisher.hpp │ │ │ │ ├── trajectory_critic.hpp │ │ │ │ ├── trajectory_generator.hpp │ │ │ │ └── trajectory_utils.hpp │ │ ├── local_planner_plugin.xml │ │ ├── package.xml │ │ ├── src │ │ │ ├── dwb_local_planner.cpp │ │ │ ├── illegal_trajectory_tracker.cpp │ │ │ ├── publisher.cpp │ │ │ └── trajectory_utils.cpp │ │ └── test │ │ │ ├── CMakeLists.txt │ │ │ └── utils_test.cpp │ ├── dwb_critics │ │ ├── CMakeLists.txt │ │ ├── default_critics.xml │ │ ├── include │ │ │ └── dwb_critics │ │ │ │ ├── alignment_util.hpp │ │ │ │ ├── base_obstacle.hpp │ │ │ │ ├── goal_align.hpp │ │ │ │ ├── goal_dist.hpp │ │ │ │ ├── line_iterator.hpp │ │ │ │ ├── map_grid.hpp │ │ │ │ ├── obstacle_footprint.hpp │ │ │ │ ├── oscillation.hpp │ │ │ │ ├── path_align.hpp │ │ │ │ ├── path_dist.hpp │ │ │ │ ├── prefer_forward.hpp │ │ │ │ ├── rotate_to_goal.hpp │ │ │ │ └── twirling.hpp │ │ ├── package.xml │ │ ├── src │ │ │ ├── alignment_util.cpp │ │ │ ├── base_obstacle.cpp │ │ │ ├── goal_align.cpp │ │ │ ├── goal_dist.cpp │ │ │ ├── map_grid.cpp │ │ │ ├── obstacle_footprint.cpp │ │ │ ├── oscillation.cpp │ │ │ ├── path_align.cpp │ │ │ ├── path_dist.cpp │ │ │ ├── prefer_forward.cpp │ │ │ ├── rotate_to_goal.cpp │ │ │ └── twirling.cpp │ │ └── test │ │ │ ├── CMakeLists.txt │ │ │ ├── alignment_util_test.cpp │ │ │ ├── base_obstacle_test.cpp │ │ │ ├── obstacle_footprint_test.cpp │ │ │ ├── prefer_forward_test.cpp │ │ │ └── twirling_test.cpp │ ├── dwb_msgs │ │ ├── CMakeLists.txt │ │ ├── msg │ │ │ ├── CriticScore.msg │ │ │ ├── LocalPlanEvaluation.msg │ │ │ ├── Trajectory2D.msg │ │ │ └── TrajectoryScore.msg │ │ ├── package.xml │ │ └── srv │ │ │ ├── DebugLocalPlan.srv │ │ │ ├── GenerateTrajectory.srv │ │ │ ├── GenerateTwists.srv │ │ │ ├── GetCriticScore.srv │ │ │ └── ScoreTrajectory.srv │ ├── dwb_plugins │ │ ├── CMakeLists.txt │ │ ├── cfg │ │ │ └── KinematicParams.cfg │ │ ├── include │ │ │ └── dwb_plugins │ │ │ │ ├── kinematic_parameters.hpp │ │ │ │ ├── limited_accel_generator.hpp │ │ │ │ ├── one_d_velocity_iterator.hpp │ │ │ │ ├── standard_traj_generator.hpp │ │ │ │ ├── velocity_iterator.hpp │ │ │ │ └── xy_theta_iterator.hpp │ │ ├── package.xml │ │ ├── plugins.xml │ │ ├── src │ │ │ ├── kinematic_parameters.cpp │ │ │ ├── limited_accel_generator.cpp │ │ │ ├── standard_traj_generator.cpp │ │ │ └── xy_theta_iterator.cpp │ │ └── test │ │ │ ├── CMakeLists.txt │ │ │ ├── kinematic_parameters_test.cpp │ │ │ ├── speed_limit_test.cpp │ │ │ ├── twist_gen.cpp │ │ │ └── velocity_iterator_test.cpp │ ├── images │ │ ├── DWB_Structure_Simplified.svg │ │ └── LocalPlanner.svg │ ├── nav2_dwb_controller │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml │ ├── nav_2d_msgs │ │ ├── CMakeLists.txt │ │ ├── msg │ │ │ ├── Path2D.msg │ │ │ ├── Pose2D32.msg │ │ │ ├── Pose2DStamped.msg │ │ │ ├── Twist2D.msg │ │ │ ├── Twist2D32.msg │ │ │ └── Twist2DStamped.msg │ │ └── package.xml │ └── nav_2d_utils │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── nav_2d_utils │ │ │ ├── conversions.hpp │ │ │ ├── odom_subscriber.hpp │ │ │ ├── parameters.hpp │ │ │ ├── path_ops.hpp │ │ │ └── tf_help.hpp │ │ ├── package.xml │ │ ├── src │ │ ├── conversions.cpp │ │ ├── path_ops.cpp │ │ └── tf_help.cpp │ │ └── test │ │ ├── 2d_utils_test.cpp │ │ ├── CMakeLists.txt │ │ ├── path_ops_test.cpp │ │ └── tf_help_test.cpp ├── nav2_lifecycle_manager │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ │ ├── diagram_lifecycle_manager.JPG │ │ └── uml_lifecycle_manager.JPG │ ├── include │ │ └── nav2_lifecycle_manager │ │ │ ├── lifecycle_manager.hpp │ │ │ └── lifecycle_manager_client.hpp │ ├── package.xml │ ├── src │ │ ├── lifecycle_manager.cpp │ │ ├── lifecycle_manager_client.cpp │ │ └── main.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── launch_bond_test.py │ │ ├── launch_lifecycle_test.py │ │ ├── test_bond.cpp │ │ └── test_lifecycle_manager.cpp ├── nav2_map_server │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── cmake_modules │ │ └── FindGRAPHICSMAGICKCPP.cmake │ ├── include │ │ └── nav2_map_server │ │ │ ├── costmap_filter_info_server.hpp │ │ │ ├── map_io.hpp │ │ │ ├── map_mode.hpp │ │ │ ├── map_saver.hpp │ │ │ └── map_server.hpp │ ├── launch │ │ └── map_saver_server.launch.py │ ├── package.xml │ ├── src │ │ ├── costmap_filter_info │ │ │ ├── costmap_filter_info_server.cpp │ │ │ └── main.cpp │ │ ├── map_io.cpp │ │ ├── map_mode.cpp │ │ ├── map_saver │ │ │ ├── main_cli.cpp │ │ │ ├── main_server.cpp │ │ │ └── map_saver.cpp │ │ └── map_server │ │ │ ├── main.cpp │ │ │ └── map_server.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── component │ │ ├── CMakeLists.txt │ │ ├── test_map_saver_launch.py │ │ ├── test_map_saver_node.cpp │ │ ├── test_map_saver_publisher.cpp │ │ ├── test_map_server_launch.py │ │ └── test_map_server_node.cpp │ │ ├── invalid_image.yaml │ │ ├── map_saver_cli │ │ ├── CMakeLists.txt │ │ └── test_map_saver_cli.cpp │ │ ├── map_saver_params.yaml │ │ ├── map_server_params.yaml │ │ ├── test_constants.cpp │ │ ├── test_constants │ │ └── test_constants.h │ │ ├── test_launch_files │ │ ├── map_saver_node.launch.py │ │ └── map_server_node.launch.py │ │ ├── testmap.bmp │ │ ├── testmap.pgm │ │ ├── testmap.png │ │ ├── testmap.yaml │ │ └── unit │ │ ├── CMakeLists.txt │ │ ├── test_costmap_filter_info_server.cpp │ │ └── test_map_io.cpp ├── nav2_msgs │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── action │ │ ├── BackUp.action │ │ ├── ComputePathThroughPoses.action │ │ ├── ComputePathToPose.action │ │ ├── DummyRecovery.action │ │ ├── FollowPath.action │ │ ├── FollowWaypoints.action │ │ ├── NavigateThroughPoses.action │ │ ├── NavigateToPose.action │ │ ├── Spin.action │ │ └── Wait.action │ ├── msg │ │ ├── BehaviorTreeLog.msg │ │ ├── BehaviorTreeStatusChange.msg │ │ ├── Costmap.msg │ │ ├── CostmapFilterInfo.msg │ │ ├── CostmapMetaData.msg │ │ ├── Particle.msg │ │ ├── ParticleCloud.msg │ │ ├── SpeedLimit.msg │ │ └── VoxelGrid.msg │ ├── package.xml │ └── srv │ │ ├── ClearCostmapAroundRobot.srv │ │ ├── ClearCostmapExceptRegion.srv │ │ ├── ClearEntireCostmap.srv │ │ ├── GetCostmap.srv │ │ ├── LoadMap.srv │ │ ├── ManageLifecycleNodes.srv │ │ └── SaveMap.srv ├── nav2_navfn_planner │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── global_planner_plugin.xml │ ├── include │ │ └── nav2_navfn_planner │ │ │ ├── navfn.hpp │ │ │ └── navfn_planner.hpp │ ├── package.xml │ └── src │ │ ├── navfn.cpp │ │ └── navfn_planner.cpp ├── nav2_planner │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_planner │ │ │ └── planner_server.hpp │ ├── package.xml │ └── src │ │ ├── main.cpp │ │ └── planner_server.cpp ├── nav2_recoveries │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_recoveries │ │ │ ├── recovery.hpp │ │ │ └── recovery_server.hpp │ ├── package.xml │ ├── plugins │ │ ├── back_up.cpp │ │ ├── back_up.hpp │ │ ├── spin.cpp │ │ ├── spin.hpp │ │ ├── wait.cpp │ │ └── wait.hpp │ ├── recovery_plugin.xml │ ├── src │ │ ├── main.cpp │ │ └── recovery_server.cpp │ └── test │ │ ├── CMakeLists.txt │ │ └── test_recoveries.cpp ├── nav2_regulated_pure_pursuit_controller │ ├── CMakeLists.txt │ ├── README.md │ ├── doc │ │ └── lookahead_algorithm.png │ ├── include │ │ └── nav2_regulated_pure_pursuit_controller │ │ │ └── regulated_pure_pursuit_controller.hpp │ ├── nav2_regulated_pure_pursuit_controller.xml │ ├── package.xml │ ├── src │ │ └── regulated_pure_pursuit_controller.cpp │ └── test │ │ ├── CMakeLists.txt │ │ └── test_regulated_pp.cpp ├── nav2_rotation_shim_controller │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_rotation_shim_controller │ │ │ └── nav2_rotation_shim_controller.hpp │ ├── nav2_rotation_shim_controller.xml │ ├── package.xml │ ├── src │ │ └── nav2_rotation_shim_controller.cpp │ └── test │ │ ├── CMakeLists.txt │ │ └── test_shim_controller.cpp ├── nav2_smac_planner │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_smac_planner │ │ │ ├── a_star.hpp │ │ │ ├── collision_checker.hpp │ │ │ ├── constants.hpp │ │ │ ├── costmap_downsampler.hpp │ │ │ ├── node_2d.hpp │ │ │ ├── node_basic.hpp │ │ │ ├── node_hybrid.hpp │ │ │ ├── smac_planner_2d.hpp │ │ │ ├── smac_planner_hybrid.hpp │ │ │ ├── smoother.hpp │ │ │ ├── types.hpp │ │ │ └── utils.hpp │ ├── media │ │ ├── A.png │ │ └── B.png │ ├── package.xml │ ├── smac_plugin.xml │ ├── smac_plugin_2d.xml │ ├── src │ │ ├── a_star.cpp │ │ ├── costmap_downsampler.cpp │ │ ├── node_2d.cpp │ │ ├── node_hybrid.cpp │ │ ├── smac_planner_2d.cpp │ │ └── smac_planner_hybrid.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── deprecated │ │ ├── options.hpp │ │ ├── smoother.hpp │ │ ├── smoother_cost_function.hpp │ │ ├── upsampler.hpp │ │ ├── upsampler_cost_function.hpp │ │ └── upsampler_cost_function_nlls.hpp │ │ ├── path.png │ │ ├── test_a_star.cpp │ │ ├── test_collision_checker.cpp │ │ ├── test_costmap_downsampler.cpp │ │ ├── test_node2d.cpp │ │ ├── test_nodebasic.cpp │ │ ├── test_nodehybrid.cpp │ │ ├── test_smac_2d.cpp │ │ └── test_smac_hybrid.cpp ├── nav2_theta_star_planner │ ├── CMakeLists.txt │ ├── README.md │ ├── img │ │ └── 00-37.png │ ├── include │ │ └── nav2_theta_star_planner │ │ │ ├── theta_star.hpp │ │ │ └── theta_star_planner.hpp │ ├── package.xml │ ├── src │ │ ├── theta_star.cpp │ │ └── theta_star_planner.cpp │ ├── test │ │ └── test_theta_star.cpp │ └── theta_star_planner.xml ├── nav2_util │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_util │ │ │ ├── clear_entirely_costmap_service_client.hpp │ │ │ ├── costmap.hpp │ │ │ ├── execution_timer.hpp │ │ │ ├── geometry_utils.hpp │ │ │ ├── lifecycle_node.hpp │ │ │ ├── lifecycle_service_client.hpp │ │ │ ├── lifecycle_utils.hpp │ │ │ ├── line_iterator.hpp │ │ │ ├── node_thread.hpp │ │ │ ├── node_utils.hpp │ │ │ ├── occ_grid_values.hpp │ │ │ ├── odometry_utils.hpp │ │ │ ├── robot_utils.hpp │ │ │ ├── service_client.hpp │ │ │ ├── simple_action_server.hpp │ │ │ └── string_utils.hpp │ ├── package.xml │ ├── src │ │ ├── CMakeLists.txt │ │ ├── costmap.cpp │ │ ├── dump_params.cpp │ │ ├── lifecycle_bringup_commandline.cpp │ │ ├── lifecycle_node.cpp │ │ ├── lifecycle_service_client.cpp │ │ ├── lifecycle_utils.cpp │ │ ├── node_thread.cpp │ │ ├── node_utils.cpp │ │ ├── odometry_utils.cpp │ │ ├── robot_utils.cpp │ │ └── string_utils.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── test_actions.cpp │ │ ├── test_dump_params │ │ ├── dump_params_default.txt │ │ ├── dump_params_error.txt │ │ ├── dump_params_help.txt │ │ ├── dump_params_md.txt │ │ ├── dump_params_md_verbose.txt │ │ ├── dump_params_multiple.txt │ │ ├── dump_params_yaml.txt │ │ ├── dump_params_yaml_verbose.txt │ │ ├── test_dump_params_default.test.py │ │ ├── test_dump_params_md.test.py │ │ ├── test_dump_params_multiple.test.py │ │ ├── test_dump_params_node.py │ │ └── test_dump_params_yaml.test.py │ │ ├── test_execution_timer.cpp │ │ ├── test_geometry_utils.cpp │ │ ├── test_lifecycle_cli_node.cpp │ │ ├── test_lifecycle_node.cpp │ │ ├── test_lifecycle_utils.cpp │ │ ├── test_node_utils.cpp │ │ ├── test_odometry_utils.cpp │ │ ├── test_robot_utils.cpp │ │ ├── test_service_client.cpp │ │ └── test_string_utils.cpp ├── nav2_voxel_grid │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_voxel_grid │ │ │ └── voxel_grid.hpp │ ├── package.xml │ ├── src │ │ └── voxel_grid.cpp │ └── test │ │ ├── CMakeLists.txt │ │ ├── voxel_grid_bresenham_3d.cpp │ │ └── voxel_grid_tests.cpp ├── nav2_waypoint_follower │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ │ └── nav2_waypoint_follower │ │ │ ├── plugins │ │ │ ├── input_at_waypoint.hpp │ │ │ ├── photo_at_waypoint.hpp │ │ │ └── wait_at_waypoint.hpp │ │ │ └── waypoint_follower.hpp │ ├── package.xml │ ├── plugins.xml │ ├── plugins │ │ ├── input_at_waypoint.cpp │ │ ├── photo_at_waypoint.cpp │ │ └── wait_at_waypoint.cpp │ ├── src │ │ ├── main.cpp │ │ └── waypoint_follower.cpp │ └── test │ │ ├── CMakeLists.txt │ │ └── test_task_executors.cpp ├── navigation2 │ ├── CHANGELOG.rst │ ├── CMakeLists.txt │ └── package.xml └── tools │ ├── bt2img.py │ ├── build_all.sh │ ├── code_coverage_report.bash │ ├── ctest_retry.bash │ ├── initial_ros_setup.sh │ ├── run_sanitizers │ ├── run_test_suite.bash │ ├── skip_keys.txt │ ├── underlay.repos │ └── update_bt_diagrams.bash ├── navigation_bringup ├── CMakeLists.txt ├── config │ └── ekf.yaml ├── launch │ ├── auto_dock.launch.py │ ├── navigation.launch.py │ ├── node.algorithm_manager.launch.py │ ├── node.camera_server.launch.py │ ├── node.charging_localization.launch.py │ ├── node.data.launch.py │ ├── node.elevation_mapping_odom.launch.py │ ├── node.emergency_stop.launch.py │ ├── node.head_tof_pc_publisher.launch.py │ ├── node.laser_localization.launch.py │ ├── node.laser_mapping.launch.py │ ├── node.map_label_server.launch.py │ ├── node.mcr_uwb.launch.py │ ├── node.mcr_voice.launch.py │ ├── node.miloc.launch.py │ ├── node.mivins_localization.launch.py │ ├── node.mivins_mapping.launch.py │ ├── node.mivins_vo.launch.py │ ├── node.nav2_base.launch.py │ ├── node.occmap.launch.py │ ├── node.realsense.launch.py │ ├── node.realsense_align.launch.py │ ├── node.report_dog_pose.launch.py │ ├── node.rosbag_recorder.launch.py │ ├── node.seat_adjust_server.launch.py │ ├── node.stair_align.launch.py │ ├── node.state_publisher.launch.py │ ├── node.static_tf.launch.py │ ├── node.stereo_camera.launch.py │ ├── node.tracking.launch.py │ ├── node.tracking_base.launch.py │ ├── node.tracking_indication.launch.py │ ├── node.velocity_adaptor.launch.py │ └── node.vision_manager.launch.py ├── package.xml ├── params │ ├── follow_params.yaml │ ├── map_saver_params.yaml │ ├── nav2_params-backup.yaml │ └── nav2_params.yaml ├── rviz │ ├── nav2_config.rviz │ └── urdf_config.rviz ├── test │ └── test_node.cpp └── urdf │ ├── static_tf.urdf │ └── static_tf_map.urdf ├── positionchecker ├── CMakeLists.txt ├── include │ └── positionchecker │ │ └── position_checker_node.hpp ├── package.xml └── src │ ├── position_checker_node.cpp │ └── positionchecker.cpp └── velocity_adaptor ├── CMakeLists.txt ├── include └── velocity_adaptor │ └── velocity_adaptor.hpp ├── package.xml └── src ├── main.cpp └── velocity_adaptor.cpp /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. iOS] 28 | - Browser [e.g. chrome, safari] 29 | - Version [e.g. 22] 30 | 31 | **Smartphone (please complete the following information):** 32 | - Device: [e.g. iPhone6] 33 | - OS: [e.g. iOS8.1] 34 | - Browser [e.g. stock browser, safari] 35 | - Version [e.g. 22] 36 | 37 | **Additional context** 38 | Add any other context about the problem here. 39 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/custom.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Custom issue template 3 | about: Describe this issue template's purpose here. 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | 11 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Feature request 3 | about: Suggest an idea for this project 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Is your feature request related to a problem? Please describe.** 11 | A clear and concise description of what the problem is. Ex. I'm always frustrated when [...] 12 | 13 | **Describe the solution you'd like** 14 | A clear and concise description of what you want to happen. 15 | 16 | **Describe alternatives you've considered** 17 | A clear and concise description of any alternative solutions or features you've considered. 18 | 19 | **Additional context** 20 | Add any other context or screenshots about the feature request here. 21 | -------------------------------------------------------------------------------- /.github/workflows/workflow.yml: -------------------------------------------------------------------------------- 1 | name: GitHub Actions CI 2 | run-name: ${{ github.actor }} is run GitHub Actions 🚀 3 | on: [push] 4 | defaults: 5 | run: 6 | shell: bash 7 | jobs: 8 | build-job: 9 | runs-on: ubuntu-latest 10 | steps: 11 | - name: Login to Docker Hub 12 | uses: docker/login-action@v2 13 | with: 14 | registry: ghcr.io 15 | username: ${{ github.actor }} 16 | password: ${{ secrets.GIT_TOKEN }} 17 | - name: Install dependence 18 | run: | 19 | sudo apt install -y qemu-user-static binfmt-support 20 | - name: Download code 21 | uses: actions/checkout@v3 22 | - name: Build and code test 23 | run: | 24 | docker pull ghcr.io/miroboticslab/cyberdog:v1 25 | docker run -i -v $GITHUB_WORKSPACE:/home/ros2/src ghcr.io/miroboticslab/cyberdog:v1 bash -c \ 26 | "cd /home/ros2 && source /opt/ros2/galactic/setup.bash \ 27 | && colcon build --packages-up-to navigation_bringup cyberdog_trajectory_checker positionchecker velocity_adaptor map_label_server algorithm_manager behavior_manager lidar_obstacle_layer cyberdog_rosbag_recorder tracking_base \ 28 | && colcon test --event-handlers console_cohesion+ --return-code-on-test-failure --packages-select navigation_bringup cyberdog_trajectory_checker positionchecker velocity_adaptor map_label_server algorithm_manager behavior_manager lidar_obstacle_layer cyberdog_rosbag_recorder tracking_base" 29 | # colcon build test 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # cache and binary files during ros build 2 | build/ 3 | log/ 4 | install/ 5 | 6 | # files create by macOS 7 | *.DS_Store 8 | 9 | # vs code workspace 10 | .vscode/ 11 | 12 | # swp files 13 | *.swp 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/README.md -------------------------------------------------------------------------------- /algorithm_manager/config/AutoDock.toml: -------------------------------------------------------------------------------- 1 | stage1_enable = false 2 | stage2_enable = true 3 | stage3_enable = true -------------------------------------------------------------------------------- /algorithm_manager/config/Executor.toml: -------------------------------------------------------------------------------- 1 | executors = ["LaserMapping", "UwbTracking"] 2 | 3 | [type] 4 | LaserMapping = 5 5 | UwbTracking = 11 6 | -------------------------------------------------------------------------------- /algorithm_manager/config/UwbTracking.toml: -------------------------------------------------------------------------------- 1 | stair_detect = false 2 | static_detect = false 3 | enable_visual_obstacle_avoidance = false # 跟随默认关闭视觉避障 -------------------------------------------------------------------------------- /algorithm_manager/config/VisionTracking.toml: -------------------------------------------------------------------------------- 1 | tracking_keep_distance = 1.5 2 | tracking_relative_pos = 1 -------------------------------------------------------------------------------- /algorithm_manager/include/algorithm_manager/timer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef ALGORITHM_MANAGER__TIMER_HPP_ 16 | #define ALGORITHM_MANAGER__TIMER_HPP_ 17 | 18 | #include 19 | 20 | namespace cyberdog 21 | { 22 | namespace algorithm 23 | { 24 | 25 | class Timer 26 | { 27 | public: 28 | Timer(); 29 | 30 | void Start(); 31 | void Restart(); 32 | void Pause(); 33 | void Resume(); 34 | void Reset(); 35 | 36 | double ElapsedMicroSeconds() const; 37 | double ElapsedSeconds() const; 38 | double ElapsedMinutes() const; 39 | double ElapsedHours() const; 40 | 41 | private: 42 | bool started_; 43 | bool paused_; 44 | std::chrono::high_resolution_clock::time_point start_time_; 45 | std::chrono::high_resolution_clock::time_point pause_time_; 46 | }; 47 | 48 | } // namespace algorithm 49 | } // namespace cyberdog 50 | #endif // ALGORITHM_MANAGER__TIMER_HPP_ 51 | -------------------------------------------------------------------------------- /algorithm_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | algorithm_manager 5 | 0.0.0 6 | the navigation interfaces for other module 7 | tianhonghai 8 | Apache-2.0 9 | 10 | ament_cmake 11 | rclcpp 12 | nav2_util 13 | nav2_lifecycle_manager 14 | nav2_msgs 15 | nav_msgs 16 | nav2_map_server 17 | rclcpp_lifecycle 18 | std_msgs 19 | builtin_interfaces 20 | action_msgs 21 | protocol 22 | rclcpp_action 23 | rclcpp_components 24 | geometry_msgs 25 | cyberdog_common 26 | cyberdog_debug 27 | motion_action 28 | cyberdog_system 29 | mcr_msgs 30 | visualization 31 | behavior_manager 32 | filesystem 33 | cyberdog_machine 34 | cyberdog_visions_interfaces 35 | tf2 36 | tf2_ros 37 | nav2_core 38 | ament_lint_auto 39 | ament_lint_common 40 | 41 | ament_cmake 42 | 43 | 44 | -------------------------------------------------------------------------------- /algorithm_manager/params/nav2_application_params.yaml: -------------------------------------------------------------------------------- 1 | laser_slam: 2 | ros__parameters: 3 | map_builder: 4 | localization_service_timeout: 5 5 | mapping_start_service_timeout: 5 6 | mapping_stop_service_timeout: 5 7 | pose_report_service_timeout: 5 8 | velocity_smoother_service_timeout: 5 9 | localization: 10 | start_service_timeout: 5 11 | stop_service_timeout: 5 12 | pose_report_service_timeout: 5 13 | 14 | nav_application: 15 | ros__parameters: 16 | velocity_smoother_service_timeout: 5 17 | 18 | -------------------------------------------------------------------------------- /algorithm_manager/src/executor_base.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "algorithm_manager/executor_base.hpp" 20 | #include "algorithm_manager/algorithm_task_manager.hpp" 21 | namespace cyberdog 22 | { 23 | namespace algorithm 24 | { 25 | std::unordered_map> ExecutorBase::nav2_lifecycle_clients; 27 | std::unordered_map> ExecutorBase::lifecycle_clients; 29 | std::unordered_map ExecutorBase::task_map_; 30 | std::shared_ptr ExecutorBase::behavior_manager_; 31 | } // namespace algorithm 32 | } // namespace cyberdog 33 | -------------------------------------------------------------------------------- /algorithm_manager/src/executor_poses_through_navigation.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "algorithm_manager/executor_poses_through_navigation.hpp" 20 | 21 | namespace cyberdog 22 | { 23 | namespace algorithm 24 | { 25 | 26 | ExecutorPosesThroughNavigation::ExecutorPosesThroughNavigation(std::string node_name) 27 | : ExecutorBase(node_name) 28 | { 29 | // spin 30 | std::thread{[this]() {rclcpp::spin(this->get_node_base_interface());}}.detach(); 31 | } 32 | 33 | ExecutorPosesThroughNavigation::~ExecutorPosesThroughNavigation() 34 | { 35 | } 36 | 37 | void ExecutorPosesThroughNavigation::Start(const AlgorithmMGR::Goal::ConstSharedPtr goal) 38 | { 39 | (void)goal; 40 | } 41 | 42 | void ExecutorPosesThroughNavigation::Stop( 43 | const StopTaskSrv::Request::SharedPtr request, 44 | StopTaskSrv::Response::SharedPtr response) 45 | { 46 | (void)request; 47 | (void)response; 48 | } 49 | 50 | void ExecutorPosesThroughNavigation::Cancel() 51 | { 52 | INFO("Poses Through Canceled"); 53 | } 54 | 55 | } // namespace algorithm 56 | } // namespace cyberdog 57 | -------------------------------------------------------------------------------- /algorithm_manager/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "cyberdog_common/cyberdog_log.hpp" 18 | #include "cyberdog_debug/backtrace.hpp" 19 | #include "algorithm_manager/algorithm_task_manager.hpp" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | LOGGER_MAIN_INSTANCE("AlgorithmTaskManager"); 24 | cyberdog::debug::register_signal(); 25 | rclcpp::init(argc, argv); 26 | auto atm_ptr = std::make_shared(); 27 | if (!atm_ptr->Init()) { 28 | ERROR("Init failed, will exit with error!"); 29 | return -1; 30 | } 31 | atm_ptr->Run(); 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /algorithm_manager/test/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/algorithm_manager/test/AMENT_IGNORE -------------------------------------------------------------------------------- /behavior_manager/README: -------------------------------------------------------------------------------- 1 | 电子皮肤跟随效果演示涉及的仓 2 | 1. motion, demo-behavior-elec-skin, pkg: elec_skin, motion_action, motion_manager 3 | 2. algorithm_manager包中, 在config/UwbTracking.toml中关闭台阶检测 -------------------------------------------------------------------------------- /behavior_manager/config/auto_tracking.toml: -------------------------------------------------------------------------------- 1 | #动作组合 2 | [[behavior_ids]] 3 | behavior_id = 1 4 | property = "motion" 5 | motion_id = 146 6 | vel_des = [0.0, 0.0, 0.0] 7 | rpy_des = [0.0, 0.0, 0.0] 8 | pos_des = [0.0, 0.0, 0.0] 9 | step_height = [0.0, 0.0] 10 | duration = 0 11 | module_name = "node_" 12 | is_online = true 13 | text = "铁蛋休息一会儿，顺便伸个懒腰" 14 | occupation = true 15 | client = "tracking" 16 | target = 1 17 | mode = 0x01 18 | effect = 0xA4 19 | 20 | #等待组合 21 | [[behavior_ids]] 22 | behavior_id = 2 23 | property = "wait" 24 | wait_time = 10 25 | module_name = "node_" 26 | is_online = true 27 | text = "" 28 | occupation = true 29 | client = "tracking" 30 | target = 1 31 | mode = 0x01 32 | effect = 0xA8 33 | 34 | #动作组合 35 | [[behavior_ids]] 36 | behavior_id = 3 37 | property = "motion" 38 | motion_id = 101 39 | vel_des = [0.0, 0.0, 0.0] 40 | rpy_des = [0.0, 0.0, 0.0] 41 | pos_des = [0.0, 0.0, 0.0] 42 | step_height = [0.0, 0.0] 43 | duration = 0 44 | module_name = "node_" 45 | is_online = true 46 | text = "主人休息好了叫我哦" 47 | occupation = true 48 | client = "tracking" 49 | target = 1 50 | mode = 0x01 51 | effect = 0xAF 52 | 53 | #等待组合 54 | [[behavior_ids]] 55 | behavior_id = 4 56 | property = "wait" 57 | wait_time = 10 58 | module_name = "node_" 59 | is_online = true 60 | text = "" 61 | occupation = true 62 | client = "tracking" 63 | target = 1 64 | mode = 0x01 65 | effect = 0xB3 -------------------------------------------------------------------------------- /behavior_manager/config/detect.toml: -------------------------------------------------------------------------------- 1 | diff_x_threashold = 0.7 2 | diff_y_threashold = 0.7 3 | detect_duration = 5 4 | filter_size = 5 5 | pose_topic_name = "tracking_pose_transformed" # tracking_pose_transformed -------------------------------------------------------------------------------- /behavior_manager/config/parameter.toml: -------------------------------------------------------------------------------- 1 | #相邻动作执行的时间间隔 2 | time = 3000 -------------------------------------------------------------------------------- /behavior_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | behavior_manager 5 | 0.0.0 6 | the navigation interfaces for other module 7 | tianhonghai 8 | Apache-2.0 9 | 10 | ament_cmake 11 | rclcpp 12 | nav2_util 13 | nav2_lifecycle_manager 14 | nav2_msgs 15 | nav_msgs 16 | nav2_map_server 17 | rclcpp_lifecycle 18 | std_msgs 19 | builtin_interfaces 20 | action_msgs 21 | protocol 22 | rclcpp_action 23 | rclcpp_components 24 | geometry_msgs 25 | cyberdog_common 26 | cyberdog_debug 27 | cyberdog_system 28 | mcr_msgs 29 | motion_manager 30 | motion_action 31 | ament_lint_auto 32 | ament_lint_common 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /behavior_manager/test/AMENT_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/behavior_manager/test/AMENT_IGNORE -------------------------------------------------------------------------------- /behavior_manager/test/client_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "rclcpp/rclcpp.hpp" 3 | #include "std_msgs/msg/bool.hpp" 4 | #include "geometry_msgs/msg/pose_stamped.hpp" 5 | #include "std_srvs/srv/trigger.hpp" 6 | #include "behavior_manager/executor_auto_tracking.hpp" 7 | 8 | int main(int argc, char ** argv) 9 | { 10 | rclcpp::init(argc, argv); 11 | auto node = std::make_shared("node_01"); 12 | auto executor_auto_tracking_ = std::make_shared(node); 13 | executor_auto_tracking_->Execute(true); 14 | rclcpp::spin(node); 15 | } -------------------------------------------------------------------------------- /cyberdog_emergency_stop/config/configuration.toml: -------------------------------------------------------------------------------- 1 | # lidar_coeff 系数 从area1->area3 越来越小的趋势 2 | # 1.5 1.2 0.9 0.6 3 | # 1.9 1.6 1.3 1.0 4 | 5 | [free] 6 | # 升档时长（单位为秒）阈值，超过该阈值且环境条件满足才进行升档 7 | up_gear_period_threshold = 3 8 | 9 | # 跟随跑起来才进行升挡，避免在停止时也进行换挡 10 | free_linear_vel_threshold = 0.3 11 | 12 | [area1] 13 | # 是否启用换挡 14 | enable_gear = true 15 | 16 | # 换挡范围 17 | length = 1.5 18 | 19 | # 3个条件均满足才实现换挡 20 | totol_points_threshold = 10 21 | points_as_group = 7 22 | groups_threshold = 2 23 | 24 | # 跟随跑起来才进行降挡，避免在停止时也进行换挡 25 | area1_linear_vel_threshold = 0.3 26 | 27 | [stop] 28 | length = 0.7 29 | lidar_coeff = 1.2 30 | 31 | totol_points_threshold = 15 32 | stop_linear_vel_threshold = 0.7 33 | 34 | # 发送停止指令的次数为100，则停止时长约为 50*10 毫秒 35 | send_stop_number = 50 36 | 37 | [turning] 38 | truning_vel = 0.5 39 | send_stop_number = 50 40 | 41 | [recoil] 42 | # 是否急停后执行后退 43 | enable = false 44 | 45 | # 后退速度 46 | speed = -0.8 47 | 48 | # 默认发送后退指令的次数为50，后退时长为 50*10 毫秒 49 | send_recoil_number = 50 50 | 51 | # 当stop_distance_threshold = 1.0（米）时 52 | # lidar_coeff 为1.0时，雷达探测范围为 l91机身宽度边缘处 外延3cm 53 | # lidar_coeff 为1.1时，雷达探测范围为 l91机身宽度边缘处 外延13cm 54 | # lidar_coeff 为1.2时，雷达探测范围为 l91机身宽度边缘处 外延24cm 55 | # lidar_coeff 为1.3时，雷达探测范围为 l91机身宽度边缘处 外延34cm 56 | # lidar_coeff 为1.4时，雷达探测范围为 l91机身宽度边缘处 外延45cm 57 | # lidar_coeff 为1.5时，雷达探测范围为 l91机身宽度边缘处 外延55cm 58 | # lidar_coeff 为1.6时，雷达探测范围为 l91机身宽度边缘处 外延65cm 59 | # lidar_coeff 为1.7时，雷达探测范围为 l91机身宽度边缘处 外延75cm -------------------------------------------------------------------------------- /cyberdog_emergency_stop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_emergency_stop 5 | 0.0.0 6 | TODO: Package description 7 | yzyrobot 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rclcpp 16 | cyberdog_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /cyberdog_emergency_stop/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "cyberdog_emergency_stop/emergency_stop.hpp" 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | rclcpp::init(argc, argv); 22 | auto node = std::make_shared(); 23 | rclcpp::executors::MultiThreadedExecutor executor; 24 | executor.add_node(node); 25 | executor.spin(); 26 | 27 | // rclcpp::spin(std::make_shared()); 28 | rclcpp::shutdown(); 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /cyberdog_path_checker/src/cyberdog_controller/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Controller 2 | 3 | The Nav2 Controller is an [Execution Module](../doc/requirements/requirements.md) that implements the `nav2_msgs::action::FollowPath` action server. 4 | 5 | An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The cyberdog_controller package is designed to be loaded with plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. 6 | 7 | 8 | Currently available controller plugins are: DWB, and [TEB (dashing release)](https://github.com/rst-tu-dortmund/teb_local_planner/tree/dashing-devel). -------------------------------------------------------------------------------- /cyberdog_path_checker/src/cyberdog_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_trajectory_checker 5 | 1.0.8 6 | load cost map for path checking 7 | Du Zhonghua 8 | Apache-2.0 9 | 10 | ament_cmake 11 | nav2_common 12 | angles 13 | rclcpp 14 | rclcpp_action 15 | std_msgs 16 | nav2_util 17 | nav2_msgs 18 | nav_2d_utils 19 | nav_2d_msgs 20 | nav2_core 21 | pluginlib 22 | dwb_core 23 | rclcpp_components 24 | 25 | ament_lint_common 26 | ament_lint_auto 27 | ament_cmake_gtest 28 | ament_cmake_pytest 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /cyberdog_path_checker/src/cyberdog_controller/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "cyberdog_controller/cyberdog_trajectory_checker.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /cyberdog_rosbag_recorder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(cyberdog_rosbag_recorder) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(std_msgs REQUIRED) 12 | find_package(rcpputils REQUIRED) 13 | find_package(protocol REQUIRED) 14 | find_package(cyberdog_common REQUIRED) 15 | 16 | include_directories(include) 17 | 18 | set(dependencies 19 | rclcpp 20 | std_msgs 21 | rcpputils 22 | protocol 23 | cyberdog_common) 24 | 25 | set(sources 26 | src/main.cpp 27 | src/rosbag_record.cpp) 28 | 29 | # rosbag_record 30 | add_executable(rosbag_recorder ${sources}) 31 | ament_target_dependencies(rosbag_recorder ${dependencies}) 32 | 33 | # install targets 34 | install(TARGETS rosbag_recorder DESTINATION lib/${PROJECT_NAME}) 35 | 36 | # install config launch 37 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 38 | 39 | if(BUILD_TESTING) 40 | find_package(ament_lint_auto REQUIRED) 41 | ament_lint_auto_find_test_dependencies() 42 | endif() 43 | 44 | ament_package() 45 | -------------------------------------------------------------------------------- /cyberdog_rosbag_recorder/config/topics.toml: -------------------------------------------------------------------------------- 1 | use = false 2 | rosbag_file_path = "/SDCARD/rosbags" 3 | topics = ["tf", "tf_static", "global_costmap/costmap", "global_costmap/footprint", "local_costmap/costmap", "local_plan", "plan", "cmd_vel", "scan"] -------------------------------------------------------------------------------- /cyberdog_rosbag_recorder/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cyberdog_rosbag_recorder 5 | 0.0.0 6 | TODO: Package description 7 | quan 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | std_msgs 16 | rclcpp 17 | rcpputils 18 | protocol 19 | cyberdog_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /cyberdog_rosbag_recorder/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "cyberdog_rosbag_recorder/rosbag_record.hpp" 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | rclcpp::init(argc, argv); 22 | auto node = std::make_shared(); 23 | rclcpp::spin(node); 24 | rclcpp::shutdown(); 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /lidar_obstacle_layer/lidar_obstacle_layer_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This is an example plugin which lidar_obstacle_layer. 4 | 5 | -------------------------------------------------------------------------------- /lidar_obstacle_layer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_obstacle_layer 5 | 1.0.0 6 | Simple straight line planner. 7 | Shivang Patel 8 | BSD-3-Clause 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | rclcpp_lifecycle 15 | std_msgs 16 | visualization_msgs 17 | nav2_util 18 | nav2_msgs 19 | nav_msgs 20 | geometry_msgs 21 | builtin_interfaces 22 | tf2_ros 23 | nav2_costmap_2d 24 | nav2_core 25 | pluginlib 26 | 27 | ament_lint_auto 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /map_label_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | map_label_server 5 | 0.0.0 6 | server for save and get map label 7 | duzhonghua 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | protocol 12 | nav_msgs 13 | rclcpp 14 | cyberdog_common 15 | cyberdog_debug 16 | nav2_map_server 17 | filesystem 18 | visualization 19 | cyberdog_visions_interfaces 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /map_label_server/src/labelserver.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "map_label_server/labelserver_node.hpp" 19 | #include "cyberdog_common/cyberdog_log.hpp" 20 | #include "cyberdog_debug/backtrace.hpp" 21 | 22 | int main(int argc, char ** argv) 23 | { 24 | LOGGER_MAIN_INSTANCE("LabelServer"); 25 | cyberdog::debug::register_signal(); 26 | rclcpp::init(argc, argv); 27 | auto node = std::make_shared(); 28 | rclcpp::executors::MultiThreadedExecutor exec_; 29 | exec_.add_node(node->get_node_base_interface()); 30 | exec_.spin(); 31 | rclcpp::shutdown(); 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /navigation2/.circleci/defaults.yaml: -------------------------------------------------------------------------------- 1 | "build": 2 | "executor": "parallel" 3 | "parallel-workers": 2 4 | "symlink-install": true 5 | -------------------------------------------------------------------------------- /navigation2/.dockerhub/debug/Dockerfile: -------------------------------------------------------------------------------- 1 | ../../Dockerfile -------------------------------------------------------------------------------- /navigation2/.dockerhub/release/Dockerfile: -------------------------------------------------------------------------------- 1 | ../../Dockerfile -------------------------------------------------------------------------------- /navigation2/.dockerignore: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Repo 3 | 4 | .circleci/ 5 | .dockerhub/ 6 | .git/ 7 | .github/ 8 | .dockerignore 9 | .gitignore 10 | **Dockerfile 11 | **.Dockerfile 12 | -------------------------------------------------------------------------------- /navigation2/.github/mergify.yml: -------------------------------------------------------------------------------- 1 | pull_request_rules: 2 | - name: backport to galactic at reviewers discretion 3 | conditions: 4 | - base=rolling 5 | - "label=backport-galactic" 6 | actions: 7 | backport: 8 | branches: 9 | - galactic 10 | 11 | - name: backport to foxy at reviewers discretion 12 | conditions: 13 | - base=rolling 14 | - "label=backport-foxy" 15 | actions: 16 | backport: 17 | branches: 18 | - foxy 19 | -------------------------------------------------------------------------------- /navigation2/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | 31 | # Colcon output 32 | build 33 | log 34 | install 35 | 36 | # Visual Studio Code files 37 | .vscode 38 | 39 | # Eclipse project files 40 | .cproject 41 | .project 42 | .pydevproject 43 | 44 | # Python artifacts 45 | __pycache__/ 46 | *.py[cod] 47 | 48 | sphinx_doc/_build 49 | 50 | # CLion artifacts 51 | .idea 52 | cmake-build-debug/ 53 | -------------------------------------------------------------------------------- /navigation2/LICENSE: -------------------------------------------------------------------------------- 1 | Portions of this repository are available under one of the following licenses 2 | 3 | SPDX-ID: 4 | * LGPL-2.1-or-later 5 | * Apache-2.0 6 | * BSD-3-Clause 7 | * Apache-2.0 AND BSD-3-Clause 8 | 9 | Please see the package.xml file for each package to see the specific license for 10 | that package. 11 | 12 | Contributions to existing files should be made under the license of that file. 13 | New files should be made under the first license listed in the appropriate 14 | package.xml file 15 | 16 | For files that are not otherwise marked, they are provided under the Apache-2.0 17 | license. 18 | -------------------------------------------------------------------------------- /navigation2/codecov.yml: -------------------------------------------------------------------------------- 1 | fixes: 2 | - "src/navigation2/::" 3 | 4 | ignore: 5 | - "*/**/test/*" # ignore package test directories, e.g. nav2_dwb_controller/costmap_queue/tests 6 | - "*/test/**/*" # ignore package test directories, e.g. nav2_costmap_2d/tests 7 | - "**/test_*.*" # ignore files starting with test_ e.g. nav2_map_server/test/test_constants.cpp 8 | - "**/*_tests.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp 9 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_amcl/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_amcl/include/nav2_amcl/pf/eig3.hpp: -------------------------------------------------------------------------------- 1 | /* Eigen-decomposition for symmetric 3x3 real matrices. 2 | Public domain, copied from the public domain Java library JAMA. */ 3 | 4 | #ifndef NAV2_AMCL__PF__EIG3_HPP_ 5 | #define NAV2_AMCL__PF__EIG3_HPP_ 6 | 7 | /* Symmetric matrix A => eigenvectors in columns of V, corresponding 8 | eigenvalues in d. */ 9 | void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); 10 | 11 | #endif // NAV2_AMCL__PF__EIG3_HPP_ 12 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_amcl 5 | 1.0.8 6 | 7 |

8 | amcl is a probabilistic localization system for a robot moving in 9 | 2D. It implements the adaptive (or KLD-sampling) Monte Carlo 10 | localization approach (as described by Dieter Fox), which uses a 11 | particle filter to track the pose of a robot against a known map. 12 |

13 |

14 | This node is derived, with thanks, from Andrew Howard's excellent 15 | 'amcl' Player driver. 16 |

17 | 18 | 20 | Mohammad Haghighipanah 21 | LGPL-2.1-or-later 22 | 23 | ament_cmake 24 | nav2_common 25 | rclcpp 26 | tf2_geometry_msgs 27 | geometry_msgs 28 | message_filters 29 | nav_msgs 30 | sensor_msgs 31 | std_srvs 32 | tf2_ros 33 | tf2 34 | nav2_util 35 | nav2_msgs 36 | launch_ros 37 | launch_testing 38 | 39 | ament_lint_common 40 | ament_lint_auto 41 | 42 | 43 | ament_cmake 44 | 45 | 46 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/src/include/portable_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef PORTABLE_UTILS_H 2 | #define PORTABLE_UTILS_H 3 | 4 | #include 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | #ifndef HAVE_DRAND48 11 | // Some system (e.g., Windows) doesn't come with drand48(), srand48(). 12 | // Use rand, and srand for such system. 13 | static double drand48(void) 14 | { 15 | return ((double)rand()) / RAND_MAX; 16 | } 17 | 18 | static void srand48(long int seedval) 19 | { 20 | srand(seedval); 21 | } 22 | #endif 23 | 24 | #ifdef __cplusplus 25 | } 26 | #endif 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // This library is free software; you can redistribute it and/or 4 | // modify it under the terms of the GNU Lesser General Public 5 | // License as published by the Free Software Foundation; either 6 | // version 2.1 of the License, or (at your option) any later version. 7 | // 8 | // This library is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | // Lesser General Public License for more details. 12 | // 13 | // You should have received a copy of the GNU Lesser General Public 14 | // License along with this library; if not, write to the Free Software 15 | // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 16 | 17 | #include 18 | 19 | #include "nav2_amcl/amcl_node.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | int main(int argc, char ** argv) 23 | { 24 | rclcpp::init(argc, argv); 25 | auto node = std::make_shared(); 26 | rclcpp::spin(node->get_node_base_interface()); 27 | rclcpp::shutdown(); 28 | 29 | return 0; 30 | } 31 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/src/map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(map_lib SHARED 2 | map.c 3 | map_range.c 4 | map_draw.c 5 | map_cspace.cpp 6 | ) 7 | 8 | install(TARGETS 9 | map_lib 10 | ARCHIVE DESTINATION lib 11 | LIBRARY DESTINATION lib 12 | RUNTIME DESTINATION bin 13 | ) 14 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/src/motion_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(motions_lib SHARED 2 | omni_motion_model.cpp 3 | differential_motion_model.cpp 4 | motion_model.cpp 5 | ) 6 | target_link_libraries(motions_lib pf_lib) 7 | ament_target_dependencies(motions_lib 8 | nav2_util 9 | ) 10 | 11 | install(TARGETS 12 | motions_lib 13 | ARCHIVE DESTINATION lib 14 | LIBRARY DESTINATION lib 15 | RUNTIME DESTINATION bin 16 | ) 17 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/src/motion_model/motion_model.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // This library is free software; you can redistribute it and/or 4 | // modify it under the terms of the GNU Lesser General Public 5 | // License as published by the Free Software Foundation; either 6 | // version 2.1 of the License, or (at your option) any later version. 7 | // 8 | // This library is distributed in the hope that it will be useful, 9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | // Lesser General Public License for more details. 12 | // 13 | // You should have received a copy of the GNU Lesser General Public 14 | // License along with this library; if not, write to the Free Software 15 | // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 16 | 17 | #include "nav2_amcl/motion_model/motion_model.hpp" 18 | 19 | #include 20 | 21 | namespace nav2_amcl 22 | { 23 | 24 | MotionModel * 25 | MotionModel::createMotionModel( 26 | std::string & type, double alpha1, double alpha2, 27 | double alpha3, double alpha4, double alpha5) 28 | { 29 | if (type == "differential") { 30 | return new DifferentialMotionModel(alpha1, alpha2, alpha3, alpha4); 31 | } else if (type == "omnidirectional") { 32 | return new OmniMotionModel(alpha1, alpha2, alpha3, alpha4, alpha5); 33 | } 34 | 35 | return nullptr; 36 | } 37 | 38 | } // namespace nav2_amcl 39 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/src/pf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(CMAKE_CXX_COMPILER_ID MATCHES "Clang") 2 | add_compile_options(-Wno-gnu-folding-constant) 3 | endif() 4 | 5 | add_library(pf_lib SHARED 6 | pf.c 7 | pf_kdtree.c 8 | pf_pdf.c 9 | pf_vector.c 10 | eig3.c 11 | pf_draw.c 12 | ) 13 | 14 | target_include_directories(pf_lib PRIVATE ../include) 15 | if(HAVE_DRAND48) 16 | target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") 17 | endif() 18 | 19 | install(TARGETS 20 | pf_lib 21 | ARCHIVE DESTINATION lib 22 | LIBRARY DESTINATION lib 23 | RUNTIME DESTINATION bin 24 | ) 25 | -------------------------------------------------------------------------------- /navigation2/nav2_amcl/src/sensors/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sensors_lib SHARED 2 | laser/laser.cpp 3 | laser/beam_model.cpp 4 | laser/likelihood_field_model.cpp 5 | laser/likelihood_field_model_prob.cpp 6 | ) 7 | # map_update_cspace 8 | target_link_libraries(sensors_lib pf_lib map_lib) 9 | 10 | install(TARGETS 11 | sensors_lib 12 | ARCHIVE DESTINATION lib 13 | LIBRARY DESTINATION lib 14 | RUNTIME DESTINATION bin 15 | ) 16 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_behavior_tree/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/doc/hierarchy.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_behavior_tree/doc/hierarchy.odg -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/groot_instructions.md: -------------------------------------------------------------------------------- 1 | # Instructions on using Groot 2 | [Groot](https://github.com/BehaviorTree/Groot) is the companion application of [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP) to create, edit, and monitor behavior trees. 3 | 4 | ##### Note: Currently fully supports visualization of the behavior trees. It also supports the creation of custom nodes except control flow nodes. Support for custom control flow nodes and real-time monitoring is under development. 5 | 6 | ### BehaviorTree visualization 7 | To visualize the behavior trees using Groot: 8 | 1. Open Groot in editor mode 9 | 2. Select the `Load palette from file` option (import button) near the top left corner. 10 | 3. Open the file `/path/to/nav2/nav2_behavior_tree/nav2_tree_nodes.xml` to import all the behavior tree nodes used for navigation. 11 | 4. Select `Load tree` option near the top left corner 12 | 5. Browse the tree you want to visualize the select ok. 13 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ 16 | #define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ 17 | 18 | #include "behaviortree_cpp_v3/behavior_tree.h" 19 | 20 | namespace nav2_behavior_tree 21 | { 22 | /** 23 | * @brief A BT::ConditionNode that returns SUCCESS if initial pose 24 | * has been received and FAILURE otherwise 25 | */ 26 | BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node); 27 | } 28 | 29 | #endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__INITIAL_POSE_RECEIVED_CONDITION_HPP_ 30 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Samsung Research America 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include "nav2_behavior_tree/plugins/action/reinitialize_global_localization_service.hpp" 17 | 18 | namespace nav2_behavior_tree 19 | { 20 | 21 | ReinitializeGlobalLocalizationService::ReinitializeGlobalLocalizationService( 22 | const std::string & service_node_name, 23 | const BT::NodeConfiguration & conf) 24 | : BtServiceNode(service_node_name, conf) 25 | {} 26 | 27 | } // namespace nav2_behavior_tree 28 | 29 | #include "behaviortree_cpp_v3/bt_factory.h" 30 | BT_REGISTER_NODES(factory) 31 | { 32 | factory.registerNodeType( 33 | "ReinitializeGlobalLocalization"); 34 | } 35 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/plugins/action/spin_action.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "nav2_behavior_tree/plugins/action/spin_action.hpp" 19 | 20 | namespace nav2_behavior_tree 21 | { 22 | 23 | SpinAction::SpinAction( 24 | const std::string & xml_tag_name, 25 | const std::string & action_name, 26 | const BT::NodeConfiguration & conf) 27 | : BtActionNode(xml_tag_name, action_name, conf) 28 | { 29 | double dist; 30 | getInput("spin_dist", dist); 31 | goal_.target_yaw = dist; 32 | } 33 | 34 | void SpinAction::on_tick() 35 | { 36 | increment_recovery_count(); 37 | } 38 | 39 | } // namespace nav2_behavior_tree 40 | 41 | #include "behaviortree_cpp_v3/bt_factory.h" 42 | BT_REGISTER_NODES(factory) 43 | { 44 | BT::NodeBuilder builder = 45 | [](const std::string & name, const BT::NodeConfiguration & config) 46 | { 47 | return std::make_unique(name, "spin", config); 48 | }; 49 | 50 | factory.registerBuilder("Spin", builder); 51 | } 52 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/plugins/condition/initial_pose_received_condition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "nav2_behavior_tree/plugins/condition/initial_pose_received_condition.hpp" 16 | 17 | namespace nav2_behavior_tree 18 | { 19 | 20 | BT::NodeStatus initialPoseReceived(BT::TreeNode & tree_node) 21 | { 22 | auto initPoseReceived = tree_node.config().blackboard->get("initial_pose_received"); 23 | return initPoseReceived ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; 24 | } 25 | 26 | } // namespace nav2_behavior_tree 27 | 28 | #include "behaviortree_cpp_v3/bt_factory.h" 29 | BT_REGISTER_NODES(factory) 30 | { 31 | factory.registerSimpleCondition( 32 | "InitialPoseReceived", 33 | std::bind(&nav2_behavior_tree::initialPoseReceived, std::placeholders::_1)); 34 | } 35 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(test_bt_conversions test_bt_conversions.cpp) 2 | ament_target_dependencies(test_bt_conversions ${dependencies}) 3 | 4 | add_subdirectory(plugins/condition) 5 | add_subdirectory(plugins/decorator) 6 | add_subdirectory(plugins/control) 7 | add_subdirectory(plugins/action) 8 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/test/plugins/control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(test_control_recovery_node test_recovery_node.cpp) 2 | target_link_libraries(test_control_recovery_node nav2_recovery_node_bt_node) 3 | ament_target_dependencies(test_control_recovery_node ${dependencies}) 4 | 5 | ament_add_gtest(test_control_pipeline_sequence test_pipeline_sequence.cpp) 6 | target_link_libraries(test_control_pipeline_sequence nav2_pipeline_sequence_bt_node) 7 | ament_target_dependencies(test_control_pipeline_sequence ${dependencies}) 8 | 9 | ament_add_gtest(test_control_round_robin_node test_round_robin_node.cpp) 10 | target_link_libraries(test_control_round_robin_node nav2_round_robin_node_bt_node) 11 | ament_target_dependencies(test_control_round_robin_node ${dependencies}) 12 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/test/plugins/decorator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(test_decorator_distance_controller test_distance_controller.cpp) 2 | target_link_libraries(test_decorator_distance_controller nav2_distance_controller_bt_node) 3 | ament_target_dependencies(test_decorator_distance_controller ${dependencies}) 4 | 5 | ament_add_gtest(test_decorator_speed_controller test_speed_controller.cpp) 6 | target_link_libraries(test_decorator_speed_controller nav2_speed_controller_bt_node) 7 | ament_target_dependencies(test_decorator_speed_controller ${dependencies}) 8 | 9 | ament_add_gtest(test_decorator_rate_controller test_rate_controller.cpp) 10 | target_link_libraries(test_decorator_rate_controller nav2_rate_controller_bt_node) 11 | ament_target_dependencies(test_decorator_rate_controller ${dependencies}) 12 | 13 | ament_add_gtest(test_goal_updater_node test_goal_updater_node.cpp) 14 | target_link_libraries(test_goal_updater_node nav2_goal_updater_node_bt_node) 15 | ament_target_dependencies(test_goal_updater_node ${dependencies}) 16 | 17 | ament_add_gtest(test_single_trigger_node test_single_trigger_node.cpp) 18 | target_link_libraries(test_single_trigger_node nav2_single_trigger_bt_node) 19 | ament_target_dependencies(test_single_trigger_node ${dependencies}) 20 | -------------------------------------------------------------------------------- /navigation2/nav2_behavior_tree/test/test_dummy_tree_node.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // Copyright (c) 2020 Sarthak Mittal 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #ifndef TEST_DUMMY_TREE_NODE_HPP_ 17 | #define TEST_DUMMY_TREE_NODE_HPP_ 18 | 19 | #include 20 | #include 21 | 22 | namespace nav2_behavior_tree 23 | { 24 | 25 | /** 26 | * @brief A Dummy TreeNode to be used as a child for testing nodes 27 | * Returns the current status on tick without any execution logic 28 | */ 29 | class DummyNode : public BT::ActionNodeBase 30 | { 31 | public: 32 | DummyNode() 33 | : BT::ActionNodeBase("dummy", {}) 34 | { 35 | } 36 | 37 | void changeStatus(BT::NodeStatus status) 38 | { 39 | setStatus(status); 40 | } 41 | 42 | BT::NodeStatus executeTick() override 43 | { 44 | return tick(); 45 | } 46 | 47 | BT::NodeStatus tick() override 48 | { 49 | return status(); 50 | } 51 | 52 | void halt() override 53 | { 54 | } 55 | }; 56 | 57 | } // namespace nav2_behavior_tree 58 | 59 | #endif // TEST_DUMMY_TREE_NODE_HPP_ 60 | -------------------------------------------------------------------------------- /navigation2/nav2_bringup/bringup/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bringup/bringup/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_bringup/bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_bringup) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(navigation2 REQUIRED) 7 | 8 | nav2_package() 9 | 10 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 11 | install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) 12 | 13 | if(BUILD_TESTING) 14 | find_package(ament_lint_auto REQUIRED) 15 | ament_lint_auto_find_test_dependencies() 16 | endif() 17 | 18 | ament_package() 19 | -------------------------------------------------------------------------------- /navigation2/nav2_bringup/bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_bringup 5 | 1.0.8 6 | Bringup scripts and configurations for the Nav2 stack 7 | Michael Jeronimo 8 | Steve Macenski 9 | Carlos Orduno 10 | Apache-2.0 11 | 12 | ament_cmake 13 | nav2_common 14 | 15 | navigation2 16 | launch_ros 17 | 18 | launch_ros 19 | navigation2 20 | nav2_common 21 | slam_toolbox 22 | 23 | ament_lint_common 24 | ament_lint_auto 25 | ament_cmake_gtest 26 | ament_cmake_pytest 27 | launch 28 | launch_testing 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/behavior_trees/follow_point.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/doc/follow_point.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/doc/follow_point.png -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/doc/legend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/doc/legend.png -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/doc/navigate_w_replanning_distance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/doc/navigate_w_replanning_distance.png -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/doc/navigate_w_replanning_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/doc/navigate_w_replanning_time.png -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/doc/parallel_w_recovery.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/doc/parallel_w_recovery.png -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/doc/recovery_node.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/doc/recovery_node.png -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/doc/recovery_w_goal_updated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_bt_navigator/doc/recovery_w_goal_updated.png -------------------------------------------------------------------------------- /navigation2/nav2_bt_navigator/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include "cyberdog_debug/backtrace.hpp" 17 | #include "nav2_bt_navigator/bt_navigator.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | cyberdog::debug::register_signal(); 23 | rclcpp::init(argc, argv); 24 | auto node = std::make_shared(); 25 | rclcpp::spin(node->get_node_base_interface()); 26 | rclcpp::shutdown(); 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /navigation2/nav2_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | 3 | project(nav2_common NONE) 4 | 5 | find_package(ament_cmake_core REQUIRED) 6 | find_package(ament_cmake_python REQUIRED) 7 | 8 | ament_python_install_package(nav2_common) 9 | 10 | ament_package( 11 | CONFIG_EXTRAS "nav2_common-extras.cmake" 12 | ) 13 | 14 | install( 15 | DIRECTORY cmake 16 | DESTINATION share/${PROJECT_NAME} 17 | ) 18 | -------------------------------------------------------------------------------- /navigation2/nav2_common/nav2_common-extras.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | set(AMENT_BUILD_CONFIGURATION_KEYWORD_SEPARATOR ":") 16 | 17 | include("${nav2_common_DIR}/nav2_package.cmake") 18 | -------------------------------------------------------------------------------- /navigation2/nav2_common/nav2_common/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_common/nav2_common/__init__.py -------------------------------------------------------------------------------- /navigation2/nav2_common/nav2_common/launch/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2019 Intel Corporation 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .has_node_params import HasNodeParams 16 | from .rewritten_yaml import RewrittenYaml 17 | from .replace_string import ReplaceString 18 | -------------------------------------------------------------------------------- /navigation2/nav2_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_common 5 | 1.0.8 6 | Common support functionality used throughout the navigation 2 stack 7 | Carl Delsey 8 | Apache-2.0 9 | 10 | launch 11 | launch_ros 12 | osrf_pycommon 13 | rclpy 14 | python3-yaml 15 | 16 | ament_cmake_core 17 | 18 | ament_cmake_python 19 | 20 | ament_cmake_core 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /navigation2/nav2_controller/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Controller 2 | 3 | The Nav2 Controller is an [Execution Module](../doc/requirements/requirements.md) that implements the `nav2_msgs::action::FollowPath` action server. 4 | 5 | An execution module implementing the `nav2_msgs::action::FollowPath` action server is responsible for generating command velocities for the robot, given the computed path from the planner module in `nav2_planner`. The nav2_controller package is designed to be loaded with plugins for path execution. The plugins need to implement functions in the virtual base class defined in the `controller` header file in `nav2_core` package. 6 | 7 | 8 | Currently available controller plugins are: DWB, and [TEB (dashing release)](https://github.com/rst-tu-dortmund/teb_local_planner/tree/dashing-devel). -------------------------------------------------------------------------------- /navigation2/nav2_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_controller 5 | 1.0.8 6 | Controller action interface 7 | Carl Delsey 8 | Apache-2.0 9 | 10 | ament_cmake 11 | nav2_common 12 | angles 13 | rclcpp 14 | rclcpp_action 15 | std_msgs 16 | nav2_util 17 | nav2_msgs 18 | nav_2d_utils 19 | nav_2d_msgs 20 | nav2_core 21 | pluginlib 22 | 23 | ament_lint_common 24 | ament_lint_auto 25 | ament_cmake_gtest 26 | ament_cmake_pytest 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /navigation2/nav2_controller/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Checks if distance between current and previous pose is above a threshold 5 | 6 | 7 | 8 | 9 | Checks if current pose is within goal window for x,y and yaw 10 | 11 | 12 | 13 | 14 | Checks linear and angular velocity after stopping 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /navigation2/nav2_controller/plugins/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(pctest progress_checker.cpp) 2 | target_link_libraries(pctest simple_progress_checker) 3 | ament_add_gtest(gctest goal_checker.cpp) 4 | target_link_libraries(gctest simple_goal_checker stopped_goal_checker) 5 | -------------------------------------------------------------------------------- /navigation2/nav2_controller/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "nav2_controller/nav2_controller.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /navigation2/nav2_core/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_core) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | find_package(rclcpp_lifecycle REQUIRED) 8 | find_package(std_msgs REQUIRED) 9 | find_package(geometry_msgs REQUIRED) 10 | find_package(nav2_costmap_2d REQUIRED) 11 | find_package(pluginlib REQUIRED) 12 | find_package(nav_msgs REQUIRED) 13 | find_package(tf2_ros REQUIRED) 14 | find_package(nav2_util REQUIRED) 15 | find_package(nav_2d_msgs REQUIRED) 16 | find_package(protocol REQUIRED) 17 | 18 | nav2_package() 19 | 20 | set(dependencies 21 | rclcpp 22 | rclcpp_lifecycle 23 | std_msgs 24 | geometry_msgs 25 | nav2_costmap_2d 26 | pluginlib 27 | visualization_msgs 28 | nav_msgs 29 | tf2_ros 30 | nav_2d_msgs 31 | protocol 32 | ) 33 | 34 | install(DIRECTORY include/ 35 | DESTINATION include/ 36 | ) 37 | 38 | if(BUILD_TESTING) 39 | find_package(ament_lint_auto REQUIRED) 40 | # the following line skips the linter which checks for copyrights 41 | set(ament_cmake_copyright_FOUND TRUE) 42 | ament_lint_auto_find_test_dependencies() 43 | endif() 44 | 45 | ament_export_include_directories(include) 46 | ament_export_dependencies(${dependencies}) 47 | 48 | ament_package() 49 | -------------------------------------------------------------------------------- /navigation2/nav2_core/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Core 2 | 3 | This package hosts the abstract interface (virtual base classes) for plugins to be used with the following: 4 | - global planner (e.g., `nav2_navfn_planner`) 5 | - controller (i.e, path execution controller, e.g `nav2_dwb_controller`) 6 | - goal checker 7 | - recovery behaviors 8 | -------------------------------------------------------------------------------- /navigation2/nav2_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_core 5 | 1.0.8 6 | A set of headers for plugins core to the Nav2 stack 7 | Steve Macenski 8 | Carl Delsey 9 | Apache-2.0 10 | 11 | ament_cmake 12 | nav2_common 13 | 14 | rclcpp 15 | rclcpp_lifecycle 16 | std_msgs 17 | geometry_msgs 18 | nav2_costmap_2d 19 | pluginlib 20 | nav_msgs 21 | tf2_ros 22 | nav2_util 23 | nav_2d_msgs 24 | protocol 25 | 26 | ament_lint_common 27 | ament_lint_auto 28 | ament_cmake_gtest 29 | ament_cmake_pytest 30 | launch 31 | launch_testing 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package nav2_costmap_2d 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | (2018-10-9) 6 | ------------------- 7 | * Port nav2_costmap_2d from costmap_2d version 1.16.2 (2018-07-31) 8 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/cfg/Costmap2D.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) 8 | gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) 9 | gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100) 10 | 11 | #map params 12 | gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) 13 | gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) 14 | gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) 15 | gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) 16 | gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) 17 | 18 | # robot footprint shape 19 | gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") 20 | 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) 21 | gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) 22 | 23 | exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D")) 24 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/cfg/GenericPlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t 4 | 5 | gen = ParameterGenerator() 6 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) 7 | exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin")) 8 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/cfg/InflationPlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) 8 | gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) 9 | gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) 10 | gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) 11 | 12 | exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin")) 13 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/cfg/ObstaclePlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) 8 | gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) 9 | gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) 10 | 11 | combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), 12 | gen.const("Maximum", int_t, 1, "Take the maximum of the values"), 13 | gen.const("Nothing", int_t, 99, "Do nothing")], 14 | "Method for combining layers enum") 15 | gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) 16 | 17 | 18 | # gen.add("obstacle_max_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) 19 | # gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50) 20 | # gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) 21 | # gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50) 22 | exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) 23 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/cfg/VoxelPlugin.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t 4 | 5 | gen = ParameterGenerator() 6 | 7 | gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) 8 | gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) 9 | gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) 10 | gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) 11 | gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) 12 | gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) 13 | gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) 14 | gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) 15 | 16 | combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), 17 | gen.const("Maximum", int_t, 1, "Take the maximum of the values"), 18 | gen.const("Nothing", int_t, 99, "Do nothing")], 19 | "Method for combining layers enum") 20 | gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) 21 | 22 | exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin")) 23 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/costmap_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles. 5 | 6 | 7 | Listens to laser scan and point cloud messages and marks and clears grid cells. 8 | 9 | 10 | Listens to OccupancyGrid messages and copies them in, like from map_server. 11 | 12 | 13 | Similar to obstacle costmap, but uses 3D voxel grid to store data. 14 | 15 | 16 | A range-sensor (sonar, IR) based obstacle layer for costmap_2d 17 | 18 | 19 | 20 | 21 | 22 | Prevents the robot from appearing in keepout zones marked on map. 23 | 24 | 25 | Restricts maximum speed of robot in speed-limit zones marked on map. 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/launch/example_params.yaml: -------------------------------------------------------------------------------- 1 | global_frame: map 2 | robot_base_frame: base_link 3 | update_frequency: 5.0 4 | publish_frequency: 1.0 5 | 6 | #set if you want the voxel map published 7 | publish_voxel_map: true 8 | 9 | #set to true if you want to initialize the costmap from a static map 10 | static_map: false 11 | 12 | #begin - COMMENT these lines if you set static_map to true 13 | rolling_window: true 14 | width: 6.0 15 | height: 6.0 16 | resolution: 0.025 17 | #end - COMMENT these lines if you set static_map to true 18 | 19 | #START VOXEL STUFF 20 | map_type: voxel 21 | origin_z: 0.0 22 | z_resolution: 0.2 23 | z_voxels: 10 24 | unknown_threshold: 10 25 | mark_threshold: 0 26 | #END VOXEL STUFF 27 | 28 | transform_tolerance: 0.3 29 | obstacle_max_range: 2.5 30 | obstacle_min_range: 0.0 31 | max_obstacle_height: 2.0 32 | raytrace_max_range: 3.0 33 | raytrace_min_range: 0.0 34 | 35 | footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 36 | #robot_radius: 0.46 37 | footprint_padding: 0.01 38 | inflation_radius: 0.55 39 | cost_scaling_factor: 10.0 40 | lethal_cost_threshold: 100 41 | observation_sources: base_scan 42 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, 43 | observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 44 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(TEST_MAP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/map) 2 | set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) 3 | 4 | add_subdirectory(unit) 5 | add_subdirectory(integration) 6 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_frame: map 2 | robot_base_frame: base_link 3 | update_frequency: 5.0 4 | publish_frequency: 0.0 5 | static_map: true 6 | rolling_window: false 7 | 8 | #START VOXEL STUFF 9 | map_type: voxel 10 | origin_z: 0.0 11 | z_resolution: 0.2 12 | z_voxels: 10 13 | unknown_threshold: 10 14 | mark_threshold: 0 15 | #END VOXEL STUFF 16 | 17 | transform_tolerance: 0.3 18 | obstacle_max_range: 2.5 19 | obstacle_min_range: 0.0 20 | max_obstacle_height: 2.0 21 | raytrace_max_range: 3.0 22 | raytrace_min_range: 0.0 23 | footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] 24 | #robot_radius: 0.46 25 | footprint_padding: 0.01 26 | inflation_radius: 0.55 27 | cost_scaling_factor: 10.0 28 | lethal_cost_threshold: 100 29 | observation_sources: base_scan 30 | base_scan: {data_type: LaserScan, expected_update_rate: 0.4, 31 | observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} 32 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/map/TenByTen.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_costmap_2d/test/map/TenByTen.pgm -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/map/TenByTen.yaml: -------------------------------------------------------------------------------- 1 | image: TenByTen.pgm 2 | resolution: 1.0 3 | origin: [0,0,0] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/simple_driving_test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Intel Corporation 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | 19 | from launch import LaunchDescription 20 | import launch_ros.actions 21 | 22 | 23 | def generate_launch_description(): 24 | mapFile = os.getenv('TEST_MAP') 25 | return LaunchDescription([ 26 | launch_ros.actions.Node( 27 | package='nav2_map_server', 28 | executable='map_server', 29 | name='map_server', 30 | output='screen', 31 | parameters=[{'yaml_filename': mapFile}]) 32 | ]) 33 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(array_parser_test array_parser_test.cpp) 2 | target_link_libraries(array_parser_test 3 | nav2_costmap_2d_core 4 | ) 5 | 6 | ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) 7 | target_link_libraries(collision_footprint_test 8 | nav2_costmap_2d_core 9 | ) 10 | 11 | ament_add_gtest(costmap_convesion_test costmap_conversion_test.cpp) 12 | target_link_libraries(costmap_convesion_test 13 | nav2_costmap_2d_core 14 | ) 15 | 16 | ament_add_gtest(declare_parameter_test declare_parameter_test.cpp) 17 | target_link_libraries(declare_parameter_test 18 | nav2_costmap_2d_core 19 | ) 20 | 21 | ament_add_gtest(keepout_filter_test keepout_filter_test.cpp) 22 | target_link_libraries(keepout_filter_test 23 | nav2_costmap_2d_core 24 | filters 25 | ) 26 | 27 | ament_add_gtest(speed_filter_test speed_filter_test.cpp) 28 | target_link_libraries(speed_filter_test 29 | nav2_costmap_2d_core 30 | filters 31 | ) 32 | 33 | ament_add_gtest(copy_window_test copy_window_test.cpp) 34 | target_link_libraries(copy_window_test 35 | nav2_costmap_2d_core 36 | ) 37 | -------------------------------------------------------------------------------- /navigation2/nav2_costmap_2d/test/unit/keepout_filter_test.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_costmap_2d/test/unit/keepout_filter_test.jpg -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/costmap_queue/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(costmap_queue) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(nav2_costmap_2d REQUIRED) 7 | find_package(rclcpp REQUIRED) 8 | 9 | nav2_package() 10 | 11 | include_directories( 12 | include 13 | ) 14 | 15 | add_library(${PROJECT_NAME} 16 | src/costmap_queue.cpp 17 | src/limited_costmap_queue.cpp 18 | ) 19 | 20 | set(dependencies 21 | rclcpp 22 | nav2_costmap_2d 23 | ) 24 | 25 | ament_target_dependencies(${PROJECT_NAME} 26 | ${dependencies} 27 | ) 28 | 29 | if(BUILD_TESTING) 30 | find_package(ament_lint_auto REQUIRED) 31 | # the following line skips the linter which checks for copyrights 32 | set(ament_cmake_copyright_FOUND TRUE) 33 | ament_lint_auto_find_test_dependencies() 34 | 35 | find_package(ament_cmake_gtest REQUIRED) 36 | 37 | ament_add_gtest(mbq_test test/mbq_test.cpp) 38 | ament_target_dependencies(mbq_test ${dependencies}) 39 | 40 | ament_add_gtest(utest test/utest.cpp) 41 | ament_target_dependencies(utest ${dependencies}) 42 | target_link_libraries(utest ${PROJECT_NAME}) 43 | endif() 44 | 45 | install(TARGETS ${PROJECT_NAME} 46 | ARCHIVE DESTINATION lib 47 | LIBRARY DESTINATION lib 48 | RUNTIME DESTINATION bin 49 | ) 50 | install(DIRECTORY include/ 51 | DESTINATION include/ 52 | ) 53 | 54 | ament_export_include_directories(include) 55 | ament_export_libraries(${PROJECT_NAME}) 56 | 57 | ament_package() 58 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/costmap_queue/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | costmap_queue 4 | 1.0.8 5 | The costmap_queue package 6 | David V. Lu!! 7 | BSD-3-Clause 8 | 9 | ament_cmake 10 | nav2_common 11 | 12 | nav2_costmap_2d 13 | rclcpp 14 | 15 | ament_lint_common 16 | ament_lint_auto 17 | ament_cmake_gtest 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_core/local_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_core/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(utils_test utils_test.cpp) 2 | target_link_libraries(utils_test dwb_core) 3 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_critics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dwb_critics 4 | 1.0.8 5 | The dwb_critics package 6 | David V. Lu!! 7 | BSD-3-Clause 8 | 9 | ament_cmake 10 | nav2_common 11 | 12 | angles 13 | nav2_costmap_2d 14 | nav2_util 15 | costmap_queue 16 | dwb_core 17 | geometry_msgs 18 | nav_2d_msgs 19 | nav_2d_utils 20 | pluginlib 21 | rclcpp 22 | sensor_msgs 23 | 24 | ament_lint_common 25 | ament_lint_auto 26 | ament_cmake_gtest 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_critics/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(prefer_forward_tests prefer_forward_test.cpp) 2 | target_link_libraries(prefer_forward_tests dwb_critics) 3 | 4 | ament_add_gtest(base_obstacle_tests base_obstacle_test.cpp) 5 | target_link_libraries(base_obstacle_tests dwb_critics) 6 | 7 | ament_add_gtest(obstacle_footprint_tests obstacle_footprint_test.cpp) 8 | target_link_libraries(obstacle_footprint_tests dwb_critics) 9 | 10 | ament_add_gtest(alignment_util_tests alignment_util_test.cpp) 11 | target_link_libraries(alignment_util_tests dwb_critics) 12 | 13 | ament_add_gtest(twirling_tests twirling_test.cpp) 14 | target_link_libraries(twirling_tests dwb_critics) 15 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(dwb_msgs) 3 | 4 | find_package(builtin_interfaces REQUIRED) 5 | find_package(geometry_msgs REQUIRED) 6 | find_package(nav_2d_msgs REQUIRED) 7 | find_package(nav_msgs REQUIRED) 8 | find_package(std_msgs REQUIRED) 9 | find_package(rosidl_default_generators REQUIRED) 10 | 11 | rosidl_generate_interfaces(dwb_msgs 12 | "msg/CriticScore.msg" 13 | "msg/LocalPlanEvaluation.msg" 14 | "msg/Trajectory2D.msg" 15 | "msg/TrajectoryScore.msg" 16 | "srv/DebugLocalPlan.srv" 17 | "srv/GenerateTrajectory.srv" 18 | "srv/GenerateTwists.srv" 19 | "srv/GetCriticScore.srv" 20 | "srv/ScoreTrajectory.srv" 21 | DEPENDENCIES geometry_msgs std_msgs nav_2d_msgs nav_msgs builtin_interfaces 22 | ) 23 | 24 | ament_export_dependencies(rosidl_default_runtime) 25 | 26 | ament_package() 27 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/msg/CriticScore.msg: -------------------------------------------------------------------------------- 1 | # The result from one critic scoring a Twist. 2 | # Name of the critic 3 | string name 4 | # Score for the critic, not multiplied by the scale 5 | float32 raw_score 6 | # Scale for the critic, multiplied by the raw_score and added to the total score 7 | float32 scale 8 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/msg/LocalPlanEvaluation.msg: -------------------------------------------------------------------------------- 1 | # Full Scoring for running the local planner 2 | 3 | # Header, used for timestamp 4 | std_msgs/Header header 5 | # All the trajectories evaluated and their scores 6 | TrajectoryScore[] twists 7 | # Convenience index of the best (lowest) score in the twists array 8 | uint16 best_index 9 | # Convenience index of the worst (highest) score in the twists array. Useful for scaling. 10 | uint16 worst_index 11 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg: -------------------------------------------------------------------------------- 1 | # For a given velocity command, the poses that the robot will go to in the allotted time. 2 | 3 | # Input Velocity 4 | nav_2d_msgs/Twist2D velocity 5 | # Time difference between first and last poses 6 | builtin_interfaces/Duration[] time_offsets 7 | # Poses the robot will go to, given our kinematic model 8 | geometry_msgs/Pose2D[] poses 9 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/msg/TrajectoryScore.msg: -------------------------------------------------------------------------------- 1 | # Complete scoring for a given twist. 2 | 3 | # The trajectory being scored 4 | Trajectory2D traj 5 | # The Scores for each of the critics employed 6 | CriticScore[] scores 7 | # Convenience member that totals the critic scores 8 | float32 total 9 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dwb_msgs 5 | 1.0.8 6 | Message/Service definitions specifically for the dwb_core 7 | David V. Lu!! 8 | BSD-3-Clause 9 | 10 | ament_cmake 11 | 12 | builtin_interfaces 13 | geometry_msgs 14 | nav_2d_msgs 15 | std_msgs 16 | nav_msgs 17 | rosidl_default_runtime 18 | 19 | rosidl_interface_packages 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/srv/DebugLocalPlan.srv: -------------------------------------------------------------------------------- 1 | # For a given pose velocity and global_plan, run the local planner and return full results 2 | nav_2d_msgs/Pose2DStamped pose 3 | nav_2d_msgs/Twist2D velocity 4 | nav_2d_msgs/Path2D global_plan 5 | --- 6 | LocalPlanEvaluation results 7 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/srv/GenerateTrajectory.srv: -------------------------------------------------------------------------------- 1 | # For a given start pose, velocity and desired velocity, generate which poses will be visited 2 | geometry_msgs/Pose2D start_pose 3 | nav_2d_msgs/Twist2D start_vel 4 | nav_2d_msgs/Twist2D cmd_vel 5 | --- 6 | Trajectory2D traj 7 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/srv/GenerateTwists.srv: -------------------------------------------------------------------------------- 1 | # For a given velocity, generate which twist commands will be evaluated 2 | nav_2d_msgs/Twist2D current_vel 3 | --- 4 | nav_2d_msgs/Twist2D[] twists 5 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/srv/GetCriticScore.srv: -------------------------------------------------------------------------------- 1 | nav_2d_msgs/Pose2DStamped pose 2 | nav_2d_msgs/Twist2D velocity 3 | nav_2d_msgs/Path2D global_plan 4 | Trajectory2D traj 5 | string critic_name 6 | --- 7 | CriticScore score 8 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_msgs/srv/ScoreTrajectory.srv: -------------------------------------------------------------------------------- 1 | nav_2d_msgs/Pose2DStamped pose 2 | nav_2d_msgs/Twist2D velocity 3 | nav_2d_msgs/Path2D global_plan 4 | Trajectory2D traj 5 | --- 6 | TrajectoryScore score 7 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dwb_plugins 4 | 1.0.8 5 | 6 | Standard implementations of the GoalChecker 7 | and TrajectoryGenerators for dwb_core 8 | 9 | David V. Lu!! 10 | BSD-3-Clause 11 | 12 | ament_cmake 13 | nav2_common 14 | 15 | angles 16 | dwb_core 17 | nav_2d_msgs 18 | nav_2d_utils 19 | pluginlib 20 | rclcpp 21 | nav2_util 22 | 23 | ament_lint_common 24 | ament_lint_auto 25 | ament_cmake_gtest 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_plugins/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/dwb_plugins/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(vtest velocity_iterator_test.cpp) 2 | 3 | ament_add_gtest(twist_gen_test twist_gen.cpp) 4 | target_link_libraries(twist_gen_test standard_traj_generator) 5 | 6 | ament_add_gtest(kinematic_parameters_test kinematic_parameters_test.cpp) 7 | target_link_libraries(kinematic_parameters_test standard_traj_generator) 8 | 9 | ament_add_gtest(speed_limit_test speed_limit_test.cpp) 10 | target_link_libraries(speed_limit_test standard_traj_generator) 11 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav2_dwb_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_dwb_controller/nav2_dwb_controller/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav2_dwb_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_dwb_controller) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 14) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | 10 | ament_package() 11 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav2_dwb_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_dwb_controller 5 | 1.0.8 6 | 7 | ROS2 controller (DWB) metapackage 8 | 9 | Carl Delsey 10 | Steve Macenski 11 | Apache-2.0 12 | 13 | ament_cmake 14 | 15 | costmap_queue 16 | dwb_core 17 | dwb_critics 18 | dwb_msgs 19 | dwb_plugins 20 | nav_2d_msgs 21 | nav_2d_utils 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav_2d_msgs) 3 | 4 | find_package(geometry_msgs REQUIRED) 5 | find_package(std_msgs REQUIRED) 6 | find_package(rosidl_default_generators REQUIRED) 7 | 8 | rosidl_generate_interfaces(nav_2d_msgs 9 | "msg/Path2D.msg" 10 | "msg/Pose2D32.msg" 11 | "msg/Pose2DStamped.msg" 12 | "msg/Twist2D.msg" 13 | "msg/Twist2D32.msg" 14 | "msg/Twist2DStamped.msg" 15 | DEPENDENCIES geometry_msgs std_msgs 16 | ) 17 | 18 | ament_export_dependencies(rosidl_default_runtime) 19 | 20 | ament_package() 21 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Path2D.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Pose2D[] poses 3 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Pose2D32.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta 4 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Pose2DStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | geometry_msgs/Pose2D pose 3 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Twist2D.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 theta 4 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Twist2D32.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 theta 4 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/msg/Twist2DStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Twist2D velocity 3 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav_2d_msgs 5 | 1.0.8 6 | Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. 7 | David V. Lu!! 8 | BSD-3-Clause 9 | 10 | rosidl_default_runtime 11 | 12 | ament_cmake 13 | 14 | geometry_msgs 15 | std_msgs 16 | rosidl_default_generators 17 | 18 | rosidl_interface_packages 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav_2d_utils 5 | 1.0.8 6 | A handful of useful utility functions for nav_2d packages. 7 | David V. Lu!! 8 | BSD-3-Clause 9 | 10 | ament_cmake 11 | nav2_common 12 | 13 | geometry_msgs 14 | nav_2d_msgs 15 | nav_msgs 16 | std_msgs 17 | tf2 18 | tf2_geometry_msgs 19 | nav2_msgs 20 | nav2_util 21 | 22 | ament_lint_common 23 | ament_lint_auto 24 | ament_cmake_gtest 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /navigation2/nav2_dwb_controller/nav_2d_utils/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(2d_utils_tests 2d_utils_test.cpp) 2 | target_link_libraries(2d_utils_tests conversions) 3 | 4 | ament_add_gtest(path_ops_tests path_ops_test.cpp) 5 | target_link_libraries(path_ops_tests path_ops) 6 | 7 | ament_add_gtest(tf_help_tests tf_help_test.cpp) 8 | target_link_libraries(tf_help_tests tf_help conversions) 9 | 10 | -------------------------------------------------------------------------------- /navigation2/nav2_lifecycle_manager/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_lifecycle_manager/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_lifecycle_manager/doc/diagram_lifecycle_manager.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_lifecycle_manager/doc/diagram_lifecycle_manager.JPG -------------------------------------------------------------------------------- /navigation2/nav2_lifecycle_manager/doc/uml_lifecycle_manager.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_lifecycle_manager/doc/uml_lifecycle_manager.JPG -------------------------------------------------------------------------------- /navigation2/nav2_lifecycle_manager/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include "cyberdog_debug/backtrace.hpp" 17 | #include "nav2_lifecycle_manager/lifecycle_manager.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | cyberdog::debug::register_signal(); 23 | rclcpp::init(argc, argv); 24 | auto node = std::make_shared(); 25 | rclcpp::spin(node); 26 | rclcpp::shutdown(); 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /navigation2/nav2_lifecycle_manager/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest_executable(test_lifecycle_gtest 2 | test_lifecycle_manager.cpp 3 | ) 4 | 5 | target_link_libraries(test_lifecycle_gtest 6 | ${library_name} 7 | ) 8 | 9 | ament_target_dependencies(test_lifecycle_gtest 10 | ${dependencies} 11 | ) 12 | 13 | ament_add_test(test_lifecycle 14 | GENERATE_RESULT_FOR_RETURN_CODE_ZERO 15 | COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py" 16 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" 17 | TIMEOUT 20 18 | ENV 19 | TEST_EXECUTABLE=$ 20 | ) 21 | 22 | ament_add_gtest_executable(test_bond_gtest 23 | test_bond.cpp 24 | ) 25 | 26 | target_link_libraries(test_bond_gtest 27 | ${library_name} 28 | ) 29 | 30 | ament_target_dependencies(test_bond_gtest 31 | ${dependencies} 32 | ) 33 | 34 | ament_add_test(test_bond 35 | GENERATE_RESULT_FOR_RETURN_CODE_ZERO 36 | COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_bond_test.py" 37 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" 38 | TIMEOUT 20 39 | ENV 40 | TEST_EXECUTABLE=$ 41 | ) 42 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_map_server/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake: -------------------------------------------------------------------------------- 1 | # Copyright 2019 Rover Robotics 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # CMake script for finding Magick++, the C++ interface for the 16 | # GraphicsMagick library 17 | # 18 | # Output variables: 19 | # GRAPHICSMAGICKCPP_FOUND - system has GraphicsMagick Magick++ 20 | # GRAPHICSMAGICKCPP_INCLUDE_DIRS - include directories for Magick++ 21 | # GRAPHICSMAGICKCPP_LIBRARIES - libraries you need to link to 22 | include(FindPackageHandleStandardArgs) 23 | 24 | find_path(GRAPHICSMAGICKCPP_INCLUDE_DIRS 25 | NAMES "Magick++.h" 26 | PATH_SUFFIXES GraphicsMagick) 27 | 28 | find_library(GRAPHICSMAGICKCPP_LIBRARIES 29 | NAMES "GraphicsMagick++" "graphicsmagick") 30 | 31 | find_package_handle_standard_args( 32 | GRAPHICSMAGICKCPP 33 | GRAPHICSMAGICKCPP_LIBRARIES 34 | GRAPHICSMAGICKCPP_INCLUDE_DIRS) -------------------------------------------------------------------------------- /navigation2/nav2_map_server/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_map_server 5 | 1.0.8 6 | 7 | Refactored map server for ROS2 Navigation 8 | 9 | Brian Wilcox 10 | Apache-2.0 11 | BSD-3-Clause 12 | 13 | ament_cmake 14 | 15 | nav2_common 16 | 17 | rclcpp_lifecycle 18 | nav_msgs 19 | std_msgs 20 | rclcpp 21 | yaml_cpp_vendor 22 | launch_ros 23 | launch_testing 24 | tf2 25 | nav2_msgs 26 | nav2_util 27 | graphicsmagick 28 | 29 | ament_lint_common 30 | ament_lint_auto 31 | ament_cmake_gtest 32 | ament_cmake_pytest 33 | launch 34 | launch_testing 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/src/costmap_filter_info/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Samsung Research Russia 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "nav2_map_server/costmap_filter_info_server.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char * argv[]) 21 | { 22 | auto logger = rclcpp::get_logger("costmap_filter_info_server"); 23 | 24 | RCLCPP_INFO(logger, "This is costmap filter info publisher"); 25 | 26 | rclcpp::init(argc, argv); 27 | auto node = std::make_shared(); 28 | rclcpp::spin(node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/src/map_mode.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2019 Rover Robotics 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "nav2_map_server/map_mode.hpp" 16 | 17 | #include 18 | #include 19 | 20 | namespace nav2_map_server 21 | { 22 | const char * map_mode_to_string(MapMode map_mode) 23 | { 24 | switch (map_mode) { 25 | case MapMode::Trinary: 26 | return "trinary"; 27 | case MapMode::Scale: 28 | return "scale"; 29 | case MapMode::Raw: 30 | return "raw"; 31 | default: 32 | throw std::invalid_argument("map_mode"); 33 | } 34 | } 35 | 36 | MapMode map_mode_from_string(std::string map_mode_name) 37 | { 38 | for (auto & c : map_mode_name) { 39 | c = tolower(c); 40 | } 41 | 42 | if (map_mode_name == "scale") { 43 | return MapMode::Scale; 44 | } else if (map_mode_name == "raw") { 45 | return MapMode::Raw; 46 | } else if (map_mode_name == "trinary") { 47 | return MapMode::Trinary; 48 | } else { 49 | throw std::invalid_argument("map_mode_name"); 50 | } 51 | } 52 | } // namespace nav2_map_server 53 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/src/map_saver/main_server.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Samsung Research Russia 2 | // Copyright (c) 2018 Intel Corporation 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "nav2_map_server/map_saver.hpp" 21 | #include "rclcpp/rclcpp.hpp" 22 | 23 | int main(int argc, char ** argv) 24 | { 25 | rclcpp::init(argc, argv); 26 | 27 | auto logger = rclcpp::get_logger("map_saver_server"); 28 | auto service_node = std::make_shared(); 29 | rclcpp::spin(service_node->get_node_base_interface()); 30 | rclcpp::shutdown(); 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/src/map_server/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "nav2_map_server/map_server.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | int main(int argc, char ** argv) 23 | { 24 | std::string node_name("map_server"); 25 | 26 | rclcpp::init(argc, argv); 27 | auto node = std::make_shared(); 28 | rclcpp::spin(node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | } 31 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) 2 | set(TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR}) 3 | 4 | add_definitions( -DTEST_DIRECTORY=\"${CMAKE_CURRENT_SOURCE_DIR}\") 5 | 6 | add_subdirectory(unit) 7 | add_subdirectory(component) 8 | add_subdirectory(map_saver_cli) 9 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/component/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${PROJECT_SOURCE_DIR}/test) 2 | 3 | # map_server component test 4 | ament_add_gtest_executable(test_map_server_node 5 | test_map_server_node.cpp 6 | ${PROJECT_SOURCE_DIR}/test/test_constants.cpp 7 | ) 8 | ament_target_dependencies(test_map_server_node rclcpp nav_msgs) 9 | target_link_libraries(test_map_server_node 10 | ${library_name} 11 | stdc++fs 12 | ) 13 | 14 | ament_add_test(test_map_server_node 15 | GENERATE_RESULT_FOR_RETURN_CODE_ZERO 16 | COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_server_launch.py" 17 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" 18 | ENV 19 | TEST_DIR=${TEST_DIR} 20 | TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} 21 | TEST_EXECUTABLE=$ 22 | ) 23 | 24 | # map_saver component test 25 | ament_add_gtest_executable(test_map_saver_node 26 | test_map_saver_node.cpp 27 | ${PROJECT_SOURCE_DIR}/test/test_constants.cpp 28 | ) 29 | 30 | ament_target_dependencies(test_map_saver_node rclcpp nav_msgs) 31 | target_link_libraries(test_map_saver_node 32 | ${library_name} 33 | stdc++fs 34 | ) 35 | 36 | add_executable(test_map_saver_publisher 37 | test_map_saver_publisher.cpp 38 | ${PROJECT_SOURCE_DIR}/test/test_constants.cpp 39 | ) 40 | 41 | target_link_libraries(test_map_saver_publisher 42 | ${map_io_library_name} 43 | stdc++fs 44 | ) 45 | 46 | ament_add_test(test_map_saver_node 47 | GENERATE_RESULT_FOR_RETURN_CODE_ZERO 48 | COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_saver_launch.py" 49 | WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" 50 | ENV 51 | TEST_DIR=${TEST_DIR} 52 | TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} 53 | TEST_EXECUTABLE=$ 54 | ) 55 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/component/test_map_saver_launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Intel Corporation 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | import sys 19 | 20 | from launch import LaunchDescription 21 | from launch import LaunchService 22 | from launch.actions import ExecuteProcess 23 | from launch.actions import IncludeLaunchDescription 24 | from launch.launch_description_sources import PythonLaunchDescriptionSource 25 | from launch_testing.legacy import LaunchTestService 26 | 27 | 28 | def main(argv=sys.argv[1:]): 29 | launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py') 30 | testExecutable = os.getenv('TEST_EXECUTABLE') 31 | ld = LaunchDescription([ 32 | IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), 33 | ]) 34 | test1_action = ExecuteProcess( 35 | cmd=[testExecutable], 36 | name='test_map_saver_node', 37 | ) 38 | lts = LaunchTestService() 39 | lts.add_test_action(ld, test1_action) 40 | ls = LaunchService(argv=argv) 41 | ls.include_launch_description(ld) 42 | os.chdir(os.getenv('TEST_LAUNCH_DIR')) 43 | return lts.run(ls) 44 | 45 | 46 | if __name__ == '__main__': 47 | sys.exit(main()) 48 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/invalid_image.yaml: -------------------------------------------------------------------------------- 1 | image: "invalid.png" 2 | resolution: 0.1 3 | origin: [2.0, 3.0, 1.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | mode: "trinary" 8 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/map_saver_cli/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${PROJECT_SOURCE_DIR}/test) 2 | 3 | # map_saver CLI 4 | ament_add_gtest(test_map_saver_cli 5 | test_map_saver_cli.cpp 6 | ${PROJECT_SOURCE_DIR}/test/test_constants.cpp 7 | ) 8 | 9 | ament_target_dependencies(test_map_saver_cli rclcpp nav_msgs) 10 | target_link_libraries(test_map_saver_cli 11 | stdc++fs 12 | ${dependencies} 13 | ) 14 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/map_saver_params.yaml: -------------------------------------------------------------------------------- 1 | map_saver: 2 | ros__parameters: 3 | save_map_timeout: 5.0 4 | free_thresh_default: 0.196 5 | occupied_thresh_default: 0.65 6 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/map_server_params.yaml: -------------------------------------------------------------------------------- 1 | map_server: 2 | ros__parameters: 3 | yaml_filename: "../testmap.yaml" 4 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/test_launch_files/map_saver_node.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Intel Corporation 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | 19 | from launch import LaunchDescription 20 | from launch.actions import ExecuteProcess 21 | import launch_ros.actions 22 | 23 | 24 | def generate_launch_description(): 25 | map_publisher = f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher" 26 | 27 | ld = LaunchDescription() 28 | 29 | map_saver_server_cmd = launch_ros.actions.Node( 30 | package='nav2_map_server', 31 | executable='map_saver_server', 32 | output='screen', 33 | parameters=[os.path.join(os.getenv('TEST_DIR'), 34 | 'map_saver_params.yaml')]) 35 | 36 | map_publisher_cmd = ExecuteProcess( 37 | cmd=[map_publisher]) 38 | 39 | ld.add_action(map_saver_server_cmd) 40 | ld.add_action(map_publisher_cmd) 41 | 42 | return ld 43 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/test_launch_files/map_server_node.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright (c) 2018 Intel Corporation 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | 19 | from launch import LaunchDescription 20 | import launch_ros.actions 21 | 22 | 23 | def generate_launch_description(): 24 | return LaunchDescription([ 25 | launch_ros.actions.Node( 26 | package='nav2_map_server', 27 | executable='map_server', 28 | output='screen', 29 | parameters=[os.path.join(os.getenv('TEST_DIR'), 30 | 'map_server_params.yaml')]) 31 | ]) 32 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/testmap.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_map_server/test/testmap.bmp -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/testmap.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_map_server/test/testmap.pgm -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/testmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_map_server/test/testmap.png -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/testmap.yaml: -------------------------------------------------------------------------------- 1 | image: "testmap.png" 2 | resolution: 0.1 3 | origin: [2.0, 3.0, 1.0] 4 | occupied_thresh: 0.65 5 | free_thresh: 0.196 6 | negate: 0 7 | mode: "trinary" 8 | -------------------------------------------------------------------------------- /navigation2/nav2_map_server/test/unit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${PROJECT_SOURCE_DIR}/test) 2 | 3 | # map_io unit test 4 | ament_add_gtest(test_map_io test_map_io.cpp ${PROJECT_SOURCE_DIR}/test/test_constants.cpp) 5 | 6 | ament_target_dependencies(test_map_io rclcpp nav_msgs) 7 | 8 | target_link_libraries(test_map_io 9 | ${map_io_library_name} 10 | stdc++fs 11 | ) 12 | 13 | # costmap_filter_info_server unit test 14 | ament_add_gtest(test_costmap_filter_info_server 15 | test_costmap_filter_info_server.cpp 16 | ) 17 | 18 | ament_target_dependencies(test_costmap_filter_info_server rclcpp) 19 | 20 | target_link_libraries(test_costmap_filter_info_server 21 | ${library_name} 22 | ) 23 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_msgs/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_msgs) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(builtin_interfaces REQUIRED) 7 | find_package(nav_msgs REQUIRED) 8 | find_package(geometry_msgs REQUIRED) 9 | find_package(rosidl_default_generators REQUIRED) 10 | find_package(std_msgs REQUIRED) 11 | find_package(action_msgs REQUIRED) 12 | 13 | nav2_package() 14 | 15 | rosidl_generate_interfaces(${PROJECT_NAME} 16 | "msg/Costmap.msg" 17 | "msg/CostmapMetaData.msg" 18 | "msg/CostmapFilterInfo.msg" 19 | "msg/SpeedLimit.msg" 20 | "msg/VoxelGrid.msg" 21 | "msg/BehaviorTreeStatusChange.msg" 22 | "msg/BehaviorTreeLog.msg" 23 | "msg/Particle.msg" 24 | "msg/ParticleCloud.msg" 25 | "srv/GetCostmap.srv" 26 | "srv/ClearCostmapExceptRegion.srv" 27 | "srv/ClearCostmapAroundRobot.srv" 28 | "srv/ClearEntireCostmap.srv" 29 | "srv/ManageLifecycleNodes.srv" 30 | "srv/LoadMap.srv" 31 | "srv/SaveMap.srv" 32 | "action/BackUp.action" 33 | "action/ComputePathToPose.action" 34 | "action/ComputePathThroughPoses.action" 35 | "action/FollowPath.action" 36 | "action/NavigateToPose.action" 37 | "action/NavigateThroughPoses.action" 38 | "action/Wait.action" 39 | "action/Spin.action" 40 | "action/DummyRecovery.action" 41 | "action/FollowWaypoints.action" 42 | DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs 43 | ) 44 | 45 | ament_export_dependencies(rosidl_default_runtime) 46 | 47 | ament_package() 48 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/README.md: -------------------------------------------------------------------------------- 1 | # nav2_msgs 2 | 3 | The `nav2_msgs` package is a set of messages, services, and actions for the `Nav2` system. `Nav2` still makes use of `nav_msgs` from ROS (1) Navigation. 4 | 5 | See the ROS 1 to ROS 2 [Migration Guide](https://index.ros.org/doc/ros2/Migration-Guide/#messages-and-services) for details about use of the new message and service types. 6 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/BackUp.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/Point target 3 | float32 speed 4 | --- 5 | #result definition 6 | builtin_interfaces/Duration total_elapsed_time 7 | --- 8 | float32 distance_traveled 9 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/ComputePathThroughPoses.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/PoseStamped[] goals 3 | geometry_msgs/PoseStamped start 4 | string planner_id 5 | bool use_start # If true, use current robot pose as path start, if false, use start above instead 6 | --- 7 | #result definition 8 | nav_msgs/Path path 9 | builtin_interfaces/Duration planning_time 10 | --- 11 | #feedback 12 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/ComputePathToPose.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/PoseStamped goal 3 | geometry_msgs/PoseStamped start 4 | string planner_id 5 | bool use_start # If true, use current robot pose as path start, if false, use start above instead 6 | --- 7 | #result definition 8 | nav_msgs/Path path 9 | builtin_interfaces/Duration planning_time 10 | --- 11 | #feedback 12 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/DummyRecovery.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | std_msgs/String command 3 | --- 4 | #result definition 5 | builtin_interfaces/Duration total_elapsed_time 6 | --- 7 | #feedback 8 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/FollowPath.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | nav_msgs/Path path 3 | geometry_msgs/PoseStamped goal 4 | string controller_id 5 | string goal_checker_id 6 | string progress_checker_id 7 | --- 8 | #result definition 9 | std_msgs/Empty result 10 | --- 11 | #feedback 12 | float32 distance_to_goal 13 | float32 speed 14 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/FollowWaypoints.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/PoseStamped[] poses 3 | --- 4 | #result definition 5 | int32[] missed_waypoints 6 | --- 7 | #feedback 8 | uint32 current_waypoint 9 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/NavigateThroughPoses.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/PoseStamped[] poses 3 | string behavior_tree 4 | --- 5 | #result definition 6 | std_msgs/Empty result 7 | --- 8 | geometry_msgs/PoseStamped current_pose 9 | builtin_interfaces/Duration navigation_time 10 | builtin_interfaces/Duration estimated_time_remaining 11 | int16 number_of_recoveries 12 | float32 distance_remaining 13 | int16 number_of_poses_remaining 14 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/NavigateToPose.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | geometry_msgs/PoseStamped pose 3 | string behavior_tree 4 | --- 5 | #result definition 6 | std_msgs/Empty result 7 | --- 8 | geometry_msgs/PoseStamped current_pose 9 | builtin_interfaces/Duration navigation_time 10 | builtin_interfaces/Duration estimated_time_remaining 11 | int16 number_of_recoveries 12 | float32 distance_remaining 13 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/Spin.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | float32 target_yaw 3 | --- 4 | #result definition 5 | builtin_interfaces/Duration total_elapsed_time 6 | --- 7 | float32 angular_distance_traveled 8 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/action/Wait.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | builtin_interfaces/Duration time 3 | --- 4 | #result definition 5 | builtin_interfaces/Duration total_elapsed_time 6 | --- 7 | #feedback 8 | builtin_interfaces/Duration time_left 9 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/BehaviorTreeLog.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time timestamp # ROS time that this log message was sent. 2 | BehaviorTreeStatusChange[] event_log 3 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/BehaviorTreeStatusChange.msg: -------------------------------------------------------------------------------- 1 | builtin_interfaces/Time timestamp # internal behavior tree event timestamp. Typically this is wall clock time 2 | string node_name 3 | string previous_status # IDLE, RUNNING, SUCCESS or FAILURE 4 | string current_status # IDLE, RUNNING, SUCCESS or FAILURE 5 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/Costmap.msg: -------------------------------------------------------------------------------- 1 | # This represents a 2-D grid map, in which each cell has an associated cost 2 | 3 | std_msgs/Header header 4 | 5 | # MetaData for the map 6 | CostmapMetaData metadata 7 | 8 | # The cost data, in row-major order, starting with (0,0). 9 | uint8[] data 10 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/CostmapFilterInfo.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # Type of plugin used (keepout filter, speed limit in m/s, speed limit in percent, etc...) 3 | # 0: keepout/lanes filter 4 | # 1: speed limit filter in % of maximum speed 5 | # 2: speed limit filter in absolute values (m/s) 6 | uint8 type 7 | # Name of filter mask topic 8 | string filter_mask_topic 9 | # Multiplier base offset and multiplier coefficient for conversion of OccGrid. 10 | # Used to convert OccupancyGrid data values to filter space values. 11 | # data -> into some other number space: 12 | # space = data * multiplier + base 13 | float32 base 14 | float32 multiplier 15 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/CostmapMetaData.msg: -------------------------------------------------------------------------------- 1 | # This hold basic information about the characteristics of the Costmap 2 | 3 | # The time at which the static map was loaded 4 | builtin_interfaces/Time map_load_time 5 | 6 | # The time of the last update to costmap 7 | builtin_interfaces/Time update_time 8 | 9 | # The corresponding layer name 10 | string layer 11 | 12 | # The map resolution [m/cell] 13 | float32 resolution 14 | 15 | # Number of cells in the horizontal direction 16 | uint32 size_x 17 | 18 | # Number of cells in the vertical direction 19 | uint32 size_y 20 | 21 | # The origin of the costmap [m, m, rad]. 22 | # This is the real-world pose of the cell (0,0) in the map. 23 | geometry_msgs/Pose origin 24 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/Particle.msg: -------------------------------------------------------------------------------- 1 | # This represents an individual particle with weight produced by a particle filter 2 | 3 | geometry_msgs/Pose pose 4 | 5 | float64 weight 6 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/ParticleCloud.msg: -------------------------------------------------------------------------------- 1 | # This represents a particle cloud containing particle poses and weights 2 | 3 | std_msgs/Header header 4 | 5 | # Array of particles in the cloud 6 | Particle[] particles 7 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/SpeedLimit.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | # Setting speed limit in percentage if true or in absolute values in false case 3 | bool percentage 4 | # Maximum allowed speed (in percent of maximum robot speed or in m/s depending 5 | # on "percentage" value). When no-limit it is set to 0.0 6 | float64 speed_limit 7 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/msg/VoxelGrid.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | uint32[] data 3 | geometry_msgs/Point32 origin 4 | geometry_msgs/Vector3 resolutions 5 | uint32 size_x 6 | uint32 size_y 7 | uint32 size_z 8 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_msgs 5 | 1.0.8 6 | Messages and service files for the Nav2 stack 7 | Michael Jeronimo 8 | Steve Macenski 9 | Carlos Orduno 10 | Apache-2.0 11 | 12 | ament_cmake 13 | nav2_common 14 | 15 | rclcpp 16 | std_msgs 17 | builtin_interfaces 18 | rosidl_default_generators 19 | geometry_msgs 20 | action_msgs 21 | nav_msgs 22 | 23 | rosidl_interface_packages 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/srv/ClearCostmapAroundRobot.srv: -------------------------------------------------------------------------------- 1 | # Clears the costmap within a distance 2 | 3 | float32 reset_distance 4 | --- 5 | std_msgs/Empty response 6 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/srv/ClearCostmapExceptRegion.srv: -------------------------------------------------------------------------------- 1 | # Clears the costmap except a rectangular region specified by reset_distance 2 | 3 | float32 reset_distance 4 | --- 5 | std_msgs/Empty response 6 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/srv/ClearEntireCostmap.srv: -------------------------------------------------------------------------------- 1 | # Clears all layers on the costmap 2 | 3 | std_msgs/Empty request 4 | --- 5 | std_msgs/Empty response 6 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/srv/GetCostmap.srv: -------------------------------------------------------------------------------- 1 | # Get the costmap 2 | 3 | # Specifications for the requested costmap 4 | nav2_msgs/CostmapMetaData specs 5 | --- 6 | nav2_msgs/Costmap map 7 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/srv/LoadMap.srv: -------------------------------------------------------------------------------- 1 | # URL of map resource 2 | # Can be an absolute path to a file: file:///path/to/maps/floor1.yaml 3 | # Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml 4 | string map_url 5 | --- 6 | # Result code defintions 7 | uint8 RESULT_SUCCESS=0 8 | uint8 RESULT_MAP_DOES_NOT_EXIST=1 9 | uint8 RESULT_INVALID_MAP_DATA=2 10 | uint8 RESULT_INVALID_MAP_METADATA=3 11 | uint8 RESULT_UNDEFINED_FAILURE=255 12 | 13 | # Returned map is only valid if result equals RESULT_SUCCESS 14 | nav_msgs/OccupancyGrid map 15 | uint8 result 16 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/srv/ManageLifecycleNodes.srv: -------------------------------------------------------------------------------- 1 | uint8 STARTUP = 0 2 | uint8 PAUSE = 1 3 | uint8 RESUME = 2 4 | uint8 RESET = 3 5 | uint8 SHUTDOWN = 4 6 | 7 | uint8 command 8 | --- 9 | bool success 10 | -------------------------------------------------------------------------------- /navigation2/nav2_msgs/srv/SaveMap.srv: -------------------------------------------------------------------------------- 1 | # URL of map resource 2 | # Can be an absolute path to a file: file:///path/to/maps/floor1.yaml 3 | # Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml 4 | string map_topic 5 | string map_url 6 | # Constants for image_format. Supported formats: pgm, png, bmp 7 | string image_format 8 | # Map modes: trinary, scale or raw 9 | string map_mode 10 | # Thresholds. Values in range of [0.0 .. 1.0] 11 | float32 free_thresh 12 | float32 occupied_thresh 13 | --- 14 | bool result 15 | -------------------------------------------------------------------------------- /navigation2/nav2_navfn_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_navfn_planner/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_navfn_planner/README.md: -------------------------------------------------------------------------------- 1 | # Navfn Planner 2 | 3 | The NavfnPlanner is a plugin for the Nav2 Planner server. 4 | 5 | It provides the equivalent functionality to a [GlobalPlanner](http://wiki.ros.org/nav_core#BaseGlobalPlanner) in ROS1 [MoveBase](http://wiki.ros.org/move_base). 6 | 7 | ## Status 8 | Currently, NavfnPlanner's core algorithm is a direct port from the ROS1 MoveBase [Navfn](http://wiki.ros.org/navfn) planner. The Navfn planning algorithm is based on the [Global Dynamic Window Approach](https://cs.stanford.edu/group/manips/publications/pdfs/Brock_1999_ICRA.pdf). 9 | 10 | ## Characteristics 11 | 12 | In Dijkstra mode (`use_astar = false`) Dijkstra's search algorithm is guaranteed to find the shortest path under any condition. 13 | In A* mode (`use_astar = true`) A*'s search algorithm is not guaranteed to find the shortest path, however it uses a heuristic to expand the potential field towards the goal. 14 | 15 | The Navfn planner assumes a circular robot and operates on a costmap. 16 | 17 | ## Next Steps 18 | - Implement additional planners based on optimal control, potential field or other graph search algorithms that require transformation of the world model to other representations (topological, tree map, etc.) to confirm sufficient generalization. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) 19 | - Implement planners for non-holonomic robots. [Issue #225](http://github.com/ros-planning/navigation2/issues/225) 20 | -------------------------------------------------------------------------------- /navigation2/nav2_navfn_planner/global_planner_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /navigation2/nav2_navfn_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_navfn_planner 5 | 1.0.8 6 | TODO 7 | Steve Macenski 8 | Carlos Orduno 9 | Apache-2.0 10 | BSD-3-Clause 11 | 12 | ament_cmake 13 | 14 | rclcpp 15 | rclcpp_action 16 | rclcpp_lifecycle 17 | visualization_msgs 18 | nav2_util 19 | nav2_msgs 20 | nav_msgs 21 | geometry_msgs 22 | builtin_interfaces 23 | nav2_common 24 | tf2_ros 25 | nav2_costmap_2d 26 | nav2_core 27 | pluginlib 28 | 29 | ament_lint_common 30 | ament_lint_auto 31 | 32 | 33 | ament_cmake 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /navigation2/nav2_planner/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_planner/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_planner/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Planner 2 | 3 | The Nav2 planner is a [planning module](../doc/requirements/requirements.md) that implements the `nav2_behavior_tree::ComputePathToPose` interface. 4 | 5 | A planning module implementing the `nav2_behavior_tree::ComputePathToPose` interface is responsible for generating a feasible path given start and end robot poses. It loads a map of potential planner plugins like NavFn to do the path generation in different user-defined situations. 6 | -------------------------------------------------------------------------------- /navigation2/nav2_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_planner 5 | 1.0.8 6 | TODO 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | rclcpp_lifecycle 15 | visualization_msgs 16 | nav2_util 17 | nav2_msgs 18 | mcr_msgs 19 | nav_msgs 20 | geometry_msgs 21 | builtin_interfaces 22 | nav2_common 23 | tf2_ros 24 | nav2_costmap_2d 25 | pluginlib 26 | nav2_core 27 | 28 | ament_lint_common 29 | ament_lint_auto 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | -------------------------------------------------------------------------------- /navigation2/nav2_planner/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // Copyright (c) 2019 Samsung Research America 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. 15 | 16 | #include 17 | 18 | #include "nav2_planner/planner_server.hpp" 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | rclcpp::init(argc, argv); 24 | auto node = std::make_shared(); 25 | rclcpp::spin(node->get_node_base_interface()); 26 | rclcpp::shutdown(); 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /navigation2/nav2_recoveries/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_recoveries/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_recoveries/recovery_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /navigation2/nav2_recoveries/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // Copyright (c) 2019 Samsung Research America 3 | // 4 | // Licensed under the Apache License, Version 2.0 (the "License"); 5 | // you may not use this file except in compliance with the License. 6 | // You may obtain a copy of the License at 7 | // 8 | // http://www.apache.org/licenses/LICENSE-2.0 9 | // 10 | // Unless required by applicable law or agreed to in writing, software 11 | // distributed under the License is distributed on an "AS IS" BASIS, 12 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | // See the License for the specific language governing permissions and 14 | // limitations under the License. Reserved. 15 | 16 | #include 17 | #include 18 | #include "cyberdog_debug/backtrace.hpp" 19 | #include "nav2_recoveries/recovery_server.hpp" 20 | #include "rclcpp/rclcpp.hpp" 21 | 22 | int main(int argc, char ** argv) 23 | { 24 | cyberdog::debug::register_signal(); 25 | rclcpp::init(argc, argv); 26 | auto recoveries_node = std::make_shared(); 27 | 28 | rclcpp::spin(recoveries_node->get_node_base_interface()); 29 | rclcpp::shutdown(); 30 | 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /navigation2/nav2_recoveries/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(test_recoveries 2 | test_recoveries.cpp 3 | ) 4 | 5 | ament_target_dependencies(test_recoveries 6 | ${dependencies} 7 | ) 8 | -------------------------------------------------------------------------------- /navigation2/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png -------------------------------------------------------------------------------- /navigation2/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | nav2_regulated_pure_pursuit_controller 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /navigation2/nav2_regulated_pure_pursuit_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_regulated_pure_pursuit_controller 5 | 1.0.8 6 | Regulated Pure Pursuit Controller 7 | Steve Macenski 8 | Shrijit Singh 9 | Apache-2.0 10 | 11 | ament_cmake 12 | 13 | nav2_common 14 | nav2_core 15 | nav2_util 16 | nav2_costmap_2d 17 | rclcpp 18 | geometry_msgs 19 | nav2_msgs 20 | pluginlib 21 | tf2 22 | 23 | ament_cmake_gtest 24 | ament_lint_common 25 | ament_lint_auto 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /navigation2/nav2_regulated_pure_pursuit_controller/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # tests for regulated PP 2 | ament_add_gtest(test_regulated_pp 3 | test_regulated_pp.cpp 4 | ) 5 | ament_target_dependencies(test_regulated_pp 6 | ${dependencies} 7 | ) 8 | target_link_libraries(test_regulated_pp 9 | ${library_name} 10 | ) 11 | -------------------------------------------------------------------------------- /navigation2/nav2_rotation_shim_controller/nav2_rotation_shim_controller.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | nav2_rotation_shim_controller 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /navigation2/nav2_rotation_shim_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_rotation_shim_controller 5 | 1.0.8 6 | Rotation Shim Controller 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | nav2_common 13 | nav2_core 14 | nav2_util 15 | nav2_costmap_2d 16 | rclcpp 17 | geometry_msgs 18 | nav2_msgs 19 | angles 20 | pluginlib 21 | tf2 22 | 23 | ament_cmake_gtest 24 | ament_lint_common 25 | ament_lint_auto 26 | nav2_regulated_pure_pursuit_controller 27 | nav2_controller 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /navigation2/nav2_rotation_shim_controller/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # tests 2 | find_package(nav2_controller REQUIRED) 3 | 4 | ament_add_gtest(test_shim_controller 5 | test_shim_controller.cpp 6 | ) 7 | ament_target_dependencies(test_shim_controller 8 | ${dependencies} 9 | nav2_controller 10 | ) 11 | target_link_libraries(test_shim_controller 12 | ${library_name} 13 | ) 14 | -------------------------------------------------------------------------------- /navigation2/nav2_smac_planner/media/A.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_smac_planner/media/A.png -------------------------------------------------------------------------------- /navigation2/nav2_smac_planner/media/B.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_smac_planner/media/B.png -------------------------------------------------------------------------------- /navigation2/nav2_smac_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_smac_planner 5 | 1.0.8 6 | Smac global planning plugin: A*, Hybrid-A*, State Lattice 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | rclcpp_action 14 | rclcpp_lifecycle 15 | visualization_msgs 16 | nav2_util 17 | nav2_msgs 18 | nav_msgs 19 | geometry_msgs 20 | builtin_interfaces 21 | nav2_common 22 | tf2_ros 23 | nav2_costmap_2d 24 | nav2_core 25 | pluginlib 26 | eigen3_cmake_module 27 | eigen 28 | ompl 29 | 30 | ament_lint_common 31 | ament_lint_auto 32 | ament_cmake_gtest 33 | ament_cmake_pytest 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /navigation2/nav2_smac_planner/smac_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Hybrid-A* SMAC planner 4 | 5 | 6 | -------------------------------------------------------------------------------- /navigation2/nav2_smac_planner/smac_plugin_2d.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 2D A* SMAC Planner 4 | 5 | 6 | -------------------------------------------------------------------------------- /navigation2/nav2_smac_planner/test/path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_smac_planner/test/path.png -------------------------------------------------------------------------------- /navigation2/nav2_smac_planner/test/test_nodebasic.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020, Samsung Research America 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. Reserved. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "gtest/gtest.h" 21 | #include "rclcpp/rclcpp.hpp" 22 | #include "nav2_costmap_2d/costmap_2d.hpp" 23 | #include "nav2_costmap_2d/costmap_subscriber.hpp" 24 | #include "nav2_util/lifecycle_node.hpp" 25 | #include "nav2_smac_planner/node_basic.hpp" 26 | #include "nav2_smac_planner/node_2d.hpp" 27 | #include "nav2_smac_planner/node_hybrid.hpp" 28 | #include "nav2_smac_planner/collision_checker.hpp" 29 | 30 | class RclCppFixture 31 | { 32 | public: 33 | RclCppFixture() {rclcpp::init(0, nullptr);} 34 | ~RclCppFixture() {rclcpp::shutdown();} 35 | }; 36 | RclCppFixture g_rclcppfixture; 37 | 38 | TEST(NodeBasicTest, test_node_basic) 39 | { 40 | nav2_smac_planner::NodeBasic node(50); 41 | 42 | EXPECT_EQ(node.index, 50u); 43 | EXPECT_EQ(node.graph_node_ptr, nullptr); 44 | 45 | nav2_smac_planner::NodeBasic node2(100); 46 | 47 | EXPECT_EQ(node2.index, 100u); 48 | EXPECT_EQ(node2.graph_node_ptr, nullptr); 49 | } 50 | -------------------------------------------------------------------------------- /navigation2/nav2_theta_star_planner/img/00-37.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_theta_star_planner/img/00-37.png -------------------------------------------------------------------------------- /navigation2/nav2_theta_star_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_theta_star_planner 5 | 1.0.8 6 | Theta* Global Planning Plugin 7 | Steve Macenski 8 | Anshumaan Singh 9 | Apache-2.0 10 | 11 | ament_cmake 12 | 13 | builtin_interfaces 14 | nav2_common 15 | nav2_core 16 | nav2_costmap_2d 17 | nav2_msgs 18 | nav2_util 19 | pluginlib 20 | rclcpp 21 | rclcpp_action 22 | rclcpp_lifecycle 23 | tf2_ros 24 | 25 | ament_lint_auto 26 | ament_lint_common 27 | ament_cmake_gtest 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /navigation2/nav2_theta_star_planner/theta_star_planner.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | An implementation of the Theta* Algorithm 4 | 5 | 6 | -------------------------------------------------------------------------------- /navigation2/nav2_util/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/nav2_util/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/nav2_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_util) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(nav2_msgs REQUIRED) 7 | find_package(tf2_ros REQUIRED) 8 | find_package(tf2 REQUIRED) 9 | find_package(tf2_geometry_msgs REQUIRED) 10 | find_package(geometry_msgs REQUIRED) 11 | find_package(nav_msgs REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(lifecycle_msgs REQUIRED) 14 | find_package(rclcpp_action REQUIRED) 15 | find_package(test_msgs REQUIRED) 16 | find_package(rclcpp_lifecycle REQUIRED) 17 | find_package(tf2_geometry_msgs REQUIRED) 18 | find_package(bondcpp REQUIRED) 19 | find_package(bond REQUIRED) 20 | find_package(action_msgs REQUIRED) 21 | 22 | set(dependencies 23 | nav2_msgs 24 | tf2_ros 25 | tf2 26 | tf2_geometry_msgs 27 | geometry_msgs 28 | nav_msgs 29 | rclcpp 30 | lifecycle_msgs 31 | rclcpp_action 32 | test_msgs 33 | rclcpp_lifecycle 34 | bondcpp 35 | bond 36 | action_msgs 37 | ) 38 | 39 | nav2_package() 40 | 41 | include_directories(include) 42 | 43 | set(library_name ${PROJECT_NAME}_core) 44 | add_subdirectory(src) 45 | 46 | install(DIRECTORY include/ 47 | DESTINATION include/ 48 | ) 49 | 50 | if(BUILD_TESTING) 51 | find_package(ament_lint_auto REQUIRED) 52 | find_package(ament_cmake_pytest REQUIRED) 53 | find_package(launch_testing_ament_cmake REQUIRED) 54 | ament_lint_auto_find_test_dependencies() 55 | 56 | find_package(ament_cmake_gtest REQUIRED) 57 | add_subdirectory(test) 58 | endif() 59 | 60 | ament_export_include_directories(include) 61 | ament_export_libraries(${library_name}) 62 | ament_export_dependencies(${dependencies}) 63 | 64 | ament_package() 65 | -------------------------------------------------------------------------------- /navigation2/nav2_util/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Util 2 | 3 | The `nav2_util` package contains utilities abstracted from individual packages which may find use in other uses. Examples include the particle filter implementation from AMCL, motion models, ROS2 node utilities, and more. 4 | 5 | ## ROS1 Comparison 6 | 7 | This package does not have a direct counter-part in Navigation. This was created to abstract out sections of the code-base from their implementations should the base algorithms/utilities find use elsewhere. 8 | -------------------------------------------------------------------------------- /navigation2/nav2_util/include/nav2_util/string_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef NAV2_UTIL__STRING_UTILS_HPP_ 16 | #define NAV2_UTIL__STRING_UTILS_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | namespace nav2_util 22 | { 23 | 24 | typedef std::vector Tokens; 25 | 26 | /* 27 | * @brief Remove leading slash from a topic name 28 | * @param in String of topic in 29 | * @return String out without slash 30 | */ 31 | std::string strip_leading_slash(const std::string & in); 32 | 33 | /// 34 | /* 35 | * @brief Split a string at the delimiters 36 | * @param in String to split 37 | * @param Delimiter criteria 38 | * @return Tokens 39 | */ 40 | Tokens split(const std::string & tokenstring, char delimiter); 41 | 42 | } // namespace nav2_util 43 | 44 | #endif // NAV2_UTIL__STRING_UTILS_HPP_ 45 | -------------------------------------------------------------------------------- /navigation2/nav2_util/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(${library_name} SHARED 2 | costmap.cpp 3 | node_utils.cpp 4 | lifecycle_service_client.cpp 5 | string_utils.cpp 6 | lifecycle_utils.cpp 7 | lifecycle_node.cpp 8 | robot_utils.cpp 9 | node_thread.cpp 10 | odometry_utils.cpp 11 | ) 12 | 13 | ament_target_dependencies(${library_name} 14 | rclcpp 15 | nav2_msgs 16 | tf2 17 | tf2_ros 18 | nav_msgs 19 | geometry_msgs 20 | lifecycle_msgs 21 | rclcpp_lifecycle 22 | tf2_geometry_msgs 23 | bondcpp 24 | ) 25 | 26 | add_executable(lifecycle_bringup 27 | lifecycle_bringup_commandline.cpp 28 | ) 29 | target_link_libraries(lifecycle_bringup ${library_name}) 30 | 31 | find_package(Boost REQUIRED COMPONENTS program_options) 32 | 33 | add_executable(dump_params dump_params.cpp) 34 | ament_target_dependencies(dump_params rclcpp) 35 | 36 | target_include_directories(dump_params PUBLIC ${Boost_INCLUDE_DIRS}) 37 | 38 | target_link_libraries(dump_params ${Boost_PROGRAM_OPTIONS_LIBRARY}) 39 | 40 | install(TARGETS 41 | ${library_name} 42 | ARCHIVE DESTINATION lib 43 | LIBRARY DESTINATION lib 44 | RUNTIME DESTINATION bin 45 | ) 46 | 47 | install(TARGETS 48 | lifecycle_bringup 49 | dump_params 50 | RUNTIME DESTINATION lib/${PROJECT_NAME} 51 | ) 52 | -------------------------------------------------------------------------------- /navigation2/nav2_util/src/lifecycle_bringup_commandline.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "rclcpp/rclcpp.hpp" 21 | #include "nav2_util/lifecycle_utils.hpp" 22 | 23 | using std::cerr; 24 | using namespace std::chrono_literals; 25 | 26 | void usage() 27 | { 28 | cerr << "Invalid command line.\n\n"; 29 | cerr << "This command will take a set of unconfigured lifecycle nodes through the\n"; 30 | cerr << "CONFIGURED to the ACTIVATED state\n"; 31 | cerr << "The nodes are brought up in the order listed on the command line\n\n"; 32 | cerr << "Usage:\n"; 33 | cerr << " > lifecycle_startup ...\n"; 34 | std::exit(1); 35 | } 36 | 37 | int main(int argc, char * argv[]) 38 | { 39 | if (argc == 1) { 40 | usage(); 41 | } 42 | rclcpp::init(0, nullptr); 43 | nav2_util::startup_lifecycle_nodes( 44 | std::vector(argv + 1, argv + argc), 45 | 10s); 46 | rclcpp::shutdown(); 47 | } 48 | -------------------------------------------------------------------------------- /navigation2/nav2_util/src/node_thread.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "nav2_util/node_thread.hpp" 18 | 19 | namespace nav2_util 20 | { 21 | 22 | NodeThread::NodeThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base) 23 | : node_(node_base) 24 | { 25 | executor_ = std::make_shared(); 26 | thread_ = std::make_unique( 27 | [&]() 28 | { 29 | executor_->add_node(node_); 30 | executor_->spin(); 31 | executor_->remove_node(node_); 32 | }); 33 | } 34 | 35 | NodeThread::NodeThread(rclcpp::executors::SingleThreadedExecutor::SharedPtr executor) 36 | : executor_(executor) 37 | { 38 | thread_ = std::make_unique([&]() {executor_->spin();}); 39 | } 40 | 41 | NodeThread::~NodeThread() 42 | { 43 | executor_->cancel(); 44 | thread_->join(); 45 | } 46 | 47 | } // namespace nav2_util 48 | -------------------------------------------------------------------------------- /navigation2/nav2_util/src/string_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "nav2_util/string_utils.hpp" 16 | #include 17 | 18 | using std::string; 19 | 20 | namespace nav2_util 21 | { 22 | 23 | std::string strip_leading_slash(const string & in) 24 | { 25 | string out = in; 26 | 27 | if ((!in.empty()) && (in[0] == '/')) { 28 | out.erase(0, 1); 29 | } 30 | 31 | return out; 32 | } 33 | 34 | Tokens split(const string & tokenstring, char delimiter) 35 | { 36 | Tokens tokens; 37 | 38 | size_t current_pos = 0; 39 | size_t pos = 0; 40 | while ((pos = tokenstring.find(delimiter, current_pos)) != string::npos) { 41 | tokens.push_back(tokenstring.substr(current_pos, pos - current_pos)); 42 | current_pos = pos + 1; 43 | } 44 | tokens.push_back(tokenstring.substr(current_pos)); 45 | return tokens; 46 | } 47 | 48 | } // namespace nav2_util 49 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_dump_params/dump_params_default.txt: -------------------------------------------------------------------------------- 1 | /dump_params: 2 | ros__parameters: 3 | use_sim_time: False 4 | 5 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_dump_params/dump_params_error.txt: -------------------------------------------------------------------------------- 1 | Error: ListParameters service for test_dump_params_error not available 2 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_dump_params/dump_params_help.txt: -------------------------------------------------------------------------------- 1 | Usage: dump_params 2 | Options: 3 | -h [ --help ] Print help message 4 | -n [ --node_names ] arg A list of comma-separated node names 5 | -f [ --format ] arg The format to dump ('yaml' or 'markdown') 6 | -v [ --verbose ] Verbose option 7 | 8 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_dump_params/dump_params_md.txt: -------------------------------------------------------------------------------- 1 | ## test_dump_params Parameters 2 | |Parameter|Default Value| 3 | |---|---| 4 | |use_sim_time|False| 5 | |param_bool|True| 6 | |param_int|1234| 7 | |param_double|3.14| 8 | |param_string|"foobar"| 9 | |param_bool_array|[True, False]| 10 | |param_int_array|[1, 2, 3]| 11 | |param_double_array|[1.5, 23.5012, 123.001]| 12 | |param_string_array|["foo", "bar"]| 13 | |param_byte_array|[0x1, 0x2, 0x3, 0x4]| 14 | 15 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_dump_params/dump_params_md_verbose.txt: -------------------------------------------------------------------------------- 1 | ## test_dump_params Parameters 2 | |Parameter|Default Value|Range|Description|Additional Constraints|Read-Only| 3 | |---|---|---|---|---|---| 4 | |use_sim_time|False|N/A|||False| 5 | |param_bool|True|N/A|boolean||True| 6 | |param_int|1234|-1000;10000;2||||False| 7 | |param_double|3.14|N/A|||False| 8 | |param_string|"foobar"|N/A|||False| 9 | |param_bool_array|[True, False]|N/A|||False| 10 | |param_int_array|[1, 2, 3]|N/A|||False| 11 | |param_double_array|[1.5, 23.5012, 123.001]|-1000.5;1000.5;0.0001||||False| 12 | |param_string_array|["foo", "bar"]|N/A|||False| 13 | |param_byte_array|[0x1, 0x2, 0x3, 0x4]|N/A|||False| 14 | 15 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_dump_params/dump_params_multiple.txt: -------------------------------------------------------------------------------- 1 | test_dump_params: 2 | ros__parameters: 3 | use_sim_time: False 4 | param_bool: True 5 | param_int: 1234 6 | param_double: 3.14 7 | param_string: "foobar" 8 | param_bool_array: [True, False] 9 | param_int_array: [1, 2, 3] 10 | param_double_array: [1.5, 23.5012, 123.001] 11 | param_string_array: ["foo", "bar"] 12 | param_byte_array: [0x1, 0x2, 0x3, 0x4] 13 | 14 | test_dump_params_copy: 15 | ros__parameters: 16 | use_sim_time: False 17 | param_bool: True 18 | param_int: 1234 19 | param_double: 3.14 20 | param_string: "foobar" 21 | param_bool_array: [True, False] 22 | param_int_array: [1, 2, 3] 23 | param_double_array: [1.5, 23.5012, 123.001] 24 | param_string_array: ["foo", "bar"] 25 | param_byte_array: [0x1, 0x2, 0x3, 0x4] 26 | 27 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_dump_params/dump_params_yaml.txt: -------------------------------------------------------------------------------- 1 | test_dump_params: 2 | ros__parameters: 3 | use_sim_time: False 4 | param_bool: True 5 | param_int: 1234 6 | param_double: 3.14 7 | param_string: "foobar" 8 | param_bool_array: [True, False] 9 | param_int_array: [1, 2, 3] 10 | param_double_array: [1.5, 23.5012, 123.001] 11 | param_string_array: ["foo", "bar"] 12 | param_byte_array: [0x1, 0x2, 0x3, 0x4] 13 | 14 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_execution_timer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2018 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "nav2_util/execution_timer.hpp" 19 | #include "gtest/gtest.h" 20 | 21 | using nav2_util::ExecutionTimer; 22 | using std::this_thread::sleep_for; 23 | using namespace std::chrono_literals; 24 | 25 | TEST(ExecutionTimer, BasicDelay) 26 | { 27 | ExecutionTimer t; 28 | t.start(); 29 | sleep_for(10ns); 30 | t.end(); 31 | ASSERT_GE(t.elapsed_time(), 10ns); 32 | ASSERT_GE(t.elapsed_time_in_seconds(), 1e-8); 33 | } 34 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_robot_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Samsung Research 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include "rclcpp/rclcpp.hpp" 17 | #include "nav2_util/robot_utils.hpp" 18 | #include "tf2_ros/transform_listener.h" 19 | #include "tf2_ros/transform_broadcaster.h" 20 | #include "geometry_msgs/msg/pose_stamped.hpp" 21 | #include "gtest/gtest.h" 22 | #include "nav2_util/node_thread.hpp" 23 | #include "tf2_ros/create_timer_ros.h" 24 | 25 | TEST(RobotUtils, LookupExceptionError) 26 | { 27 | rclcpp::init(0, nullptr); 28 | auto node = std::make_shared("name", rclcpp::NodeOptions()); 29 | geometry_msgs::msg::PoseStamped global_pose; 30 | tf2_ros::Buffer tf(node->get_clock()); 31 | ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1)); 32 | global_pose.header.frame_id = "base_link"; 33 | ASSERT_FALSE(nav2_util::transformPoseInTargetFrame(global_pose, global_pose, tf, "map", 0.1)); 34 | } 35 | -------------------------------------------------------------------------------- /navigation2/nav2_util/test/test_string_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Intel Corporation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "nav2_util/string_utils.hpp" 18 | #include "gtest/gtest.h" 19 | 20 | using nav2_util::split; 21 | using nav2_util::Tokens; 22 | 23 | TEST(Split, SplitFunction) 24 | { 25 | ASSERT_EQ(split("", ':'), Tokens({""})); 26 | ASSERT_EQ(split("foo", ':'), Tokens{"foo"}); 27 | ASSERT_EQ(split("foo:bar", ':'), Tokens({"foo", "bar"})); 28 | ASSERT_EQ(split("foo:bar:", ':'), Tokens({"foo", "bar", ""})); 29 | ASSERT_EQ(split(":", ':'), Tokens({"", ""})); 30 | ASSERT_EQ(split("foo::bar", ':'), Tokens({"foo", "", "bar"})); 31 | ASSERT_TRUE(nav2_util::strip_leading_slash(std::string("/hi")) == std::string("hi")); 32 | } 33 | -------------------------------------------------------------------------------- /navigation2/nav2_voxel_grid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(nav2_voxel_grid) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | find_package(nav2_common REQUIRED) 6 | find_package(rclcpp REQUIRED) 7 | 8 | nav2_package() 9 | 10 | include_directories( 11 | include) 12 | 13 | add_library(voxel_grid SHARED 14 | src/voxel_grid.cpp 15 | ) 16 | 17 | set(dependencies 18 | rclcpp 19 | ) 20 | 21 | ament_target_dependencies(voxel_grid 22 | ${dependencies} 23 | ) 24 | 25 | install(TARGETS voxel_grid 26 | ARCHIVE DESTINATION lib 27 | LIBRARY DESTINATION lib 28 | RUNTIME DESTINATION bin 29 | ) 30 | 31 | install(DIRECTORY include/ 32 | DESTINATION include 33 | ) 34 | 35 | if(BUILD_TESTING) 36 | find_package(ament_lint_auto REQUIRED) 37 | # the following line skips the linter which checks for copyrights 38 | set(ament_cmake_copyright_FOUND TRUE) 39 | ament_lint_auto_find_test_dependencies() 40 | 41 | find_package(ament_cmake_gtest REQUIRED) 42 | add_subdirectory(test) 43 | endif() 44 | 45 | ament_export_dependencies(rclcpp) 46 | ament_export_include_directories(include) 47 | ament_export_libraries(voxel_grid) 48 | 49 | ament_package() 50 | -------------------------------------------------------------------------------- /navigation2/nav2_voxel_grid/README.md: -------------------------------------------------------------------------------- 1 | # Nav2 Voxel Grid 2 | 3 | The `nav2_voxel_grid` package contains the VoxelGrid used by the `Voxel Layer` inside of `nav2_costmap_2d`. The voxel grid itself is simply a 2D char pointer array of the map size with bit locations corresponding to voxel values (free, unknown, occupied , etc). 4 | 5 | It is branched out as a separate package for use in other applications where a dense voxel grid representation may be useful. It also contains implementations of 3D raycasting. 6 | 7 | ## ROS1 Comparison 8 | 9 | This package is a direct port to ROS2 for use in the voxel layer. 10 | -------------------------------------------------------------------------------- /navigation2/nav2_voxel_grid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_voxel_grid 5 | 1.0.8 6 | 7 | voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. 8 | 9 | Carl Delsey 10 | BSD-3-Clause 11 | 12 | ament_cmake 13 | nav2_common 14 | 15 | rclcpp 16 | 17 | ament_lint_common 18 | ament_lint_auto 19 | ament_cmake_gtest 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /navigation2/nav2_voxel_grid/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ament_add_gtest(voxel_grid_tests voxel_grid_tests.cpp) 2 | target_link_libraries(voxel_grid_tests voxel_grid) 3 | 4 | ament_add_gtest(voxel_grid_bresenham_3d voxel_grid_bresenham_3d.cpp) 5 | target_link_libraries(voxel_grid_bresenham_3d voxel_grid) 6 | -------------------------------------------------------------------------------- /navigation2/nav2_waypoint_follower/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | nav2_waypoint_follower 5 | 1.0.8 6 | A waypoint follower navigation server 7 | Steve Macenski 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | nav2_common 13 | cv_bridge 14 | pluginlib 15 | image_transport 16 | rclcpp 17 | rclcpp_action 18 | rclcpp_lifecycle 19 | nav_msgs 20 | nav2_msgs 21 | nav2_util 22 | nav2_core 23 | tf2_ros 24 | 25 | ament_lint_common 26 | ament_lint_auto 27 | ament_cmake_gtest 28 | ament_cmake_pytest 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /navigation2/nav2_waypoint_follower/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Lets robot sleep for a specified amount of time at waypoint arrival 5 | 6 | 7 | 8 | 9 | Run-time plugin that takes photos at waypoint arrivals when using waypoint follower node. 10 | Saves the taken photos to specified directory. 11 | 12 | 13 | 14 | 15 | Lets robot wait for input at waypoint arrival 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /navigation2/nav2_waypoint_follower/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2019 Samsung Research America 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "nav2_waypoint_follower/waypoint_follower.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | int main(int argc, char ** argv) 21 | { 22 | rclcpp::init(argc, argv); 23 | auto node = std::make_shared(); 24 | rclcpp::spin(node->get_node_base_interface()); 25 | rclcpp::shutdown(); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /navigation2/nav2_waypoint_follower/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Test costmap downsampler 2 | ament_add_gtest(test_task_executors 3 | test_task_executors.cpp 4 | ) 5 | ament_target_dependencies(test_task_executors 6 | ${dependencies} 7 | ) 8 | target_link_libraries(test_task_executors 9 | ${library_name} wait_at_waypoint photo_at_waypoint input_at_waypoint 10 | ) 11 | -------------------------------------------------------------------------------- /navigation2/navigation2/CHANGELOG.rst: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MiRoboticsLab/cyberdog_nav2/77c65782bd2e09e93a03fc5d36c4b68f62395c38/navigation2/navigation2/CHANGELOG.rst -------------------------------------------------------------------------------- /navigation2/navigation2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(navigation2) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 17) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | 10 | ament_package() 11 | -------------------------------------------------------------------------------- /navigation2/navigation2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | navigation2 5 | 1.0.8 6 | 7 | ROS2 Navigation Stack 8 | 9 | Steve Macenski 10 | Carl Delsey 11 | Apache-2.0 12 | 13 | ament_cmake 14 | 15 | 16 | nav2_amcl 17 | nav2_bt_navigator 18 | nav2_costmap_2d 19 | nav2_core 20 | nav2_dwb_controller 21 | nav2_lifecycle_manager 22 | nav2_map_server 23 | nav2_recoveries 24 | nav2_planner 25 | nav2_msgs 26 | nav2_navfn_planner 27 | nav2_behavior_tree 28 | nav2_util 29 | nav2_voxel_grid 30 | nav2_controller 31 | nav2_waypoint_follower 32 | nav2_smac_planner 33 | nav2_regulated_pure_pursuit_controller 34 | nav2_rotation_shim_controller 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /navigation2/tools/ctest_retry.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | usage() { 4 | echo "ctest_retry.bash [options]" 5 | echo "Reruns ctest until the test passes or it has run 3 times (by default)." 6 | echo " -r Retry the test up to times [Default: 3]" 7 | echo " -d Execute ctest from the given path [Default: Current working directory]" 8 | echo " -t Execute only [Default: execute all tests available in ]" 9 | echo " -h displays this usage summary" 10 | exit 0 11 | } 12 | 13 | RETRIES=3 14 | TESTDIR=. 15 | SPECIFIC_TEST="" 16 | 17 | # Check Options 18 | while getopts ":hr:d:t:" opt; do 19 | case "$opt" in 20 | h) 21 | usage 22 | ;; 23 | r) 24 | RETRIES=$OPTARG 25 | ;; 26 | d) 27 | TESTDIR=$OPTARG 28 | ;; 29 | t) 30 | SPECIFIC_TEST="-R $OPTARG" 31 | ;; 32 | \?) 33 | echo "Invalid option: -$OPTARG" >&2 34 | usage 35 | exit 1 36 | ;; 37 | :) 38 | echo "Option -$OPTARG requires an argument." >&2 39 | exit 1 40 | ;; 41 | esac 42 | done 43 | 44 | total=$RETRIES 45 | cd $TESTDIR 46 | export RCUTILS_LOGGING_BUFFERED_STREAM=1 47 | export RCUTILS_LOGGING_USE_STDOUT=1 48 | 49 | echo "Retrying Ctest up to " $total " times." 50 | for ((i=1;i<=total;i++)) 51 | do 52 | ctest -V $SPECIFIC_TEST 53 | result=$? 54 | if [ "$result" == "0" ] # if ctest succeeded, then exit the retry loop 55 | then 56 | echo "Test succeeded on try " $i 57 | exit 0 58 | fi 59 | done 60 | echo "Test failed " $total " times." 61 | exit $result 62 | -------------------------------------------------------------------------------- /navigation2/tools/run_sanitizers: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # colcon sanitizer plugin instructions at 4 | # https://github.com/colcon/colcon-sanitizer-reports/blob/master/README.rst 5 | 6 | # To use this script, make sure you have the colcon plugins installed according 7 | # to the instructions above. Then build ros2_ws and navstack_dependencies_ws. 8 | # Source the navstack_dependencies_ws and then cd to the nav2_ws. 9 | # 10 | # Run this script by invoking navigation2/tools/run_sanitizers 11 | # Afterwards, there should be two files in the root of the workspace: 12 | # - sanitizer_report-asan.csv 13 | # - sanitizer_report-tsan.csv 14 | 15 | clean_workspace() { 16 | rm -rf build install log 17 | } 18 | 19 | build_with_sanitizer() { 20 | colcon build --build-base=build-$1 --install-base=install-$1 \ 21 | --cmake-args -DOSRF_TESTING_TOOLS_CPP_DISABLE_MEMORY_TOOLS=ON \ 22 | -DCMAKE_BUILD_TYPE=Debug \ 23 | --mixin $1 \ 24 | --symlink-install 25 | } 26 | 27 | test_with_sanitizer() { 28 | colcon test --build-base=build-$1 --install-base=install-$1 --retest-until-pass 3 \ 29 | --event-handlers sanitizer_report+ 30 | } 31 | 32 | 33 | clean_workspace 34 | 35 | build_with_sanitizer asan-gcc 36 | build_with_sanitizer tsan 37 | 38 | test_with_sanitizer asan-gcc 39 | mv sanitizer_report.csv sanitizer_report-asan.csv 40 | test_with_sanitizer tsan 41 | mv sanitizer_report.csv sanitizer_report-tsan.csv 42 | -------------------------------------------------------------------------------- /navigation2/tools/run_test_suite.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -ex 4 | 5 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # gets the directory of this script 6 | 7 | # Skip flaky tests. Nav2 system tests will be run later. 8 | colcon test --packages-skip nav2_system_tests nav2_recoveries 9 | 10 | # run the stable tests in nav2_recoveries 11 | colcon test --packages-select nav2_recoveries --ctest-args --exclude-regex "test_recoveries" 12 | 13 | # run the linters in nav2_system_tests. They only need to be run once. 14 | colcon test --packages-select nav2_system_tests --ctest-args --exclude-regex "test_.*" # run the linters 15 | 16 | # Each of the `colcon test` lines above runs tests on independent sets of packages. 17 | # As a result the test logs of each line won't overwrite the others. The single 18 | # call to `colcon test-result` will look through all packages and report any errors 19 | # that happened in any of the `colcon test` lines above. 20 | colcon test-result --verbose 21 | 22 | # $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_localization$ 23 | # $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_costmaps$ 24 | # $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_planner_random$ 25 | # $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator$ 26 | # $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_bt_navigator_with_dijkstra$ 27 | $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_dynamic_obstacle$ 28 | # $SCRIPT_DIR/ctest_retry.bash -r 3 -d build/nav2_system_tests -t test_multi_robot$ 29 | -------------------------------------------------------------------------------- /navigation2/tools/skip_keys.txt: -------------------------------------------------------------------------------- 1 | console_bridge 2 | fastcdr 3 | fastrtps 4 | libopensplice69 5 | rti-connext-dds-5.3.1 6 | slam_toolbox 7 | urdfdom_headers -------------------------------------------------------------------------------- /navigation2/tools/underlay.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | # BehaviorTree/BehaviorTree.CPP: 3 | # type: git 4 | # url: https://github.com/BehaviorTree/BehaviorTree.CPP.git 5 | # version: master 6 | # ros/angles: 7 | # type: git 8 | # url: https://github.com/ros/angles.git 9 | # version: ros2 10 | # ros-simulation/gazebo_ros_pkgs: 11 | # type: git 12 | # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git 13 | # version: ros2 14 | # ros-perception/vision_opencv: 15 | # type: git 16 | # url: https://github.com/ros-perception/vision_opencv.git 17 | # version: ros2 18 | # ros/bond_core: 19 | # type: git 20 | # url: https://github.com/ros/bond_core.git 21 | # version: ros2 22 | ompl/ompl: 23 | type: git 24 | url: https://github.com/ompl/ompl.git 25 | version: 1.5.0 26 | -------------------------------------------------------------------------------- /navigation2/tools/update_bt_diagrams.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Run this from the root of the workspace to update these behavior_tree images 4 | # in the doc directory of the nav2_bt_navigator package 5 | navigation2/tools/bt2img.py \ 6 | --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml \ 7 | --image_out navigation2/nav2_bt_navigator/doc/simple_parallel \ 8 | --legend navigation2/nav2_bt_navigator/doc/legend 9 | navigation2/tools/bt2img.py \ 10 | --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml \ 11 | --image_out navigation2/nav2_bt_navigator/doc/parallel_w_recovery 12 | navigation2/tools/bt2img.py \ 13 | --behavior_tree navigation2/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml \ 14 | --image_out navigation2/nav2_bt_navigator/doc/parallel_through_poses_w_recovery 15 | -------------------------------------------------------------------------------- /navigation_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(navigation_bringup) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | 19 | # find dependencies 20 | find_package(ament_cmake REQUIRED) 21 | find_package(rosidl_default_generators REQUIRED) 22 | find_package(rosidl_typesupport_cpp REQUIRED) 23 | find_package(rclcpp REQUIRED) 24 | find_package(rclcpp_components REQUIRED) 25 | find_package(tf2 REQUIRED) 26 | find_package(tf2_eigen REQUIRED) 27 | find_package(tf2_geometry_msgs REQUIRED) 28 | find_package(tf2_ros REQUIRED) 29 | find_package(sensor_msgs REQUIRED) 30 | find_package(nav_msgs REQUIRED) 31 | find_package(cyberdog_common REQUIRED) 32 | 33 | add_executable(test_node 34 | test/test_node.cpp 35 | ) 36 | ament_target_dependencies( 37 | test_node 38 | geometry_msgs 39 | rclcpp 40 | tf2 41 | tf2_ros 42 | cyberdog_common 43 | ) 44 | 45 | install(TARGETS 46 | test_node 47 | DESTINATION lib/${PROJECT_NAME}) 48 | 49 | # Mark other files for installation 50 | install( 51 | DIRECTORY 52 | launch 53 | rviz 54 | params 55 | config 56 | urdf 57 | DESTINATION share/${PROJECT_NAME} 58 | ) 59 | 60 | ament_package() 61 | -------------------------------------------------------------------------------- /navigation_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | navigation_bringup 5 | 0.0.0 6 | The navigation bringup package 7 | Apache-2.0 8 | 9 | Du ZHonghua 10 | ament_cmake 11 | rclcpp 12 | tf2 13 | tf2_eigen 14 | tf2_ros 15 | tf2_geometry_msgs 16 | nav_msgs 17 | cyberdog_common 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /navigation_bringup/params/map_saver_params.yaml: -------------------------------------------------------------------------------- 1 | map_saver: 2 | ros__parameters: 3 | use_sim_time: False 4 | save_map_timeout: 5000 # The glorious timeout parameter 5 | free_thresh_default: 0.25 6 | occupied_thresh_default: 0.65 7 | -------------------------------------------------------------------------------- /navigation_bringup/urdf/static_tf_map.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /positionchecker/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | positionchecker 5 | 0.0.0 6 | get cyberdog's pose 7 | duzhonghua 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | nav2_util 14 | tf2_ros 15 | geometry_msgs 16 | std_srvs 17 | cyberdog_common 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /positionchecker/src/positionchecker.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include "positionchecker/position_checker_node.hpp" 19 | #include "cyberdog_common/cyberdog_log.hpp" 20 | 21 | int main(int argc, char ** argv) 22 | { 23 | LOGGER_MAIN_INSTANCE("PoseServer"); 24 | rclcpp::init(argc, argv); 25 | auto node = std::make_shared(); 26 | rclcpp::executors::MultiThreadedExecutor exec_; 27 | exec_.add_node(node->get_node_base_interface()); 28 | exec_.spin(); 29 | rclcpp::shutdown(); 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /velocity_adaptor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(velocity_adaptor) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(nav2_util REQUIRED) 12 | find_package(std_msgs REQUIRED) 13 | find_package(std_srvs REQUIRED) 14 | find_package(cyberdog_common REQUIRED) 15 | find_package(protocol REQUIRED) 16 | 17 | set(dependencies 18 | rclcpp 19 | nav2_util 20 | std_msgs 21 | std_srvs 22 | protocol 23 | cyberdog_common 24 | ) 25 | 26 | add_executable(${PROJECT_NAME} 27 | src/main.cpp 28 | src/velocity_adaptor.cpp 29 | ) 30 | 31 | target_include_directories(velocity_adaptor PUBLIC 32 | $ 33 | $) 34 | 35 | ament_target_dependencies( 36 | ${PROJECT_NAME} 37 | ${dependencies} 38 | ) 39 | 40 | install(TARGETS ${PROJECT_NAME} 41 | RUNTIME DESTINATION lib/${PROJECT_NAME} 42 | ) 43 | 44 | if(BUILD_TESTING) 45 | find_package(ament_lint_auto REQUIRED) 46 | 47 | # the following line skips the linter which checks for copyrights 48 | # uncomment the line when a copyright and license is not present in all source files 49 | # set(ament_cmake_copyright_FOUND TRUE) 50 | # the following line skips cpplint (only works in a git repo) 51 | # uncomment the line when this package is not in a git repo 52 | # set(ament_cmake_cpplint_FOUND TRUE) 53 | ament_lint_auto_find_test_dependencies() 54 | endif() 55 | 56 | ament_package() 57 | -------------------------------------------------------------------------------- /velocity_adaptor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | velocity_adaptor 5 | 0.0.0 6 | a package to control lidar by topic 7 | duzhonghua 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | nav2_util 14 | std_msgs 15 | std_srvs 16 | protocol 17 | cyberdog_common 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /velocity_adaptor/src/main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 Beijing Xiaomi Mobile Software Co., Ltd. All rights reserved. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "velocity_adaptor/velocity_adaptor.hpp" 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | rclcpp::init(argc, argv); 22 | rclcpp::spin(std::make_shared()); 23 | rclcpp::shutdown(); 24 | return 0; 25 | } 26 | --------------------------------------------------------------------------------