diff --git a/.circleci/config.yml b/.circleci/config.yml index 0a39c3a9c7..f7b42afeb5 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -70,6 +70,7 @@ references: (echo vcs_export && cat) >> checksum.txt sha256sum $PWD/checksum.txt >> checksum.txt apt-get update + rosdep update dependencies=$( rosdep install -q -y \ --from-paths src \ @@ -276,37 +277,6 @@ references: file: lcov/project_coverage.info flags: project when: always - setup_doc_dependencies: &setup_doc_dependencies - run: - name: Install Doc Dependencies - command: | - apk --no-cache add python3 - python3 -m ensurepip - pip3 install sphinx==1.7.5 docutils==0.14 sphinx_rtd_theme breathe==4.9.1 sphinxcontrib-plantuml - apk --no-cache add make - apk --no-cache add doxygen - apk --no-cache add graphviz - apk --no-cache add ttf-dejavu - apk --no-cache add openjdk8-jre - apk --no-cache add bash git openssh - make_docs: &make_docs - run: - command: | - cd src/navigation2/sphinx_doc - make html - store_docs: &store_docs - store_artifacts: - path: /opt/overlay_ws/src/navigation2/sphinx_doc/_build/html - destination: html - publish_docs: &publish_docs - run: - command: | - cd src/navigation2/sphinx_doc - make publish - install_deployment_key: &install_deployment_key - add_ssh_keys: - fingerprints: - - "0e:8e:90:bf:3d:a9:04:d9:04:b4:62:38:a5:3b:90:7d" commands: <<: *common_commands @@ -349,19 +319,6 @@ commands: steps: - *collect_overlay_coverage - *upload_overlay_coverage - install_doc_dependencies: - description: "Install documentation dependencies" - steps: - - *setup_doc_dependencies - build_docs: - description: "Build docs" - steps: - - *make_docs - - *store_docs - publish_docs_to_gh_pages_branch: - description: "Commit newly built docs to gh-pages branch" - steps: - - *publish_docs executors: debug_exec: @@ -382,12 +339,6 @@ executors: CACHE_NONCE: "Release" OVERLAY_MIXINS: "release ccache" UNDERLAY_MIXINS: "release ccache" - docs_exec: - docker: - - image: alpine:latest - working_directory: /opt/overlay_ws - environment: - <<: *common_environment jobs: debug_build: &debug_build @@ -424,25 +375,6 @@ jobs: <<: *release_test environment: RMW_IMPLEMENTATION: "rmw_fastrtps_cpp" - docs_build: - executor: docs_exec - steps: - - install_doc_dependencies - - *on_checkout - - build_docs - - persist_to_workspace: - root: src/navigation2 - paths: - - sphinx_doc - docs_publish: - executor: docs_exec - steps: - - install_doc_dependencies - - *on_checkout - - *install_deployment_key - - attach_workspace: - at: src/navigation2 - - publish_docs_to_gh_pages_branch workflows: version: 2 @@ -456,15 +388,6 @@ workflows: - release_test: requires: - release_build - build_and_publish_docs: - jobs: - - docs_build - - docs_publish: - requires: - - docs_build - filters: - branches: - only: master nightly: jobs: - debug_build @@ -472,9 +395,9 @@ workflows: requires: - debug_build - release_build - - test_rmw_connext_cpp: - requires: - - release_build + # - test_rmw_connext_cpp: + # requires: + # - release_build - test_rmw_cyclonedds_cpp: requires: - release_build diff --git a/.dockerhub/release/hooks/build b/.dockerhub/release/hooks/build index af68f3c648..5f9dd7e757 100755 --- a/.dockerhub/release/hooks/build +++ b/.dockerhub/release/hooks/build @@ -1,7 +1,7 @@ #!/bin/bash set -ex -export FROM_IMAGE=osrf/ros2:nightly-rmw-nonfree +export FROM_IMAGE=osrf/ros2:nightly export FAIL_ON_BUILD_FAILURE="" export UNDERLAY_MIXINS="release ccache" export OVERLAY_MIXINS="release ccache" diff --git a/Dockerfile b/Dockerfile index 9ac07102ca..abcb8ffaea 100644 --- a/Dockerfile +++ b/Dockerfile @@ -7,96 +7,92 @@ # --build-arg UNDERLAY_MIXINS \ # --build-arg OVERLAY_MIXINS ./ ARG FROM_IMAGE=osrf/ros2:nightly +ARG UNDERLAY_WS=/opt/underlay_ws +ARG OVERLAY_WS=/opt/overlay_ws # multi-stage for caching -FROM $FROM_IMAGE AS cache +FROM $FROM_IMAGE AS cacher # clone underlay source -ENV UNDERLAY_WS /opt/underlay_ws -RUN mkdir -p $UNDERLAY_WS/src -WORKDIR $UNDERLAY_WS -COPY ./tools/ros2_dependencies.repos ./ -RUN vcs import src < ros2_dependencies.repos +ARG UNDERLAY_WS +WORKDIR $UNDERLAY_WS/src +COPY ./tools/ros2_dependencies.repos ../ +RUN vcs import ./ < ../ros2_dependencies.repos && \ + find ./ -name ".git" | xargs rm -rf # copy overlay source -ENV OVERLAY_WS /opt/overlay_ws -RUN mkdir -p $OVERLAY_WS/src -WORKDIR $OVERLAY_WS -COPY ./ src/navigation2 +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS/src +COPY ./ ./navigation2 # copy manifests for caching WORKDIR /opt -RUN find ./ -name "package.xml" | \ - xargs cp --parents -t /tmp && \ +RUN mkdir -p /tmp/opt && \ + find ./ -name "package.xml" | \ + xargs cp --parents -t /tmp/opt && \ find ./ -name "COLCON_IGNORE" | \ - xargs cp --parents -t /tmp + xargs cp --parents -t /tmp/opt || true # multi-stage for building -FROM $FROM_IMAGE AS build +FROM $FROM_IMAGE AS builder # install CI dependencies RUN apt-get update && apt-get install -q -y \ ccache \ lcov \ + && rosdep update \ && rm -rf /var/lib/apt/lists/* -# copy underlay manifests -ENV UNDERLAY_WS /opt/underlay_ws -COPY --from=cache /tmp/underlay_ws $UNDERLAY_WS -WORKDIR $UNDERLAY_WS - # install underlay dependencies +ARG UNDERLAY_WS +WORKDIR $UNDERLAY_WS +COPY --from=cacher /tmp/$UNDERLAY_WS ./ RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ apt-get update && rosdep install -q -y \ --from-paths src \ --ignore-src \ && rm -rf /var/lib/apt/lists/* -# copy underlay source -COPY --from=cache $UNDERLAY_WS ./ - # build underlay source +COPY --from=cacher $UNDERLAY_WS ./ ARG UNDERLAY_MIXINS="release ccache" ARG FAIL_ON_BUILD_FAILURE=True RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ colcon build \ --symlink-install \ - --mixin \ - $UNDERLAY_MIXINS \ + --mixin $UNDERLAY_MIXINS \ --event-handlers console_direct+ \ || touch build_failed && \ if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ exit 1; \ fi -# copy overlay manifests -ENV OVERLAY_WS /opt/overlay_ws -COPY --from=cache /tmp/overlay_ws $OVERLAY_WS -WORKDIR $OVERLAY_WS - # install overlay dependencies +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS +COPY --from=cacher /tmp/$OVERLAY_WS ./ RUN . $UNDERLAY_WS/install/setup.sh && \ apt-get update && rosdep install -q -y \ --from-paths src \ + $UNDERLAY_WS/src \ --ignore-src \ && rm -rf /var/lib/apt/lists/* -# copy overlay source -COPY --from=cache $OVERLAY_WS ./ - # build overlay source +COPY --from=cacher $OVERLAY_WS ./ ARG OVERLAY_MIXINS="release ccache" RUN . $UNDERLAY_WS/install/setup.sh && \ colcon build \ --symlink-install \ - --mixin \ - $OVERLAY_MIXINS \ + --mixin $OVERLAY_MIXINS \ || touch build_failed && \ if [ -f build_failed ] && [ -n "$FAIL_ON_BUILD_FAILURE" ]; then \ exit 1; \ fi # source overlay from entrypoint +ENV UNDERLAY_WS $UNDERLAY_WS +ENV OVERLAY_WS $OVERLAY_WS RUN sed --in-place \ 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \ /ros_entrypoint.sh diff --git a/README.md b/README.md index f9c367fdbd..cedfec718e 100644 --- a/README.md +++ b/README.md @@ -2,15 +2,15 @@ [![Build Status](https://img.shields.io/docker/pulls/rosplanning/navigation2.svg?maxAge=2592000)](https://hub.docker.com/r/rosplanning/navigation2) [![Build Status](https://img.shields.io/docker/cloud/build/rosplanning/navigation2.svg?label=docker%20build)](https://hub.docker.com/r/rosplanning/navigation2) [![codecov](https://codecov.io/gh/ros-planning/navigation2/branch/master/graph/badge.svg)](https://codecov.io/gh/ros-planning/navigation2) For detailed instructions on how to: -- [Getting Started](https://ros-planning.github.io/navigation2/getting_started/index.html) -- [Build](https://ros-planning.github.io/navigation2/build_instructions/index.html#build) -- [Install](https://ros-planning.github.io/navigation2/build_instructions/index.html#install) -- [Tutorials](https://ros-planning.github.io/navigation2/tutorials/index.html) -- [Configure](https://ros-planning.github.io/navigation2/configuration/index.html) -- [Navigation Plugins](https://ros-planning.github.io/navigation2/plugins/index.html) -- [Contribute](https://ros-planning.github.io/navigation2/contribute/index.html) +- [Getting Started](https://navigation.ros.org/getting_started/index.html) +- [Build](https://navigation.ros.org/build_instructions/index.html#build) +- [Install](https://navigation.ros.org/build_instructions/index.html#install) +- [Tutorials](https://navigation.ros.org/tutorials/index.html) +- [Configure](https://navigation.ros.org/configuration/index.html) +- [Navigation Plugins](https://navigation.ros.org/plugins/index.html) +- [Contribute](https://navigation.ros.org/contribute/index.html) -Please visit our [documentation site](https://ros-planning.github.io/navigation2/). +Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://navigation2.slack.com). | Service | Dashing | Eloquent | Master | | :---: | :---: | :---: | :---: | diff --git a/codecov.yml b/codecov.yml index c822efe456..202b5fdbbc 100644 --- a/codecov.yml +++ b/codecov.yml @@ -1,2 +1,8 @@ fixes: - "src/navigation2/::" + +ignore: + - "*/**/test/*" # ignore package test directories, e.g. nav2_dwb_controller/costmap_queue/tests + - "*/test/**/*" # ignore package test directories, e.g. nav2_costmap_2d/tests + - "**/test_*.*" # ignore files starting with test_ e.g. nav2_map_server/test/test_constants.cpp + - "**/*_tests.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md new file mode 100644 index 0000000000..f8cd1df408 --- /dev/null +++ b/doc/parameters/param_list.md @@ -0,0 +1,584 @@ +*NOTE: <> means plugin are namespaced by a name/id parameterization. The bracketed names may change due to your application configuration* + +# bt_navigator + +| Parameter | Default | Description | +| ----------| --------| ------------| +| default_bt_xml_filename | N/A | path to the default behavior tree XML description | +| plugin_lib_names | ["nav2_compute_path_to_pose_action_bt_node", "nav2_follow_path_action_bt_node", "nav2_back_up_action_bt_node", "nav2_spin_action_bt_node", "nav2_wait_action_bt_node", "nav2_clear_costmap_service_bt_node", "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", "nav2_transform_available_condition_bt_node"] | list of behavior tree node shared libraries | +| transform_tolerance | 0.1 | TF transform tolerance | +| global_frame | "map" | Reference frame | +| robot_base_frame | "base_link" | Robot base frame | + +# costmaps + +## Node: costmap_2d_ros + +Namespace: /parent_ns/local_ns + +| Parameter | Default | Description | +| ----------| --------| ------------| +| always_send_full_costmap | false | Whether to send full costmap every update, rather than updates | +| footprint_padding | 0.01 | Amount to pad footprint (m) | +| footprint | "[]" | Ordered set of footprint points, must be closed set | +| global_frame | "map" | Reference frame | +| height | 5 | Height of costmap (m) | +| width | 5 | Width of costmap (m) | +| lethal_cost_threshold | 100 | Minimum cost of an occupancy grid map to be considered a lethal obstacle | +| map_topic | "parent_namespace/map" | Topic of map from map_server or SLAM | +| observation_sources | [""] | List of sources of sensors, to be used if not specified in plugin specific configurations | +| origin_x | 0.0 | X origin of the costmap relative to width (m) | +| origin_y | 0.0 | Y origin of the costmap relative to height (m) | +| plugin_names | {"static_layer", "obstacle_layer", "inflation_layer"} | List of mapped plugin names for parameter namespaces and names | +| plugin_types | {"nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} | List of registered plugins to map to names and load | +| publish_frequency | 1.0 | Frequency to publish costmap to topic | +| resolution | 0.1 | Resolution of 1 pixel of the costmap, in meters | +| robot_base_frame | "base_link" | Robot base frame | +| robot_radius| 0.1 | Robot radius to use, if footprint coordinates not provided | +| rolling_window | false | Whether costmap should roll with robot base frame | +| track_unknown_space | false | If false, treats unknown space as free space, else as unknown space | +| transform_tolerance | 0.3 | TF transform tolerance | +| trinary_costmap | true | If occupancy grid map should be interpreted as only 3 values (free, occupied, unknown) or with its stored values | +| unknown_cost_value | 255 | Cost of unknown space if tracking it | +| update_frequency | 5.0 | Costmap update frequency | +| use_maximum | false | whether when combining costmaps to use the maximum cost or override | +| clearable_layers | ["obstacle_layer"] | Layers that may be cleared using the clearing service | + +## static_layer plugin + +* ``: Name corresponding to the `nav2_costmap_2d::StaticLayer` plugin. This name gets defined in `plugin_names`, default value is `static_layer` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.enabled | true | Whether it is enabled | +| ``.subscribe_to_updates | false | Subscribe to static map updates after receiving first | +| ``.map_subscribe_transient_local | true | QoS settings for map topic | +| ``.transform_tolerance | 0.0 | TF tolerance | + +## inflation_layer plugin + +* ``: Name corresponding to the `nav2_costmap_2d::InflationLayer` plugin. This name gets defined in `plugin_names`, default value is `inflation_layer` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.enabled | true | Whether it is enabled | +| ``.inflation_radius | 0.55 | Radius to inflate costmap around lethal obstacles | +| ``.cost_scaling_factor | 10.0 | Exponential decay factor across inflation radius | +| ``.inflate_unknown | false | Whether to inflate unknown cells as if lethal | +| ``.inflate_around_unknown | false | Whether to inflate unknown cells | + +## obstacle_layer plugin + +* ``: Name corresponding to the `nav2_costmap_2d::ObstacleLayer` plugin. This name gets defined in `plugin_names`, default value is `obstacle_layer` +* ``: Name of a source provided in ```.observation_sources` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.enabled | true | Whether it is enabled | +| ``.footprint_clearing_enabled | true | Clear any occupied cells under robot footprint | +| ``.max_obstacle_height | 2.0 | Maximum height to add return to occupancy grid | +| ``.combination_method | 1 | Enum for method to add data to master costmap, default to maximum | +| ``.observation_sources | "" | namespace of sources of data | +| ``.topic | "" | Topic of data | +| ``.sensor_frame | "" | frame of sensor, to use if not provided by message | +| ``.observation_persistence | 0.0 | How long to store messages in a buffer to add to costmap before removing them (s) | +| ``.expected_update_rate | 0.0 | Expected rate to get new data from sensor | +| ``.data_type | "LaserScan" | Data type of input, LaserScan or PointCloud2 | +| ``.min_obstacle_height | 0.0 | Minimum height to add return to occupancy grid | +| ``.max_obstacle_height | 0.0 | Maximum height to add return to occupancy grid for this source | +| ``.inf_is_valid | false | Are infinite returns from laser scanners valid measurements | +| ``.marking | true | Whether source should mark in costmap | +| ``.clearing | false | Whether source should raytrace clear in costmap | +| ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | +| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | + +## voxel_layer plugin + +* ``: Name corresponding to the `nav2_costmap_2d::VoxelLayer` plugin. This name gets defined in `plugin_names` +* ``: Name of a source provided in ``.observation_sources` + +*Note*: These parameters will only get declared if a `` name such as `voxel_layer` is appended to `plugin_names` parameter and `"nav2_costmap_2d::VoxelLayer"` is appended to `plugin_types` parameter. + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.enabled | true | Whether it is enabled | +| ``.footprint_clearing_enabled | true | Clear any occupied cells under robot footprint | +| ``.max_obstacle_height | 2.0 | Maximum height to add return to occupancy grid | +| ``.z_voxels | 10 | Number of voxels high to mark, maximum 16| +| ``.origin_z | 0.0 | Where to start marking voxels (m) | +| ``.z_resolution | 0.2 | Resolution of voxels in height (m) | +| ``.unknown_threshold | 15 | Minimum number of empty voxels in a column to mark as unknown in 2D occupancy grid | +| ``.mark_threshold | 0 | Minimum number of voxels in a column to mark as occupied in 2D occupancy grid | +| ``.combination_method | 1 | Enum for method to add data to master costmap, default to maximum | +| ``.publish_voxel_map | false | Whether to publish 3D voxel grid, computationally expensive | +| ``.observation_sources | "" | namespace of sources of data | +| ``.topic | "" | Topic of data | +| ``.sensor_frame | "" | frame of sensor, to use if not provided by message | +| ``.observation_persistence | 0.0 | How long to store messages in a buffer to add to costmap before removing them (s) | +| ``.expected_update_rate | 0.0 | Expected rate to get new data from sensor | +| ``.data_type | "LaserScan" | Data type of input, LaserScan or PointCloud2 | +| ``.min_obstacle_height | 0.0 | Minimum height to add return to occupancy grid | +| ``.max_obstacle_height | 0.0 | Maximum height to add return to occupancy grid for this source | +| ``.inf_is_valid | false | Are infinite returns from laser scanners valid measurements | +| ``.marking | true | Whether source should mark in costmap | +| ``.clearing | false | Whether source should raytrace clear in costmap | +| ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | +| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | + +# controller_server + +| Parameter | Default | Description | +| ----------| --------| ------------| +| controller_frequency | 20.0 | Frequency to run controller | +| controller_plugin_ids | ["FollowPath"] | List of mapped names for controller plugins for processing requests and parameters | +| controller_plugin_types | ["dwb_core::DWBLocalPlanner"] | List of registered plugins to load | +| min_x_velocity_threshold | 0.0001 | Minimum X velocity to use (m/s) | +| min_y_velocity_threshold | 0.0001 | Minimum Y velocity to use (m/s) | +| min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) | +| required_movement_radius | 0.5 | Minimum amount a robot must move to be progressing to goal (m) | +| movement_time_allowance | 10.0 | Maximum amount of time a robot has to move the minimum radius (s) | + +# dwb_controller + +* ``: DWB plugin name defined in `controller_plugin_ids` in the controller_server parameters + +## dwb_local_planner + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.critics | N/A | List of critic plugins to use | +| ``.default_critic_namespaces | ["dwb_critics"] | Namespaces to load critics in | +| ``.prune_plan | true | Whether to prune the path of old, passed points | +| ``.prune_distance | 1.0 | Distance (m) to prune backward until | +| ``.debug_trajectory_details | false | Publish debug information | +| ``.trajectory_generator_name | "dwb_plugins::StandardTrajectoryGenerator" | Trajectory generator plugin name | +| ``.goal_checker_name | "dwb_plugins::SimpleGoalChecker" | Goal checker plugin name | +| ``.transform_tolerance | 0.1 | TF transform tolerance | +| ``.short_circuit_trajectory_evaluation | true | Stop evaluating scores after best score is found | +| ``.path_distance_bias | N/A | Old version of `PathAlign.scale`, use that instead | +| ``.goal_distance_bias | N/A | Old version of `GoalAlign.scale`, use that instead | +| ``.occdist_scale | N/A | Old version of `ObstacleFootprint.scale`, use that instead | +| ``.max_scaling_factor | N/A | Old version of `ObstacleFootprint.max_scaling_factor`, use that instead | +| ``.scaling_speed | N/A | Old version of `ObstacleFootprint.scaling_speed`, use that instead | +| ``.PathAlign.scale | 32.0 | Scale for path align critic, overriding local default | +| ``.GoalAlign.scale | 24.0 | Scale for goal align critic, overriding local default | +| ``.PathDist.scale | 32.0 | Scale for path distance critic, overriding local default | +| ``.GoalDist.scale | 24.0 | Scale for goal distance critic, overriding local default | + +## publisher + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.publish_evaluation |true | Whether to publish the local plan evaluation | +| ``.publish_global_plan | true | Whether to publish the global plan | +| ``.publish_transformed_plan | true | Whether to publish the global plan in the odometry frame | +| ``.publish_local_plan | true | Whether to publish the local planner's plan | +| ``.publish_trajectories | true | Whether to publish debug trajectories | +| ``.publish_cost_grid_pc | false | Whether to publish the cost grid | +| ``.marker_lifetime | 0.1 | How long for the marker to remain | + +## oscillation TrajectoryCritic + +* ``: oscillation critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.oscillation_reset_dist | 0.05 | Minimum distance to move to reset oscillation watchdog (m) | +| ``.``.oscillation_reset_angle | 0.2 | Minimum angular distance to move to reset watchdog (rad) | +| ``.``.oscillation_reset_time | -1 | Duration when a reset may be called. If -1, cannot be reset. | +| ``.``.x_only_threshold | 0.05 | Threshold to check in the X velocity direction | +| ``.``.scale | 1.0 | Weighed scale for critic | + +## kinematic_parameters + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.max_vel_theta | 0.0 | Maximum angular velocity (rad/s) | +| ``.min_speed_xy | 0.0 | Minimum translational speed (m/s) | +| ``.max_speed_xy | 0.0 | Maximum translational speed (m/s) | +| ``.min_speed_theta | 0.0 | Minimum angular speed (rad/s) | +| ``.min_vel_x | 0.0 | Minimum velocity X (m/s) | +| ``.min_vel_y | 0.0 | Minimum velocity Y (m/s) | +| ``.max_vel_x | 0.0 | Maximum velocity X (m/s) | +| ``.max_vel_y | 0.0 | Maximum velocity Y (m/s) | +| ``.acc_lim_x | 0.0 | Maximum acceleration X (m/s^2) | +| ``.acc_lim_y | 0.0 | Maximum acceleration Y (m/s^2) | +| ``.acc_lim_theta | 0.0 | Maximum acceleration rotation (rad/s^2) | +| ``.decel_lim_x | 0.0 | Maximum deceleration X (m/s^2) | +| ``.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) | +| ``.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) | + +## xy_theta_iterator + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.vx_samples | 20 | Number of velocity samples in the X velocity direction | +| ``.vy_samples | 5 | Number of velocity samples in the Y velocity direction | +| ``.vtheta_samples | 20 | Number of velocity samples in the angular directions | + +## base_obstacle TrajectoryCritic + +* ``: base_obstacle critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.sum_scores | false | Whether to allow for scores to be sumed up | +| ``.``.scale | 1.0 | Weight scale | + +## obstacle_footprint TrajectoryCritic + +* ``: obstacle_footprint critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.sum_scores | false | Whether to allow for scores to be sumed up | +| ``.``.scale | 1.0 | Weight scale | + +## prefer_forward TrajectoryCritic + +* ``: prefer_forward critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.penalty | 1.0 | Penalty to apply to backward motion | +| ``.``.strafe_x | 0.1 | Minimum X velocity before penalty | +| ``.``.strafe_theta | 0.2 | Minimum angular velocity before penalty | +| ``.``.theta_scale | 10.0 | Weight for angular velocity component | +| ``.``.scale | 1.0 | Weight scale | + +## twirling TrajectoryCritic + +* ``: twirling critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.scale | 0.0 | Weight scale | + +## goal_dist TrajectoryCritic + +| ``.``.aggregation_type | "last" | last, sum, or product combination methods | +| ``.``.scale | 1.0 | Weight scale | + +## goal_align TrajectoryCritic + +* ``: goal_align critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.forward_point_distance | 0.325 | Point in front of robot to look ahead to compute angular change from | +| ``.``.scale | 1.0 | Weight scale | +| ``.``.aggregation_type | "last" | last, sum, or product combination methods | + +## map_grid TrajectoryCritic + +* ``: map_grid critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.aggregation_type | "last" | last, sum, or product combination methods | +| ``.``.scale | 1.0 | Weight scale | + +## path_dist TrajectoryCritic + +* ``: path_dist critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.aggregation_type | "last" | last, sum, or product combination methods | +| ``.``.scale | 1.0 | Weight scale | + +## path_align TrajectoryCritic + +* ``: path_align critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.``.forward_point_distance | 0.325 | Point in front of robot to look ahead to compute angular change from | +| ``.``.scale | 1.0 | Weight scale | +| ``.``.aggregation_type | "last" | last, sum, or product combination methods | + +## rotate_to_goal TrajectoryCritic + +* ``: rotate_to_goal critic name defined in `.critics` + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | +| ``.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | +| ``.slowing_factor | 5.0 | Factor to slow robot motion by while rotating to goal | +| ``.lookahead_time | -1 | If > 0, amount of time to look forward for a collision for. | +| ``.``.scale | 1.0 | Weight scale | + +## simple_goal_checker plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.xy_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (m) | +| ``.yaw_goal_tolerance | 0.25 | Tolerance to meet goal completion criteria (rad) | +| ``.stateful | true | Whether to check for XY position tolerance after rotating to goal orientation in case of minor localization changes | + +## standard_traj_generator plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.sim_time | 1.7 | Time to simulate ahead by (s) | +| ``.discretize_by_time | false | If true, forward simulate by time. If False, forward simulate by linear and angular granularity | +| ``.time_granularity | 0.5 | Time ahead to project | +| ``.linear_granularity | 0.5 | Linear distance forward to project | +| ``.angular_granularity | 0.025 | Angular distance to project | +| ``.include_last_point | true | Whether to include the last pose in the trajectory | + +## limited_accel_generator plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.sim_time | N/A | Time to simulate ahead by (s) | + +## stopped_goal_checker plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.rot_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (rad/s) | +| ``.trans_stopped_velocity | 0.25 | Velocity below is considered to be stopped at tolerance met (m/s) | + +# lifecycle_manager + +| Parameter | Default | Description | +| ----------| --------| ------------| +| node_names | N/A | Ordered list of node names to bringup through lifecycle transition | +| autostart | false | Whether to transition nodes to active state on startup | + +# map_server + +## map_server + +| Parameter | Default | Description | +| ----------| --------| ------------| +| save_map_timeout | 2000 | Timeout to attempt to save map with (ms) | +| free_thresh_default | 0.25 | Free space maximum threshold for occupancy grid | +| occupied_thresh_default | 0.65 | Occupied space minimum threshhold for occupancy grid | + +## map_server + +| Parameter | Default | Description | +| ----------| --------| ------------| +| yaml_filename | N/A | Path to map yaml file | +| topic_name | "map" | topic to publish loaded map to | +| frame_id | "map" | Frame to publish loaded map in | + +# planner_server + +| Parameter | Default | Description | +| ----------| --------| ------------| +| planner_plugin_ids | ["GridBased"] | List of Mapped plugin names for parameters and processing requests | +| planner_plugin_types | ["nav2_navfn_planner/NavfnPlanner"] | List of registered pluginlib planner types to load | + +# navfn_planner + +* ``: Corresponding planner plugin ID for this type + +| Parameter | Default | Description | +| ----------| --------| ------------| +| ``.tolerance | 0.5 | Tolerance in meters between requested goal pose and end of path | +| ``.use_astar | false | Whether to use A*, if false, uses Dijstra's expansion | +| ``.allow_unknown | true | Whether to allow planning in unknown space | + +# waypoint_follower + +| Parameter | Default | Description | +| ----------| --------| ------------| +| stop_on_failure | true | Whether to fail action task if a single waypoint fails. If false, will continue to next waypoint. | +| loop_rate | 20 | Rate to check for results from current navigation task | + +# recoveries + +## recovery_server + +| Parameter | Default | Description | +| ----------| --------| ------------| +| costmap_topic | "local_costmap/costmap_raw" | Raw costmap topic for collision checking | +| footprint_topic | "local_costmap/published_footprint" | Topic for footprint in the costmap frame | +| cycle_frequency | 10.0 | Frequency to run recovery plugins | +| transform_tolerance | 0.1 | TF transform tolerance | +| global_frame | "odom" | Reference frame | +| robot_base_frame | "base_link" | Robot base frame | +| plugin_names | {"spin", "back_up", "wait"}| List of plugin names to use, also matches action server names | +| plugin_types | {"nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"} | List of registered plugin to map to names | + +## spin plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| simulate_ahead_time | 2.0 | Time to look ahead for collisions (s) | +| max_rotational_vel | 1.0 | Maximum rotational velocity (rad/s) | +| min_rotational_vel | 0.4 | Minimum rotational velocity (rad/s) | +| rotational_acc_lim | 3.2 | maximum rotational acceleration (rad/s^2) | + +## back_up plugin + +| Parameter | Default | Description | +| ----------| --------| ------------| +| simulate_ahead_time | 2.0 | Time to look ahead for collisions (s) | + +# AMCL + +| Parameter | Default | Description | +| ----------| --------| ------------| +| alpha1 | 0.2 | Expected process noise in odometry's rotation estimate from rotation | +| alpha2 | 0.2 | Expected process noise in odometry's rotation estimate from translation | +| alpha3 | 0.2 | Expected process noise in odometry's translation estimate from translation | +| alpha4 | 0.2 | Expected process noise in odometry's translation estimate from rotation | +| alpha5 | 0.2 | For Omni models only: translation noise | +| base_frame_id | "base_footprint" | Base frame | +| beam_skip_distance | 0.5 | Ignore beams that most particles disagree with in Likelihood field model. Maximum distance to consider skipping for (m) | +| beam_skip_error_threshold | 0.9 | Percentage of beams after not matching map to force full update due to bad convergance | +| beam_skip_threshold | 0.3 | Percentage of beams required to skip | +| do_beamskip | false | Whether to do beam skipping in Likelihood field model. | +| global_frame_id | "map" | The name of the coordinate frame published by the localization system | +| lambda_short | 0.1 | Exponential decay parameter for z_short part of model | +| laser_likelihood_max_dist | 2.0 | Maximum distance to do obstacle inflation on map, for use in likelihood_field model | +| laser_max_range | 100.0 | Maximum scan range to be considered, -1.0 will cause the laser's reported maximum range to be used | +| laser_min_range | -1 | Minimum scan range to be considered, -1.0 will cause the laser's reported minimum range to be used | +| laser_model_type | "likelihood_field" | Which model to use, either beam, likelihood_field, or likelihood_field_prob. Same as likelihood_field but incorporates the beamskip feature, if enabled | +| set_initial_pose | false | Causes AMCL to set initial pose from the initial_pose* parameters instead of waiting for the initial_pose message | +| initial_pose.x | 0.0 | X coordinate of the initial robot pose in the map frame | +| initial_pose.y | 0.0 | Y coordinate of the initial robot pose in the map frame | +| initial_pose.z | 0.0 | Z coordinate of the initial robot pose in the map frame | +| initial_pose.yaw | 0.0 | Yaw of the initial robot pose in the map frame | +| max_beams | 60 | How many evenly-spaced beams in each scan to be used when updating the filter | +| max_particles | 2000 | Maximum allowed number of particles | +| min_particles | 500 | Minimum allowed number of particles | +| odom_frame_id | "odom" | Which frame to use for odometry | +| pf_err | 0.05 | Particle Filter population error | +| pf_z | 0.99 | Particle filter population density | +| recovery_alpha_fast | 0.0 | Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001| +| resample_interval | 1 | Number of filter updates required before resampling | +| robot_model_type | "differential" | | +| save_pose_rate | 0.5 | Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter (-1.0 to disable) | +| sigma_hit | 0.2 | Standard deviation for Gaussian model used in z_hit part of the model. | +| tf_broadcast | true | Set this to false to prevent amcl from publishing the transform between the global frame and the odometry frame | +| transform_tolerance | 1.0 | Time with which to post-date the transform that is published, to indicate that this transform is valid into the future | +| update_min_a | 0.2 | Rotational movement required before performing a filter update | +| update_min_d | 0.25 | Translational movement required before performing a filter update | +| z_hit | 0.5 | Mixture weight for z_hit part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | +| z_max | 0.05 | Mixture weight for z_max part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | +| z_rand | 0.5 | Mixture weight for z_rand part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | +| z_short | 0.05 | Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | +| always_reset_initial_pose | false | Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize | + +--- + +# Behavior Tree Nodes + +## Actions + +### BT Node BackUp + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| backup_dist | -0.15 | Total distance to backup | +| backup_speed | 0.025 | Backup speed | +| server_name | N/A | Action server name | +| server_timeout | 10 | Action server timeout (ms) | + +### BT Node ClearEntireCostmap + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| service_name | N/A | Server name | +| server_timeout | 10 | Action server timeout (ms) | + +### BT Node ComputePathToPose + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| goal | N/A | Goal pose | +| planner_id | N/A | Mapped name to the planner plugin type to use, e.g. GridBased | +| server_name | N/A | Action server name | +| server_timeout | 10 | Action server timeout (ms) | + +| Output Port | Default | Description | +| ----------- | ------- | ----------- | +| path | N/A | Path created by action server | + +### BT Node FollowPath + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| path | N/A | Path to follow | +| controller_id | N/A | Mapped name of the controller plugin type to use, e.g. FollowPath | +| server_name | N/A | Action server name | +| server_timeout | 10 | Action server timeout (ms) | + +### BT Node NavigateToPose + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| position | N/A | Position | +| orientation | N/A | Orientation | +| server_name | N/A | Action server name | +| server_timeout | 10 | Action server timeout (ms) | + +### BT Node ReinitializeGlobalLocalization + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| service_name | N/A | Server name | +| server_timeout | 10 | Action server timeout (ms) | + +### BT Node Spin + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| spin_dist | 1.57 | Spin distance (radians) | +| server_name | N/A | Action server name | +| server_timeout | 10 | Action server timeout (ms) | + +### BT Node Wait + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| wait_duration | 1 | Wait time (s) | +| server_name | N/A | Action server name | +| server_timeout | 10 | Action server timeout (ms) | + +## Conditions + +### BT Node GoalReached + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| goal | N/A | Destination to check | +| global_frame | "map" | Reference frame | +| robot_base_frame | "base_link" | Robot base frame | + +### BT Node TransformAvailable (condition) + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| child | "" | Child frame for transform | +| parent | "" | parent frame for transform | + +## Controls + +### BT Node RecoveryNode + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| number_of_retries | 1 | Number of retries | + +## Decorators + +### BT Node DistanceController + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| distance | 1.0 | Distance (m) | +| global_frame | "map" | Reference frame | +| robot_base_frame | "base_link" | Robot base frame | + +### BT Node RateController + +| Input Port | Default | Description | +| ---------- | ------- | ----------- | +| hz | 10.0 | Rate to throttle | diff --git a/nav2_amcl/CMakeLists.txt b/nav2_amcl/CMakeLists.txt index 8af943d5cc..be83000bd4 100644 --- a/nav2_amcl/CMakeLists.txt +++ b/nav2_amcl/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(std_srvs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_msgs REQUIRED) nav2_package() @@ -21,6 +22,9 @@ include_directories( include ) +include(CheckSymbolExists) +check_symbol_exists(drand48 stdlib.h HAVE_DRAND48) + add_subdirectory(src/pf) add_subdirectory(src/map) add_subdirectory(src/motion_model) @@ -38,6 +42,11 @@ add_library(${library_name} SHARED src/amcl_node.cpp ) +target_include_directories(${library_name} PRIVATE src/include) +if(HAVE_DRAND48) + target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48") +endif() + set(dependencies rclcpp rclcpp_lifecycle @@ -50,6 +59,7 @@ set(dependencies tf2_ros tf2 nav2_util + nav2_msgs ) ament_target_dependencies(${executable_name} @@ -68,9 +78,13 @@ target_link_libraries(${library_name} map_lib motions_lib sensors_lib ) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 763a601ed8..669ea98835 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -34,6 +34,8 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_amcl/motion_model/motion_model.hpp" #include "nav2_amcl/sensors/laser/laser.hpp" +#include "nav2_msgs/msg/particle.hpp" +#include "nav2_msgs/msg/particle_cloud.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -117,6 +119,8 @@ class AmclNode : public nav2_util::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr pose_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr particlecloud_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + particle_cloud_pub_; void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan); diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index c339e8dbc6..f81dec37ca 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -32,6 +32,7 @@ tf2_ros tf2 nav2_util + nav2_msgs launch_ros launch_testing diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 53de6a7d31..c094260319 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -48,6 +48,8 @@ #include "tf2/utils.h" #pragma GCC diagnostic pop +#include "portable_utils.h" + using namespace std::placeholders; using namespace std::chrono_literals; @@ -245,7 +247,7 @@ AmclNode::waitForTransforms() while (rclcpp::ok() && !tf_buffer_->canTransform( global_frame_id_, odom_frame_id_, tf2::TimePointZero, - tf2::durationFromSec(1.0), &tf_error)) + transform_tolerance_, &tf_error)) { RCLCPP_INFO( get_logger(), "Timed out waiting for transform from %s to %s" @@ -266,6 +268,12 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) // Lifecycle publishers must be explicitly activated pose_pub_->on_activate(); particlecloud_pub_->on_activate(); + particle_cloud_pub_->on_activate(); + + RCLCPP_WARN( + get_logger(), + "Publishing the particle cloud as geometry_msgs/PoseArray msg is deprecated, " + "will be published as nav2_msgs/ParticleCloud in the future"); first_pose_sent_ = false; @@ -301,6 +309,7 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Lifecycle publishers must be explicitly deactivated pose_pub_->on_deactivate(); particlecloud_pub_->on_deactivate(); + particle_cloud_pub_->on_deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -333,6 +342,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // PubSub pose_pub_.reset(); particlecloud_pub_.reset(); + particle_cloud_pub_.reset(); // Odometry motion_model_.reset(); @@ -741,7 +751,7 @@ bool AmclNode::addNewScanner( ident.header.stamp = rclcpp::Time(); tf2::toMsg(tf2::Transform::getIdentity(), ident.pose); try { - tf_buffer_->transform(ident, laser_pose, base_frame_id_); + tf_buffer_->transform(ident, laser_pose, base_frame_id_, transform_tolerance_); } catch (tf2::TransformException & e) { RCLCPP_ERROR( get_logger(), "Couldn't transform from %s to %s, " @@ -852,17 +862,25 @@ AmclNode::publishParticleCloud(const pf_sample_set_t * set) { // If initial pose is not known, AMCL does not know the current pose if (!initial_pose_is_known_) {return;} - geometry_msgs::msg::PoseArray cloud_msg; - cloud_msg.header.stamp = this->now(); - cloud_msg.header.frame_id = global_frame_id_; - cloud_msg.poses.resize(set->sample_count); + auto cloud_with_weights_msg = std::make_unique(); + cloud_with_weights_msg->header.stamp = this->now(); + cloud_with_weights_msg->header.frame_id = global_frame_id_; + cloud_with_weights_msg->particles.resize(set->sample_count); + + auto cloud_msg = std::make_unique(); + cloud_msg->header.stamp = this->now(); + cloud_msg->header.frame_id = global_frame_id_; + cloud_msg->poses.resize(set->sample_count); for (int i = 0; i < set->sample_count; i++) { - cloud_msg.poses[i].position.x = set->samples[i].pose.v[0]; - cloud_msg.poses[i].position.y = set->samples[i].pose.v[1]; - cloud_msg.poses[i].position.z = 0; - cloud_msg.poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]); + cloud_msg->poses[i].position.x = set->samples[i].pose.v[0]; + cloud_msg->poses[i].position.y = set->samples[i].pose.v[1]; + cloud_msg->poses[i].position.z = 0; + cloud_msg->poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]); + cloud_with_weights_msg->particles[i].pose = (*cloud_msg).poses[i]; + cloud_with_weights_msg->particles[i].weight = set->samples[i].weight; } - particlecloud_pub_->publish(cloud_msg); + particlecloud_pub_->publish(std::move(cloud_msg)); + particle_cloud_pub_->publish(std::move(cloud_with_weights_msg)); } bool @@ -923,35 +941,35 @@ AmclNode::publishAmclPose( return; } - geometry_msgs::msg::PoseWithCovarianceStamped p; + auto p = std::make_unique(); // Fill in the header - p.header.frame_id = global_frame_id_; - p.header.stamp = laser_scan->header.stamp; + p->header.frame_id = global_frame_id_; + p->header.stamp = laser_scan->header.stamp; // Copy in the pose - p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; - p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; - p.pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]); + p->pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0]; + p->pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1]; + p->pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]); // Copy in the covariance, converting from 3-D to 6-D pf_sample_set_t * set = pf_->sets + pf_->current_set; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { // Report the overall filter covariance, rather than the // covariance for the highest-weight cluster - // p.covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; - p.pose.covariance[6 * i + j] = set->cov.m[i][j]; + // p->covariance[6*i+j] = hyps[max_weight_hyp].pf_pose_cov.m[i][j]; + p->pose.covariance[6 * i + j] = set->cov.m[i][j]; } } - p.pose.covariance[6 * 5 + 5] = set->cov.m[2][2]; + p->pose.covariance[6 * 5 + 5] = set->cov.m[2][2]; float temp = 0.0; - for (auto covariance_value : p.pose.covariance) { + for (auto covariance_value : p->pose.covariance) { temp += covariance_value; } - temp += p.pose.pose.position.x + p.pose.pose.position.y; + temp += p->pose.pose.position.x + p->pose.pose.position.y; if (!std::isnan(temp)) { RCLCPP_DEBUG(get_logger(), "Publishing pose"); - pose_pub_->publish(p); + last_published_pose_ = *p; first_pose_sent_ = true; - last_published_pose_ = p; + pose_pub_->publish(std::move(p)); } else { RCLCPP_WARN( get_logger(), "AMCL covariance or pose is NaN, likely due to an invalid " @@ -1242,6 +1260,10 @@ AmclNode::initPubSub() "particlecloud", rclcpp::SensorDataQoS()); + particle_cloud_pub_ = create_publisher( + "particle_cloud", + rclcpp::SensorDataQoS()); + pose_pub_ = create_publisher( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); diff --git a/nav2_amcl/src/include/portable_utils.h b/nav2_amcl/src/include/portable_utils.h new file mode 100644 index 0000000000..1577e6875b --- /dev/null +++ b/nav2_amcl/src/include/portable_utils.h @@ -0,0 +1,28 @@ +#ifndef PORTABLE_UTILS_H +#define PORTABLE_UTILS_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef HAVE_DRAND48 +// Some system (e.g., Windows) doesn't come with drand48(), srand48(). +// Use rand, and srand for such system. +static double drand48(void) +{ + return ((double)rand()) / RAND_MAX; +} + +static void srand48(long int seedval) +{ + srand(seedval); +} +#endif + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/nav2_amcl/src/map/CMakeLists.txt b/nav2_amcl/src/map/CMakeLists.txt index 7017eb494a..602629f9fd 100644 --- a/nav2_amcl/src/map/CMakeLists.txt +++ b/nav2_amcl/src/map/CMakeLists.txt @@ -10,5 +10,5 @@ install(TARGETS map_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/motion_model/CMakeLists.txt b/nav2_amcl/src/motion_model/CMakeLists.txt index 88ed7c70e5..6322ad9870 100644 --- a/nav2_amcl/src/motion_model/CMakeLists.txt +++ b/nav2_amcl/src/motion_model/CMakeLists.txt @@ -12,5 +12,5 @@ install(TARGETS motions_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt index b277cc9378..c6f16e6a7d 100644 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ b/nav2_amcl/src/pf/CMakeLists.txt @@ -11,9 +11,14 @@ add_library(pf_lib SHARED pf_draw.c ) +target_include_directories(pf_lib PRIVATE ../include) +if(HAVE_DRAND48) + target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") +endif() + install(TARGETS pf_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/pf/eig3.c b/nav2_amcl/src/pf/eig3.c index 8b290c8abc..a87354dbc8 100644 --- a/nav2_amcl/src/pf/eig3.c +++ b/nav2_amcl/src/pf/eig3.c @@ -7,8 +7,11 @@ #define MAX(a, b) ((a) > (b) ? (a) : (b)) #endif -// #define n 3 -static const int n = 3; +#ifdef _MSC_VER +#define n 3 +#else +static int n = 3; +#endif static double hypot2(double x, double y) { diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index 664fd44a13..851e63b64e 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -35,6 +35,8 @@ #include "nav2_amcl/pf/pf_pdf.hpp" #include "nav2_amcl/pf/pf_kdtree.hpp" +#include "portable_utils.h" + // Compute the required number of samples, given that there are k bins // with samples in them. diff --git a/nav2_amcl/src/pf/pf_pdf.c b/nav2_amcl/src/pf/pf_pdf.c index b6a1d13cda..858bea7df8 100644 --- a/nav2_amcl/src/pf/pf_pdf.c +++ b/nav2_amcl/src/pf/pf_pdf.c @@ -34,6 +34,8 @@ #include "nav2_amcl/pf/pf_pdf.hpp" +#include "portable_utils.h" + // Random number generator seed value static unsigned int pf_pdf_seed; diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c index a38c4e9994..29e183749e 100644 --- a/nav2_amcl/src/pf/pf_vector.c +++ b/nav2_amcl/src/pf/pf_vector.c @@ -53,7 +53,7 @@ int pf_vector_finite(pf_vector_t a) int i; for (i = 0; i < 3; i++) { - if (!finite(a.v[i])) { + if (!isfinite(a.v[i])) { return 0; } } @@ -152,7 +152,7 @@ int pf_matrix_finite(pf_matrix_t a) for (i = 0; i < 3; i++) { for (j = 0; j < 3; j++) { - if (!finite(a.m[i][j])) { + if (!isfinite(a.m[i][j])) { return 0; } } diff --git a/nav2_amcl/src/sensors/CMakeLists.txt b/nav2_amcl/src/sensors/CMakeLists.txt index 56c3a382bf..e105675cbf 100644 --- a/nav2_amcl/src/sensors/CMakeLists.txt +++ b/nav2_amcl/src/sensors/CMakeLists.txt @@ -4,11 +4,12 @@ add_library(sensors_lib SHARED laser/likelihood_field_model.cpp laser/likelihood_field_model_prob.cpp ) -target_link_libraries(sensors_lib pf_lib) +# map_update_cspace +target_link_libraries(sensors_lib pf_lib map_lib) install(TARGETS sensors_lib ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) diff --git a/nav2_amcl/src/sensors/laser/laser.cpp b/nav2_amcl/src/sensors/laser/laser.cpp index b19e83607d..9059179874 100644 --- a/nav2_amcl/src/sensors/laser/laser.cpp +++ b/nav2_amcl/src/sensors/laser/laser.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include "nav2_amcl/sensors/laser/laser.hpp" diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 117c2629d5..816fa62d2f 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -77,6 +77,15 @@ list(APPEND plugin_libs nav2_transform_available_condition_bt_node) add_library(nav2_goal_reached_condition_bt_node SHARED plugins/condition/goal_reached_condition.cpp) list(APPEND plugin_libs nav2_goal_reached_condition_bt_node) +add_library(nav2_goal_updated_condition_bt_node SHARED plugins/condition/goal_updated_condition.cpp) +list(APPEND plugin_libs nav2_goal_updated_condition_bt_node) + +add_library(nav2_time_expired_condition_bt_node SHARED plugins/condition/time_expired_condition.cpp) +list(APPEND plugin_libs nav2_time_expired_condition_bt_node) + +add_library(nav2_distance_traveled_condition_bt_node SHARED plugins/condition/distance_traveled_condition.cpp) +list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node) + add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp) list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node) @@ -86,15 +95,15 @@ list(APPEND plugin_libs nav2_reinitialize_global_localization_service_bt_node) add_library(nav2_rate_controller_bt_node SHARED plugins/decorator/rate_controller.cpp) list(APPEND plugin_libs nav2_rate_controller_bt_node) +add_library(nav2_distance_controller_bt_node SHARED plugins/decorator/distance_controller.cpp) +list(APPEND plugin_libs nav2_distance_controller_bt_node) + add_library(nav2_recovery_node_bt_node SHARED plugins/control/recovery_node.cpp) list(APPEND plugin_libs nav2_recovery_node_bt_node) add_library(nav2_navigate_to_pose_action_bt_node SHARED plugins/action/navigate_to_pose_action.cpp) list(APPEND plugin_libs nav2_navigate_to_pose_action_bt_node) -add_library(nav2_random_crawl_action_bt_node SHARED plugins/action/random_crawl_action.cpp) -list(APPEND plugin_libs nav2_random_crawl_action_bt_node) - add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) @@ -110,19 +119,25 @@ install(TARGETS ${library_name} ${plugin_libs} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ ) +install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(test) endif() -ament_export_include_directories(include) +ament_export_include_directories( + include +) ament_export_libraries( ${library_name} diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index 9fa155c74b..6872db8ac7 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -71,9 +71,10 @@ The nav2_behavior_tree package provides several navigation-specific nodes that a | GoalReached | Condition | Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, otherwise it returns FAILURE. | | IsStuck | Condition | Determines if the robot is not progressing towards the goal. If the robot is stuck and not progressing, the condition returns SUCCESS, otherwise it returns FAILURE. | | TransformAvailable | Condition | Checks if a TF transform is available. Returns failure if it cannot be found. Once found, it will always return success. Useful for initial condition checks. | +| GoalUpdated | Condition | Checks if the global navigation goal has changed in the blackboard. Returns failure if the goal is the same, if it changes, it returns success. | | NavigateToPose | Action | Invokes the NavigateToPose ROS2 action server, which is implemented by the bt_navigator module. | | RateController | Decorator | A node that throttles the tick rate for its child. The tick rate can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `RateController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | -| RandomCrawl | Action | This BT action invokes the RandomCrawl ROS2 action server, which is implemented by the nav2_experimental/nav2_rl/nav2_turtlebot3_rl (in the nav2_experimental branch) experimental module. The RandomCrawl action server will direct the robot to randomly navigate its environment without hitting any obstacles. | +| DistanceController | Decorator | A node that controls the tick rate for its child based on the distance traveled. The distance to be traveled before replanning can be supplied to the node as a parameter. The node returns RUNNING when it is not ticking its child. Currently, in the navigation stack, the `DistanceController` is used to adjust the rate at which the `ComputePathToPose` and `GoalReached` nodes are ticked. | | RecoveryNode | Control | The RecoveryNode is a control flow node with two children. It returns SUCCESS if and only if the first child returns SUCCESS. The second child will be executed only if the first child returns FAILURE. If the second child SUCCEEDS, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning FAILURE. In nav2, the RecoveryNode is included in Behavior Trees to implement recovery actions upon failures. | Spin | Action | Invokes the Spin ROS2 action server, which is implemented by the nav2_recoveries module. This action is using in nav2 Behavior Trees as a recovery behavior. | | PipelineSequence | Control | Ticks the first child till it succeeds, then ticks the first and second children till the second one succeeds. It then ticks the first, second, and third children until the third succeeds, and so on, and so on. If at any time a child returns RUNNING, that doesn't change the behavior. If at any time a child returns FAILURE, that stops all children and returns FAILURE overall.| diff --git a/nav2_behavior_tree/groot_instructions.md b/nav2_behavior_tree/groot_instructions.md new file mode 100644 index 0000000000..b1189a6ba6 --- /dev/null +++ b/nav2_behavior_tree/groot_instructions.md @@ -0,0 +1,12 @@ +# Instructions on using Groot +[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. + +##### 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. + +### BehaviorTree visualization +To visualize the behavior trees using Groot: +1. Open Groot in editor mode +2. Select the `Load palette from file` option (import button) near the top left corner. +3. Open the file `/path/to/navigation2/nav2_behavior_tree/nav2_tree_nodes.xml` to import all the behavior tree nodes used for navigation. +4. Select `Load tree` option near the top left corner +5. Browse the tree you want to visualize the select ok. diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index dccf466e8c..ca627bbe7e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -44,21 +44,17 @@ class BehaviorTreeEngine const std::string & xml_string, BT::Blackboard::Ptr blackboard); + // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state void haltAllActions(BT::TreeNode * root_node) { - auto visitor = [](BT::TreeNode * node) { - if (auto action = dynamic_cast(node)) { - action->halt(); - } - }; - BT::applyRecursiveVisitor(root_node, visitor); - } + // this halt signal should propagate through the entire tree. + root_node->halt(); - // In order to re-run a Behavior Tree, we must be able to reset all nodes to the initial state - void resetTree(BT::TreeNode * root_node) - { + // but, just in case... auto visitor = [](BT::TreeNode * node) { - node->setStatus(BT::NodeStatus::IDLE); + if (node->status() == BT::NodeStatus::RUNNING) { + node->halt(); + } }; BT::applyRecursiveVisitor(root_node, visitor); } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index 52cb37b5cb..0f58ffe998 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -21,19 +21,20 @@ #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_behavior_tree/bt_conversions.hpp" namespace nav2_behavior_tree { template -class BtActionNode : public BT::CoroActionNode +class BtActionNode : public BT::ActionNodeBase { public: BtActionNode( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) - : BT::CoroActionNode(xml_tag_name, conf), action_name_(action_name) + : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { node_ = config().blackboard->get("node"); @@ -41,6 +42,10 @@ class BtActionNode : public BT::CoroActionNode goal_ = typename ActionT::Goal(); result_ = typename rclcpp_action::ClientGoalHandle::WrappedResult(); + std::string remapped_action_name; + if (getInput("server_name", remapped_action_name)) { + action_name_ = remapped_action_name; + } createActionClient(action_name_); // Give the derive class a chance to do any initialization @@ -69,6 +74,7 @@ class BtActionNode : public BT::CoroActionNode static BT::PortsList providedBasicPorts(BT::PortsList addition) { BT::PortsList basic = { + BT::InputPort("server_name", "Action server name"), BT::InputPort("server_timeout") }; basic.insert(addition.begin(), addition.end()); @@ -96,69 +102,71 @@ class BtActionNode : public BT::CoroActionNode } // Called upon successful completion of the action. A derived class can override this - // method to put a value on the blackboard, for example - virtual void on_success() + // method to put a value on the blackboard, for example. + virtual BT::NodeStatus on_success() { + return BT::NodeStatus::SUCCESS; } - // The main override required by a BT action - BT::NodeStatus tick() override + // Called when a the action is aborted. By default, the node will return FAILURE. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_aborted() { - on_tick(); + return BT::NodeStatus::FAILURE; + } -new_goal_received: - goal_result_available_ = false; - auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { - if (result.code != rclcpp_action::ResultCode::ABORTED) { - goal_result_available_ = true; - result_ = result; - } - }; + // Called when a the action is cancelled. By default, the node will return SUCCESS. + // The user may override it to return another value, instead. + virtual BT::NodeStatus on_cancelled() + { + return BT::NodeStatus::SUCCESS; + } - auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + // The main override required by a BT action + BT::NodeStatus tick() override + { + // first step to be done only at the beginning of the Action + if (status() == BT::NodeStatus::IDLE) { + // setting the status to RUNNING to notify the BT Loggers (if any) + setStatus(BT::NodeStatus::RUNNING); - if (rclcpp::spin_until_future_complete(node_, future_goal_handle) != - rclcpp::executor::FutureReturnCode::SUCCESS) - { - throw std::runtime_error("send_goal failed"); - } + // user defined callback + on_tick(); - goal_handle_ = future_goal_handle.get(); - if (!goal_handle_) { - throw std::runtime_error("Goal was rejected by the action server"); + on_new_goal_received(); } - while (rclcpp::ok() && !goal_result_available_) { + // The following code corresponds to the "RUNNING" loop + if (rclcpp::ok() && !goal_result_available_) { + // user defined callback. May modify the value of "goal_updated_" on_wait_for_result(); - auto status = goal_handle_->get_status(); - if (goal_updated_ && (status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || - status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) + auto goal_status = goal_handle_->get_status(); + if (goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || + goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED)) { goal_updated_ = false; - goto new_goal_received; + on_new_goal_received(); } - // Yield to any other nodes - setStatusRunningAndYield(); rclcpp::spin_some(node_); + + // check if, after invoking spin_some(), we finally received the result + if (!goal_result_available_) { + // Yield this Action, returning RUNNING + return BT::NodeStatus::RUNNING; + } } switch (result_.code) { case rclcpp_action::ResultCode::SUCCEEDED: - on_success(); - setStatus(BT::NodeStatus::IDLE); - return BT::NodeStatus::SUCCESS; + return on_success(); case rclcpp_action::ResultCode::ABORTED: - setStatus(BT::NodeStatus::IDLE); - return BT::NodeStatus::FAILURE; + return on_aborted(); case rclcpp_action::ResultCode::CANCELED: - setStatus(BT::NodeStatus::IDLE); - return BT::NodeStatus::SUCCESS; + return on_cancelled(); default: throw std::logic_error("BtActionNode::Tick: invalid status value"); @@ -172,7 +180,7 @@ class BtActionNode : public BT::CoroActionNode if (should_cancel_goal()) { auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (rclcpp::spin_until_future_complete(node_, future_cancel) != - rclcpp::executor::FutureReturnCode::SUCCESS) + rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_ERROR( node_->get_logger(), @@ -181,7 +189,6 @@ class BtActionNode : public BT::CoroActionNode } setStatus(BT::NodeStatus::IDLE); - CoroActionNode::halt(); } protected: @@ -196,16 +203,46 @@ class BtActionNode : public BT::CoroActionNode auto status = goal_handle_->get_status(); // Check if the goal is still executing - if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || - status == action_msgs::msg::GoalStatus::STATUS_EXECUTING) + return status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || + status == action_msgs::msg::GoalStatus::STATUS_EXECUTING; + } + + + void on_new_goal_received() + { + goal_result_available_ = false; + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = + [this](const typename rclcpp_action::ClientGoalHandle::WrappedResult & result) { + if (result.code != rclcpp_action::ResultCode::ABORTED) { + goal_result_available_ = true; + result_ = result; + } + }; + + auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); + + if (rclcpp::spin_until_future_complete(node_, future_goal_handle) != + rclcpp::executor::FutureReturnCode::SUCCESS) { - return true; + throw std::runtime_error("send_goal failed"); } - return false; + goal_handle_ = future_goal_handle.get(); + if (!goal_handle_) { + throw std::runtime_error("Goal was rejected by the action server"); + } + } + + void increment_recovery_count() + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT } - const std::string action_name_; + std::string action_name_; typename std::shared_ptr> action_client_; // All ROS2 actions have a goal and a result diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 39ef9c191b..2b7b16bfb9 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -21,18 +21,19 @@ #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" +#include "nav2_behavior_tree/bt_conversions.hpp" namespace nav2_behavior_tree { template -class BtServiceNode : public BT::CoroActionNode +class BtServiceNode : public BT::SyncActionNode { public: BtServiceNode( const std::string & service_node_name, const BT::NodeConfiguration & conf) - : BT::CoroActionNode(service_node_name, conf), service_node_name_(service_node_name) + : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name) { node_ = config().blackboard->get("node"); @@ -45,6 +46,9 @@ class BtServiceNode : public BT::CoroActionNode getInput("service_name", service_name_); service_client_ = node_->create_client(service_name_); + // Make a request for the service without parameter + request_ = std::make_shared(); + // Make sure the server is actually there before continuing RCLCPP_INFO( node_->get_logger(), "Waiting for \"%s\" service", @@ -85,14 +89,25 @@ class BtServiceNode : public BT::CoroActionNode { on_tick(); auto future_result = service_client_->async_send_request(request_); + return check_future(future_result); + } - rclcpp::executor::FutureReturnCode rc; + // Fill in service request with information if necessary + virtual void on_tick() + { + } + + // Check the future and decide the status of Behaviortree + virtual BT::NodeStatus check_future( + std::shared_future future_result) + { + rclcpp::FutureReturnCode rc; rc = rclcpp::spin_until_future_complete( node_, future_result, server_timeout_); - if (rc == rclcpp::executor::FutureReturnCode::SUCCESS) { + if (rc == rclcpp::FutureReturnCode::SUCCESS) { return BT::NodeStatus::SUCCESS; - } else if (rc == rclcpp::executor::FutureReturnCode::TIMEOUT) { + } else if (rc == rclcpp::FutureReturnCode::TIMEOUT) { RCLCPP_WARN( node_->get_logger(), "Node timed out while executing service call to %s.", service_name_.c_str()); @@ -101,12 +116,6 @@ class BtServiceNode : public BT::CoroActionNode return BT::NodeStatus::FAILURE; } - // Fill in service request with information if necessary - virtual void on_tick() - { - request_ = std::make_shared(); - } - // An opportunity to do something after // a timeout waiting for a result that hasn't been received yet virtual void on_wait_for_result() @@ -114,6 +123,14 @@ class BtServiceNode : public BT::CoroActionNode } protected: + void increment_recovery_count() + { + int recovery_count = 0; + config().blackboard->get("number_recoveries", recovery_count); // NOLINT + recovery_count += 1; + config().blackboard->set("number_recoveries", recovery_count); // NOLINT + } + std::string service_name_, service_node_name_; typename std::shared_ptr> service_client_; std::shared_ptr request_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp new file mode 100644 index 0000000000..b7ad07cc7f --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_controller.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_ + +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_ros/buffer.h" + +#include "behaviortree_cpp_v3/decorator_node.h" + +namespace nav2_behavior_tree +{ + +class DistanceController : public BT::DecoratorNode +{ +public: + DistanceController( + const std::string & name, + const BT::NodeConfiguration & conf); + + // Any BT node that accepts parameters must provide a requiredNodeParameters method + static BT::PortsList providedPorts() + { + return { + BT::InputPort("distance", 1.0, "Distance"), + BT::InputPort("global_frame", std::string("map"), "Global frame"), + BT::InputPort("robot_base_frame", std::string("base_link"), "Robot base frame") + }; + } + +private: + BT::NodeStatus tick() override; + + rclcpp::Node::SharedPtr node_; + + std::shared_ptr tf_; + double transform_tolerance_; + + geometry_msgs::msg::PoseStamped start_pose_; + double distance_; + + std::string global_frame_; + std::string robot_base_frame_; + + bool first_time_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_CONTROLLER_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp new file mode 100644 index 0000000000..e9a4b6bde5 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/distance_traveled_condition.hpp @@ -0,0 +1,65 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/condition_node.h" + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_ros/buffer.h" + +namespace nav2_behavior_tree +{ + +class DistanceTraveledCondition : public BT::ConditionNode +{ +public: + DistanceTraveledCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + DistanceTraveledCondition() = delete; + + BT::NodeStatus tick() override; + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("distance", 1.0, "Distance"), + BT::InputPort("global_frame", std::string("map"), "Global frame"), + BT::InputPort("robot_base_frame", std::string("base_link"), "Robot base frame") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + std::shared_ptr tf_; + + geometry_msgs::msg::PoseStamped start_pose_; + + double distance_; + double transform_tolerance_; + std::string global_frame_; + std::string robot_base_frame_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__DISTANCE_TRAVELED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp new file mode 100644 index 0000000000..6fb99d60ca --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/time_expired_condition.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ + +#include + +#include "rclcpp/rclcpp.hpp" +#include "behaviortree_cpp_v3/condition_node.h" + +namespace nav2_behavior_tree +{ + +class TimeExpiredCondition : public BT::ConditionNode +{ +public: + TimeExpiredCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf); + + TimeExpiredCondition() = delete; + + BT::NodeStatus tick() override; + + // Any BT node that accepts parameters must provide a requiredNodeParameters method + static BT::PortsList providedPorts() + { + return { + BT::InputPort("seconds", 1.0, "Seconds") + }; + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Time start_; + double period_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__TIME_EXPIRED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml new file mode 100644 index 0000000000..35675d9938 --- /dev/null +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -0,0 +1,76 @@ + + + + + + + Distance to backup + Speed at which to backup + + + + Service name + + + + Destination to plan to + Path created by ComputePathToPose node + + + + + + Path to follow + + + + Orientation + Position + + + + + + Spin distance + + + + Wait time + + + + + Destination + + + + + + Child frame for transform + Parent frame for transform + + + + + + + Number of retries + + + + + + + Rate + + + + Distance + + + + diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 21326342a3..c1eec645fb 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -51,6 +51,11 @@ class BackUpAction : public BtActionNode goal_.speed = speed; } + void on_tick() override + { + increment_recovery_count(); + } + static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp index 144244788f..8482e464c3 100644 --- a/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp +++ b/nav2_behavior_tree/plugins/action/clear_costmap_service.cpp @@ -34,6 +34,11 @@ class ClearEntireCostmapService : public BtServiceNode(service_node_name, conf) { } + + void on_tick() override + { + increment_recovery_count(); + } }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index b8028892cb..be53df37fe 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -34,11 +34,6 @@ class ComputePathToPoseAction : public BtActionNode(xml_tag_name, action_name, conf) { - std::string remapped_action_name; - if (getInput("server_name", remapped_action_name)) { - action_client_.reset(); - createActionClient(remapped_action_name); - } } void on_tick() override @@ -47,7 +42,7 @@ class ComputePathToPoseAction : public BtActionNodepath); @@ -56,6 +51,7 @@ class ComputePathToPoseAction : public BtActionNodeset("path_updated", true); } + return BT::NodeStatus::SUCCESS; } static BT::PortsList providedPorts() @@ -65,7 +61,6 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort("planner_id", ""), - BT::InputPort("server_name", "") }); } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index d3cbbdafe8..820362912e 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -33,11 +33,6 @@ class FollowPathAction : public BtActionNode const BT::NodeConfiguration & conf) : BtActionNode(xml_tag_name, action_name, conf) { - std::string remapped_action_name; - if (getInput("server_name", remapped_action_name)) { - action_client_.reset(); - createActionClient(remapped_action_name); - } config().blackboard->set("path_updated", false); } @@ -67,7 +62,6 @@ class FollowPathAction : public BtActionNode { BT::InputPort("path", "Path to follow"), BT::InputPort("controller_id", ""), - BT::InputPort("server_name", "") }); } }; diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 79d65475e6..a65e6433f6 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -22,7 +22,6 @@ #include "geometry_msgs/msg/quaternion.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_behavior_tree/bt_conversions.hpp" namespace nav2_behavior_tree { diff --git a/nav2_behavior_tree/plugins/action/random_crawl_action.cpp b/nav2_behavior_tree/plugins/action/random_crawl_action.cpp deleted file mode 100644 index 0cff27603e..0000000000 --- a/nav2_behavior_tree/plugins/action/random_crawl_action.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_ -#define NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_ - -#include -#include - -#include "nav2_behavior_tree/bt_action_node.hpp" -#include "nav2_msgs/action/random_crawl.hpp" - -namespace nav2_behavior_tree -{ - -class RandomCrawlAction : public BtActionNode -{ -public: - explicit RandomCrawlAction( - const std::string & xml_tag_name, - const std::string & action_name, - const BT::NodeConfiguration & conf) - : BtActionNode(xml_tag_name, action_name, conf) - { - } -}; - -} // namespace nav2_behavior_tree - -#include "behaviortree_cpp_v3/bt_factory.h" -BT_REGISTER_NODES(factory) -{ - BT::NodeBuilder builder = - [](const std::string & name, const BT::NodeConfiguration & config) - { - return std::make_unique( - name, "random_crawl", config); - }; - factory.registerBuilder( - "RandomCrawl", builder); -} - -#endif // NAV2_BEHAVIOR_TREE__RANDOM_CRAWL_ACTION_HPP_ diff --git a/nav2_behavior_tree/plugins/action/spin_action.cpp b/nav2_behavior_tree/plugins/action/spin_action.cpp index afa5af2bcd..ad668232c6 100644 --- a/nav2_behavior_tree/plugins/action/spin_action.cpp +++ b/nav2_behavior_tree/plugins/action/spin_action.cpp @@ -42,6 +42,11 @@ class SpinAction : public BtActionNode goal_.target_yaw = dist; } + void on_tick() override + { + increment_recovery_count(); + } + static BT::PortsList providedPorts() { return providedBasicPorts( diff --git a/nav2_behavior_tree/plugins/action/wait_action.cpp b/nav2_behavior_tree/plugins/action/wait_action.cpp index 85d0af0cd7..268a1f7e9b 100644 --- a/nav2_behavior_tree/plugins/action/wait_action.cpp +++ b/nav2_behavior_tree/plugins/action/wait_action.cpp @@ -46,6 +46,11 @@ class WaitAction : public BtActionNode goal_.time.sec = duration; } + void on_tick() override + { + increment_recovery_count(); + } + // Any BT node that accepts parameters must provide a requiredNodeParameters method static BT::PortsList providedPorts() { diff --git a/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp new file mode 100644 index 0000000000..cb700e7a41 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/distance_traveled_condition.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ + +#include +#include + +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp" + +namespace nav2_behavior_tree +{ + +DistanceTraveledCondition::DistanceTraveledCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + distance_(1.0), + transform_tolerance_(0.1), + global_frame_("map"), + robot_base_frame_("base_link") +{ + getInput("distance", distance_); + getInput("global_frame", global_frame_); + getInput("robot_base_frame", robot_base_frame_); + node_ = config().blackboard->get("node"); + tf_ = config().blackboard->get>("tf_buffer"); + node_->get_parameter("transform_tolerance", transform_tolerance_); +} + +BT::NodeStatus DistanceTraveledCondition::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + if (!nav2_util::getCurrentPose( + start_pose_, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); + } + return BT::NodeStatus::FAILURE; + } + + // Determine distance travelled since we've started this iteration + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); + return BT::NodeStatus::FAILURE; + } + + // Get euclidean distance + auto travelled = nav2_util::geometry_utils::euclidean_distance( + start_pose_.pose, current_pose.pose); + + if (travelled < distance_) { + return BT::NodeStatus::FAILURE; + } + + // Update start pose + start_pose_ = current_pose; + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("DistanceTraveled"); +} + +#endif // NAV2_BEHAVIOR_TREE__DISTANCE_TRAVELED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp index e3c2cd02b8..db49ab82fa 100644 --- a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp @@ -23,6 +23,7 @@ #include "nav2_util/robot_utils.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "tf2_ros/buffer.h" +#include "nav2_util/node_utils.hpp" namespace nav2_behavior_tree { @@ -33,8 +34,13 @@ class GoalReachedCondition : public BT::ConditionNode GoalReachedCondition( const std::string & condition_name, const BT::NodeConfiguration & conf) - : BT::ConditionNode(condition_name, conf), initialized_(false) + : BT::ConditionNode(condition_name, conf), + initialized_(false), + global_frame_("map"), + robot_base_frame_("base_link") { + getInput("global_frame", global_frame_); + getInput("robot_base_frame", robot_base_frame_); } GoalReachedCondition() = delete; @@ -59,9 +65,15 @@ class GoalReachedCondition : public BT::ConditionNode void initialize() { node_ = config().blackboard->get("node"); + + nav2_util::declare_parameter_if_not_declared( + node_, "goal_reached_tol", + rclcpp::ParameterValue(0.25)); node_->get_parameter_or("goal_reached_tol", goal_reached_tol_, 0.25); tf_ = config().blackboard->get>("tf_buffer"); + node_->get_parameter("transform_tolerance", transform_tolerance_); + initialized_ = true; } @@ -70,7 +82,9 @@ class GoalReachedCondition : public BT::ConditionNode { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_)) { + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, transform_tolerance_)) + { RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); return false; } @@ -80,17 +94,15 @@ class GoalReachedCondition : public BT::ConditionNode double dx = goal.pose.position.x - current_pose.pose.position.x; double dy = goal.pose.position.y - current_pose.pose.position.y; - if ( (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_) ) { - return true; - } else { - return false; - } + return (dx * dx + dy * dy) <= (goal_reached_tol_ * goal_reached_tol_); } static BT::PortsList providedPorts() { return { - BT::InputPort("goal", "Destination") + BT::InputPort("goal", "Destination"), + BT::InputPort("global_frame", std::string("map"), "Global frame"), + BT::InputPort("robot_base_frame", std::string("base_link"), "Robot base frame") }; } @@ -105,6 +117,9 @@ class GoalReachedCondition : public BT::ConditionNode bool initialized_; double goal_reached_tol_; + std::string global_frame_; + std::string robot_base_frame_; + double transform_tolerance_; }; } // namespace nav2_behavior_tree diff --git a/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp new file mode 100644 index 0000000000..31f992fe45 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/goal_updated_condition.cpp @@ -0,0 +1,71 @@ +// Copyright (c) 2020 Aitor Miguel Blanco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ + +#include + +#include "behaviortree_cpp_v3/condition_node.h" +#include "geometry_msgs/msg/pose_stamped.hpp" + +namespace nav2_behavior_tree +{ + +class GoalUpdatedCondition : public BT::ConditionNode +{ +public: + GoalUpdatedCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) + : BT::ConditionNode(condition_name, conf) + { + } + + GoalUpdatedCondition() = delete; + + BT::NodeStatus tick() override + { + if (status() == BT::NodeStatus::IDLE) { + goal_ = config().blackboard->get("goal"); + return BT::NodeStatus::FAILURE; + } + + auto current_goal = config().blackboard->get("goal"); + if (goal_ != current_goal) { + goal_ = current_goal; + return BT::NodeStatus::SUCCESS; + } + + return BT::NodeStatus::FAILURE; + } + + static BT::PortsList providedPorts() + { + return {}; + } + +private: + geometry_msgs::msg::PoseStamped goal_; +}; + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GoalUpdated"); +} + +#endif // NAV2_BEHAVIOR_TREE__GOAL_UPDATED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp new file mode 100644 index 0000000000..e075c420d3 --- /dev/null +++ b/nav2_behavior_tree/plugins/condition/time_expired_condition.cpp @@ -0,0 +1,69 @@ +// Copyright (c) 2019 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ +#define NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ + +#include +#include + +#include "behaviortree_cpp_v3/condition_node.h" + +#include "nav2_behavior_tree/plugins/time_expired_condition.hpp" + +namespace nav2_behavior_tree +{ + +TimeExpiredCondition::TimeExpiredCondition( + const std::string & condition_name, + const BT::NodeConfiguration & conf) +: BT::ConditionNode(condition_name, conf), + period_(1.0) +{ + getInput("seconds", period_); + node_ = config().blackboard->get("node"); + start_ = node_->now(); +} + +BT::NodeStatus TimeExpiredCondition::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + start_ = node_->now(); + return BT::NodeStatus::FAILURE; + } + + // Determine how long its been since we've started this iteration + auto elapsed = node_->now() - start_; + + // Now, get that in seconds + auto seconds = elapsed.seconds(); + + if (seconds < period_) { + return BT::NodeStatus::FAILURE; + } + + start_ = node_->now(); // Reset the timer + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("TimeExpired"); +} + +#endif // NAV2_BEHAVIOR_TREE__TIME_EXPIRED_CONDITION_HPP_ diff --git a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp index 7c7dcf7059..75333b4a8f 100644 --- a/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp +++ b/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp @@ -82,7 +82,7 @@ BT::NodeStatus PipelineSequence::tick() auto status = children_nodes_[i]->executeTick(); switch (status) { case BT::NodeStatus::FAILURE: - haltChildren(0); + ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return status; break; @@ -106,7 +106,7 @@ BT::NodeStatus PipelineSequence::tick() } } // Wrap up. - haltChildren(0); + ControlNode::haltChildren(); last_child_ticked_ = 0; // reset return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/control/recovery_node.cpp b/nav2_behavior_tree/plugins/control/recovery_node.cpp index 945f8f8a35..2a213cfa4a 100644 --- a/nav2_behavior_tree/plugins/control/recovery_node.cpp +++ b/nav2_behavior_tree/plugins/control/recovery_node.cpp @@ -88,7 +88,7 @@ class RecoveryNode : public BT::ControlNode current_child_idx_++; break; } else { - haltChildren(0); + ControlNode::haltChildren(); return BT::NodeStatus::FAILURE; } } @@ -111,7 +111,7 @@ class RecoveryNode : public BT::ControlNode { retry_count_++; current_child_idx_--; - haltChildren(1); + ControlNode::haltChildren(); } break; diff --git a/nav2_behavior_tree/plugins/control/round_robin_node.cpp b/nav2_behavior_tree/plugins/control/round_robin_node.cpp index 95b1c96e92..be85e75ec7 100644 --- a/nav2_behavior_tree/plugins/control/round_robin_node.cpp +++ b/nav2_behavior_tree/plugins/control/round_robin_node.cpp @@ -46,11 +46,11 @@ class RoundRobinNode : public BT::ControlNode current_child_idx_ = 0; } - haltChildren(0); + ControlNode::haltChildren(); return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: - haltChildren(0); + ControlNode::haltChildren(); return BT::NodeStatus::FAILURE; case BT::NodeStatus::RUNNING: diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp new file mode 100644 index 0000000000..a7d9c1f5fc --- /dev/null +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp @@ -0,0 +1,120 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2_ros/buffer.h" + +#include "behaviortree_cpp_v3/decorator_node.h" + +#include "nav2_behavior_tree/plugins/distance_controller.hpp" + +namespace nav2_behavior_tree +{ + +DistanceController::DistanceController( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::DecoratorNode(name, conf), + distance_(1.0), + global_frame_("map"), + robot_base_frame_("base_link"), + first_time_(false) +{ + getInput("distance", distance_); + getInput("global_frame", global_frame_); + getInput("robot_base_frame", robot_base_frame_); + node_ = config().blackboard->get("node"); + tf_ = config().blackboard->get>("tf_buffer"); + + node_->get_parameter("transform_tolerance", transform_tolerance_); +} + +inline BT::NodeStatus DistanceController::tick() +{ + if (status() == BT::NodeStatus::IDLE) { + // Reset the starting position since we're starting a new iteration of + // the distance controller (moving from IDLE to RUNNING) + if (!nav2_util::getCurrentPose( + start_pose_, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); + return BT::NodeStatus::FAILURE; + } + first_time_ = true; + } + + setStatus(BT::NodeStatus::RUNNING); + + // Determine distance travelled since we've started this iteration + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); + return BT::NodeStatus::FAILURE; + } + + // Get euclidean distance + auto travelled = nav2_util::geometry_utils::euclidean_distance( + start_pose_.pose, current_pose.pose); + + // The child gets ticked the first time through and every time the threshold + // distance is crossed. In addition, once the child begins to run, it is + // ticked each time 'til completion + if (first_time_ || (child_node_->status() == BT::NodeStatus::RUNNING) || + travelled >= distance_) + { + first_time_ = false; + const BT::NodeStatus child_state = child_node_->executeTick(); + + switch (child_state) { + case BT::NodeStatus::RUNNING: + return BT::NodeStatus::RUNNING; + + case BT::NodeStatus::SUCCESS: + if (!nav2_util::getCurrentPose( + start_pose_, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available."); + return BT::NodeStatus::FAILURE; + } + return BT::NodeStatus::SUCCESS; + + case BT::NodeStatus::FAILURE: + default: + return BT::NodeStatus::FAILURE; + } + } + + return status(); +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("DistanceController"); +} diff --git a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp index a80e1d078c..2c972eacd7 100644 --- a/nav2_behavior_tree/plugins/decorator/rate_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/rate_controller.cpp @@ -86,13 +86,11 @@ inline BT::NodeStatus RateController::tick() return BT::NodeStatus::RUNNING; case BT::NodeStatus::SUCCESS: - child_node_->setStatus(BT::NodeStatus::IDLE); start_ = std::chrono::high_resolution_clock::now(); // Reset the timer return BT::NodeStatus::SUCCESS; case BT::NodeStatus::FAILURE: default: - child_node_->setStatus(BT::NodeStatus::IDLE); return BT::NodeStatus::FAILURE; } } diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index acf962d356..36b852e61c 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -47,11 +47,11 @@ BehaviorTreeEngine::run( // Loop until something happens with ROS or the node completes while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { if (cancelRequested()) { - tree->root_node->halt(); + tree->rootNode()->halt(); return BtStatus::CANCELED; } - result = tree->root_node->executeTick(); + result = tree->tickRoot(); onLoop(); diff --git a/nav2_behavior_tree/test/CMakeLists.txt b/nav2_behavior_tree/test/CMakeLists.txt new file mode 100644 index 0000000000..243305dd1e --- /dev/null +++ b/nav2_behavior_tree/test/CMakeLists.txt @@ -0,0 +1,29 @@ +ament_add_gtest(test_decorator_distance_controller + plugins/decorator/test_distance_controller.cpp +) +target_link_libraries(test_decorator_distance_controller + nav2_distance_controller_bt_node +) +ament_target_dependencies(test_decorator_distance_controller + ${dependencies} +) + +ament_add_gtest(test_condition_distance_traveled + plugins/condition/test_distance_traveled.cpp +) +target_link_libraries(test_condition_distance_traveled + nav2_distance_traveled_condition_bt_node +) +ament_target_dependencies(test_condition_distance_traveled + ${dependencies} +) + +ament_add_gtest(test_condition_time_expired + plugins/condition/test_time_expired.cpp +) +target_link_libraries(test_condition_time_expired + nav2_time_expired_condition_bt_node +) +ament_target_dependencies(test_condition_time_expired + ${dependencies} +) diff --git a/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp new file mode 100644 index 0000000000..32f553f6c6 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_distance_traveled.cpp @@ -0,0 +1,134 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_transform_handler.hpp" +#include "../../test_dummy_tree_node.hpp" +#include "nav2_behavior_tree/plugins/distance_traveled_condition.hpp" + +class DistanceTraveledConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + transform_handler_ = new nav2_behavior_tree::TransformHandler(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + rclcpp::Node::SharedPtr(transform_handler_)); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete transform_handler_; + delete config_; + transform_handler_ = nullptr; + config_ = nullptr; + } + + void SetUp() + { + node_ = new nav2_behavior_tree::DistanceTraveledCondition("distance_traveled", *config_); + } + + void TearDown() + { + node_ = nullptr; + } + +protected: + static nav2_behavior_tree::TransformHandler * transform_handler_; + static BT::NodeConfiguration * config_; + static nav2_behavior_tree::DistanceTraveledCondition * node_; +}; + +nav2_behavior_tree::TransformHandler * DistanceTraveledConditionTestFixture::transform_handler_ = + nullptr; +BT::NodeConfiguration * DistanceTraveledConditionTestFixture::config_ = nullptr; +nav2_behavior_tree::DistanceTraveledCondition * DistanceTraveledConditionTestFixture::node_ = + nullptr; + +TEST_F(DistanceTraveledConditionTestFixture, test_behavior) +{ + EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.orientation.w = 1; + + double traveled = 0; + for (int i = 1; i <= 20; i++) { + pose.pose.position.x = i * 0.51; + transform_handler_->updateRobotPose(pose.pose); + + // Wait for transforms to actually update + // updated pose is i * 0.51 + // we wait for the traveled distance to reach a value > i * 0.5 + // we can assume the current transform has been updated at this point + while (traveled < i * 0.5) { + if (nav2_util::getCurrentPose(pose, *transform_handler_->getBuffer())) { + traveled = pose.pose.position.x; + } + } + + if (i % 2) { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + } else { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp new file mode 100644 index 0000000000..db19ce1444 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/condition/test_time_expired.cpp @@ -0,0 +1,116 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_transform_handler.hpp" +#include "nav2_behavior_tree/plugins/time_expired_condition.hpp" + +using namespace std::chrono; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class TimeExpiredConditionTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + transform_handler_ = new nav2_behavior_tree::TransformHandler(); + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + rclcpp::Node::SharedPtr(transform_handler_)); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete transform_handler_; + delete config_; + transform_handler_ = nullptr; + config_ = nullptr; + } + + void SetUp() + { + node_ = new nav2_behavior_tree::TimeExpiredCondition("time_expired", *config_); + } + + void TearDown() + { + node_ = nullptr; + } + +protected: + static nav2_behavior_tree::TransformHandler * transform_handler_; + static BT::NodeConfiguration * config_; + static nav2_behavior_tree::TimeExpiredCondition * node_; +}; + +nav2_behavior_tree::TransformHandler * TimeExpiredConditionTestFixture::transform_handler_ = + nullptr; +BT::NodeConfiguration * TimeExpiredConditionTestFixture::config_ = nullptr; +nav2_behavior_tree::TimeExpiredCondition * TimeExpiredConditionTestFixture::node_ = nullptr; + +TEST_F(TimeExpiredConditionTestFixture, test_behavior) +{ + EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + + for (int i = 0; i < 20; ++i) { + rclcpp::sleep_for(500ms); + if (i % 2) { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + } else { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::FAILURE); + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp new file mode 100644 index 0000000000..a1b2509d95 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/decorator/test_distance_controller.cpp @@ -0,0 +1,145 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/robot_utils.hpp" + +#include "../../test_transform_handler.hpp" +#include "../../test_dummy_tree_node.hpp" +#include "nav2_behavior_tree/plugins/distance_controller.hpp" + +class DistanceControllerTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + transform_handler_ = new nav2_behavior_tree::TransformHandler(); + config_ = new BT::NodeConfiguration(); + dummy_node_ = new nav2_behavior_tree::DummyNode(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set( + "node", + rclcpp::Node::SharedPtr(transform_handler_)); + config_->blackboard->set>( + "tf_buffer", + transform_handler_->getBuffer()); + config_->blackboard->set( + "server_timeout", + std::chrono::milliseconds(10)); + config_->blackboard->set("path_updated", false); + config_->blackboard->set("initial_pose_received", false); + + transform_handler_->activate(); + transform_handler_->waitForTransform(); + } + + static void TearDownTestCase() + { + transform_handler_->deactivate(); + delete transform_handler_; + delete config_; + delete dummy_node_; + transform_handler_ = nullptr; + config_ = nullptr; + dummy_node_ = nullptr; + } + + void SetUp() + { + node_ = new nav2_behavior_tree::DistanceController("distance_controller", *config_); + node_->setChild(dummy_node_); + } + + void TearDown() + { + node_ = nullptr; + } + +protected: + static nav2_behavior_tree::TransformHandler * transform_handler_; + static BT::NodeConfiguration * config_; + static nav2_behavior_tree::DistanceController * node_; + static nav2_behavior_tree::DummyNode * dummy_node_; +}; + +nav2_behavior_tree::TransformHandler * DistanceControllerTestFixture::transform_handler_ = nullptr; +BT::NodeConfiguration * DistanceControllerTestFixture::config_ = nullptr; +nav2_behavior_tree::DistanceController * DistanceControllerTestFixture::node_ = nullptr; +nav2_behavior_tree::DummyNode * DistanceControllerTestFixture::dummy_node_ = nullptr; + +TEST_F(DistanceControllerTestFixture, test_behavior) +{ + EXPECT_EQ(node_->status(), BT::NodeStatus::IDLE); + + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = 0; + pose.pose.position.y = 0; + pose.pose.orientation.w = 1; + + double traveled = 0; + for (int i = 1; i <= 20; i++) { + pose.pose.position.x = i * 0.51; + transform_handler_->updateRobotPose(pose.pose); + + // Wait for transforms to actually update + // updated pose is i * 0.55 + // we wait fot the traveled distance to reach a value > i * 0.5 + // we can assume the current transform has been updated at this point + while (traveled < i * 0.5) { + if (nav2_util::getCurrentPose(pose, *transform_handler_->getBuffer())) { + traveled = std::sqrt( + pose.pose.position.x * pose.pose.position.x + + pose.pose.position.y * pose.pose.position.y); + } + } + + dummy_node_->changeStatus(BT::NodeStatus::SUCCESS); + + if (i % 2) { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::RUNNING); + } else { + EXPECT_EQ(node_->executeTick(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(dummy_node_->status(), BT::NodeStatus::IDLE); + } + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behavior_tree/test/test_dummy_tree_node.hpp b/nav2_behavior_tree/test/test_dummy_tree_node.hpp new file mode 100644 index 0000000000..1f57454dd1 --- /dev/null +++ b/nav2_behavior_tree/test/test_dummy_tree_node.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_DUMMY_TREE_NODE_HPP_ +#define TEST_DUMMY_TREE_NODE_HPP_ + +#include +#include + +namespace nav2_behavior_tree +{ + +/** + * @brief A Dummy TreeNode to be used as a child for testing nodes + * Returns the current status on tick without any execution logic + */ +class DummyNode : public BT::ActionNodeBase +{ +public: + DummyNode() + : BT::ActionNodeBase("dummy", {}) + { + } + + void changeStatus(BT::NodeStatus status) + { + setStatus(status); + } + + BT::NodeStatus executeTick() override + { + return tick(); + } + + BT::NodeStatus tick() override + { + return status(); + } + + void halt() override + { + } +}; + +} // namespace nav2_behavior_tree + +#endif // TEST_DUMMY_TREE_NODE_HPP_ diff --git a/nav2_behavior_tree/test/test_transform_handler.hpp b/nav2_behavior_tree/test/test_transform_handler.hpp new file mode 100644 index 0000000000..1caea66fde --- /dev/null +++ b/nav2_behavior_tree/test/test_transform_handler.hpp @@ -0,0 +1,163 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_TRANSFORM_HANDLER_HPP_ +#define TEST_TRANSFORM_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +using namespace std::chrono_literals; // NOLINT +using namespace std::chrono; // NOLINT + +namespace nav2_behavior_tree +{ +class TransformHandler : public rclcpp::Node +{ +public: + TransformHandler() + : Node("TransformHandler"), + is_active_(false), + base_transform_(nullptr), + tf_broadcaster_(nullptr) + { + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + } + + ~TransformHandler() + { + if (is_active_) { + deactivate(); + } + } + + // Activate the tester before running tests + void activate() + { + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + } + is_active_ = true; + + // Launch a thread to process the messages for this node + spin_thread_ = std::make_unique(this); + + startRobotTransform(); + } + + void deactivate() + { + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; + spin_thread_.reset(); + tf_broadcaster_.reset(); + tf_buffer_.reset(); + tf_listener_.reset(); + } + + std::shared_ptr getBuffer() + { + return tf_buffer_; + } + + void waitForTransform() + { + if (is_active_) { + while (!tf_buffer_->canTransform("map", "base_link", rclcpp::Time(0))) { + std::this_thread::sleep_for(100ms); + } + RCLCPP_INFO(get_logger(), "Transforms are available now!"); + return; + } + throw std::runtime_error("Trying to deactivate while already inactive"); + } + + void updateRobotPose(const geometry_msgs::msg::Pose & pose) + { + // Update base transform to publish + base_transform_->transform.translation.x = pose.position.x; + base_transform_->transform.translation.y = pose.position.y; + base_transform_->transform.translation.z = pose.position.z; + base_transform_->transform.rotation.x = pose.orientation.x; + base_transform_->transform.rotation.y = pose.orientation.y; + base_transform_->transform.rotation.z = pose.orientation.z; + base_transform_->transform.rotation.w = pose.orientation.w; + publishRobotTransform(); + } + +private: + void publishRobotTransform() + { + base_transform_->header.stamp = now(); + tf_broadcaster_->sendTransform(*base_transform_); + } + + void startRobotTransform() + { + // Provide the robot pose transform + tf_broadcaster_ = std::make_shared(this); + + if (!base_transform_) { + base_transform_ = std::make_unique(); + base_transform_->header.frame_id = "map"; + base_transform_->child_frame_id = "base_link"; + } + + // Set an initial pose + geometry_msgs::msg::Pose robot_pose; + robot_pose.position.x = 0; + robot_pose.position.y = 0; + robot_pose.orientation.w = 1; + updateRobotPose(robot_pose); + + // Publish the transform periodically + transform_timer_ = create_wall_timer( + 100ms, std::bind(&TransformHandler::publishRobotTransform, this)); + } + + bool is_active_; + + // A thread for spinning the ROS node + std::unique_ptr spin_thread_; + + // Subscriber + + // The tester must provide the robot pose through a transform + std::unique_ptr base_transform_; + std::shared_ptr tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + rclcpp::TimerBase::SharedPtr transform_timer_; +}; + +} // namespace nav2_behavior_tree + +#endif // TEST_TRANSFORM_HANDLER_HPP_ diff --git a/nav2_bringup/bringup/launch/bringup_launch.py b/nav2_bringup/bringup/launch/bringup_launch.py index 467b902d7e..9904b5bc7f 100644 --- a/nav2_bringup/bringup/launch/bringup_launch.py +++ b/nav2_bringup/bringup/launch/bringup_launch.py @@ -21,7 +21,7 @@ IncludeLaunchDescription, SetEnvironmentVariable) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration +from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import PushRosNamespace @@ -33,6 +33,7 @@ def generate_launch_description(): # Create the launch configuration variables namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') + slam = LaunchConfiguration('slam') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') @@ -52,6 +53,11 @@ def generate_launch_description(): default_value='false', description='Whether to apply a namespace to the navigation stack') + declare_slam_cmd = DeclareLaunchArgument( + 'slam', + default_value='False', + description='Whether run a SLAM') + declare_map_yaml_cmd = DeclareLaunchArgument( 'map', description='Full path to map yaml file to load') @@ -83,9 +89,18 @@ def generate_launch_description(): condition=IfCondition(use_namespace), namespace=namespace), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), + condition=IfCondition(slam), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file}.items()), + IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'localization_launch.py')), + condition=IfCondition(PythonExpression(['not ', slam])), launch_arguments={'namespace': namespace, 'map': map_yaml_file, 'use_sim_time': use_sim_time, @@ -113,6 +128,7 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) diff --git a/nav2_bringup/bringup/launch/localization_launch.py b/nav2_bringup/bringup/launch/localization_launch.py index b5b825d4df..2791e0068c 100644 --- a/nav2_bringup/bringup/launch/localization_launch.py +++ b/nav2_bringup/bringup/launch/localization_launch.py @@ -82,24 +82,24 @@ def generate_launch_description(): Node( package='nav2_map_server', - node_executable='map_server', - node_name='map_server', + executable='map_server', + name='map_server', output='screen', parameters=[configured_params], remappings=remappings), Node( package='nav2_amcl', - node_executable='amcl', - node_name='amcl', + executable='amcl', + name='amcl', output='screen', parameters=[configured_params], remappings=remappings), Node( package='nav2_lifecycle_manager', - node_executable='lifecycle_manager', - node_name='lifecycle_manager_localization', + executable='lifecycle_manager', + name='lifecycle_manager_localization', output='screen', parameters=[{'use_sim_time': use_sim_time}, {'autostart': autostart}, diff --git a/nav2_bringup/bringup/launch/navigation_launch.py b/nav2_bringup/bringup/launch/navigation_launch.py index 4105586a04..ba43546a67 100644 --- a/nav2_bringup/bringup/launch/navigation_launch.py +++ b/nav2_bringup/bringup/launch/navigation_launch.py @@ -96,47 +96,47 @@ def generate_launch_description(): Node( package='nav2_controller', - node_executable='controller_server', + executable='controller_server', output='screen', parameters=[configured_params], remappings=remappings), Node( package='nav2_planner', - node_executable='planner_server', - node_name='planner_server', + executable='planner_server', + name='planner_server', output='screen', parameters=[configured_params], remappings=remappings), Node( package='nav2_recoveries', - node_executable='recoveries_server', - node_name='recoveries_server', + executable='recoveries_server', + name='recoveries_server', output='screen', - parameters=[{'use_sim_time': use_sim_time}], + parameters=[configured_params], remappings=remappings), Node( package='nav2_bt_navigator', - node_executable='bt_navigator', - node_name='bt_navigator', + executable='bt_navigator', + name='bt_navigator', output='screen', parameters=[configured_params], remappings=remappings), Node( package='nav2_waypoint_follower', - node_executable='waypoint_follower', - node_name='waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', output='screen', parameters=[configured_params], remappings=remappings), Node( package='nav2_lifecycle_manager', - node_executable='lifecycle_manager', - node_name='lifecycle_manager_navigation', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', output='screen', parameters=[{'use_sim_time': use_sim_time}, {'autostart': autostart}, diff --git a/nav2_bringup/bringup/launch/rviz_launch.py b/nav2_bringup/bringup/launch/rviz_launch.py index 67ab0c3ff2..3126b11348 100644 --- a/nav2_bringup/bringup/launch/rviz_launch.py +++ b/nav2_bringup/bringup/launch/rviz_launch.py @@ -56,8 +56,8 @@ def generate_launch_description(): start_rviz_cmd = Node( condition=UnlessCondition(use_namespace), package='rviz2', - node_executable='rviz2', - node_name='rviz2', + executable='rviz2', + name='rviz2', arguments=['-d', rviz_config_file], output='screen') @@ -68,9 +68,9 @@ def generate_launch_description(): start_namespaced_rviz_cmd = Node( condition=IfCondition(use_namespace), package='rviz2', - node_executable='rviz2', - node_name='rviz2', - node_namespace=namespace, + executable='rviz2', + name='rviz2', + namespace=namespace, arguments=['-d', namespaced_rviz_config_file], output='screen', remappings=[('/tf', 'tf'), diff --git a/nav2_bringup/bringup/launch/slam_launch.py b/nav2_bringup/bringup/launch/slam_launch.py new file mode 100644 index 0000000000..a32f68094b --- /dev/null +++ b/nav2_bringup/bringup/launch/slam_launch.py @@ -0,0 +1,107 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + namespace = LaunchConfiguration('namespace') + params_file = LaunchConfiguration('params_file') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + + # Variables + lifecycle_nodes = ['map_saver'] + + # Getting directories and launch-files + bringup_dir = get_package_share_directory('nav2_bringup') + slam_toolbox_dir = get_package_share_directory('slam_toolbox') + slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py') + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation (Gazebo) clock if true') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + # Nodes launching commands + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time}.items()) + + start_map_saver_server_cmd = Node( + package='nav2_map_server', + node_executable='map_saver_server', + output='screen', + parameters=[configured_params]) + + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + node_executable='lifecycle_manager', + node_name='lifecycle_manager_slam', + output='screen', + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + + # Running SLAM Toolbox + ld.add_action(start_slam_toolbox_cmd) + + # Running Map Saver Server + ld.add_action(start_map_saver_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + + return ld diff --git a/nav2_bringup/bringup/launch/spawn_tb3_launch.py b/nav2_bringup/bringup/launch/spawn_tb3_launch.py index 08cab9c788..af0d100372 100644 --- a/nav2_bringup/bringup/launch/spawn_tb3_launch.py +++ b/nav2_bringup/bringup/launch/spawn_tb3_launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): # TODO(orduno) might not be necessary to have it's own package launch_ros.actions.Node( package='nav2_gazebo_spawner', - node_executable='nav2_gazebo_spawner', + executable='nav2_gazebo_spawner', output='screen', arguments=[ '--robot_name', launch.substitutions.LaunchConfiguration('robot_name'), diff --git a/nav2_bringup/bringup/launch/tb3_simulation_launch.py b/nav2_bringup/bringup/launch/tb3_simulation_launch.py index 990b87630c..f3fe02b7e7 100644 --- a/nav2_bringup/bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/bringup/launch/tb3_simulation_launch.py @@ -32,6 +32,7 @@ def generate_launch_description(): launch_dir = os.path.join(bringup_dir, 'launch') # Create the launch configuration variables + slam = LaunchConfiguration('slam') namespace = LaunchConfiguration('namespace') use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') @@ -68,6 +69,11 @@ def generate_launch_description(): default_value='false', description='Whether to apply a namespace to the navigation stack') + declare_slam_cmd = DeclareLaunchArgument( + 'slam', + default_value='False', + description='Whether run a SLAM') + declare_map_yaml_cmd = DeclareLaunchArgument( 'map', default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), @@ -144,9 +150,9 @@ def generate_launch_description(): start_robot_state_publisher_cmd = Node( condition=IfCondition(use_robot_state_pub), package='robot_state_publisher', - node_executable='robot_state_publisher', - node_name='robot_state_publisher', - node_namespace=namespace, + executable='robot_state_publisher', + name='robot_state_publisher', + namespace=namespace, output='screen', parameters=[{'use_sim_time': use_sim_time}], remappings=remappings, @@ -163,6 +169,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={'namespace': namespace, 'use_namespace': use_namespace, + 'slam': slam, 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'params_file': params_file, @@ -175,6 +182,7 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml index c639695493..42e322f31d 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml @@ -49,6 +49,8 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True + global_frame: map + robot_base_frame: base_link bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -59,12 +61,16 @@ bt_navigator: - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -188,7 +194,7 @@ planner_server: planner_plugin_types: ["nav2_navfn_planner/NavfnPlanner"] planner_plugin_ids: ["GridBased"] use_sim_time: True - GridBased.tolerance: 2.0 + GridBased.tolerance: 0.5 GridBased.use_astar: false GridBased.allow_unknown: true @@ -196,6 +202,22 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + plugin_names: ["spin", "back_up", "wait"] + plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"] + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + robot_state_publisher: ros__parameters: use_sim_time: True diff --git a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml index 54b334dff5..eb539522ef 100644 --- a/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml @@ -49,6 +49,8 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True + global_frame: map + robot_base_frame: base_link bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -59,12 +61,16 @@ bt_navigator: - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -188,7 +194,7 @@ planner_server: planner_plugin_types: ["nav2_navfn_planner/NavfnPlanner"] planner_plugin_ids: ["GridBased"] use_sim_time: True - GridBased.tolerance: 2.0 + GridBased.tolerance: 0.5 GridBased.use_astar: false GridBased.allow_unknown: true @@ -196,6 +202,22 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + plugin_names: ["spin", "back_up", "wait"] + plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"] + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + robot_state_publisher: ros__parameters: use_sim_time: True diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index d125850c8f..1e08489296 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -49,6 +49,8 @@ amcl_rclcpp_node: bt_navigator: ros__parameters: use_sim_time: True + global_frame: map + robot_base_frame: base_link bt_xml_filename: "navigate_w_replanning_and_recovery.xml" plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -59,13 +61,17 @@ bt_navigator: - nav2_clear_costmap_service_bt_node - nav2_is_stuck_condition_bt_node - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node - nav2_initial_pose_received_condition_bt_node - nav2_reinitialize_global_localization_service_bt_node - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node - nav2_recovery_node_bt_node - nav2_pipeline_sequence_bt_node - nav2_round_robin_node_bt_node - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_distance_traveled_condition_bt_node bt_navigator_rclcpp_node: ros__parameters: @@ -112,8 +118,10 @@ controller_server: FollowPath.stateful: True FollowPath.critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] FollowPath.BaseObstacle.scale: 0.02 - FollowPath.PathAlign.scale: 0.0 - FollowPath.GoalAlign.scale: 0.0 + FollowPath.PathAlign.scale: 32.0 + FollowPath.GoalAlign.scale: 24.0 + FollowPath.PathAlign.forward_point_distance: 0.1 + FollowPath.GoalAlign.forward_point_distance: 0.1 FollowPath.PathDist.scale: 32.0 FollowPath.GoalDist.scale: 24.0 FollowPath.RotateToGoal.scale: 32.0 @@ -129,9 +137,9 @@ local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 + global_frame: odom robot_base_frame: base_link use_sim_time: True - global_frame: odom rolling_window: true width: 3 height: 3 @@ -181,8 +189,8 @@ global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 - robot_base_frame: base_link global_frame: map + robot_base_frame: base_link use_sim_time: True plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] @@ -230,6 +238,13 @@ map_server: use_sim_time: True yaml_filename: "turtlebot3_world.yaml" +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5000 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + planner_server: ros__parameters: planner_plugin_types: ["smac_planner/SmacPlanner"] #nav2_navfn_planner/NavfnPlanner #smac_planner/SmacPlanner @@ -281,6 +296,22 @@ planner_server_rclcpp_node: ros__parameters: use_sim_time: True +recoveries_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + plugin_names: ["spin", "backup", "wait"] + plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"] + global_frame: odom + robot_base_frame: base_link + transform_timeout: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + robot_state_publisher: ros__parameters: use_sim_time: True diff --git a/nav2_bringup/bringup/worlds/waffle.model b/nav2_bringup/bringup/worlds/waffle.model index 6426b9a8e3..74af1c7670 100644 --- a/nav2_bringup/bringup/worlds/waffle.model +++ b/nav2_bringup/bringup/worlds/waffle.model @@ -413,7 +413,7 @@ 0.035 - 0 0.047 -0.004 0 0 0 + 0 0.047 -0.005 0 0 0 0.008 0.130 0.022 @@ -441,7 +441,7 @@ - 0 0.047 -0.004 0 0 0 + 0 0.047 -0.005 0 0 0 0.008 0.130 0.022 diff --git a/nav2_bringup/nav2_gazebo_spawner/setup.cfg b/nav2_bringup/nav2_gazebo_spawner/setup.cfg index 598b7c2d6d..3edf46fbb6 100644 --- a/nav2_bringup/nav2_gazebo_spawner/setup.cfg +++ b/nav2_bringup/nav2_gazebo_spawner/setup.cfg @@ -2,3 +2,5 @@ script-dir=$base/lib/nav2_gazebo_spawner [install] install-scripts=$base/lib/nav2_gazebo_spawner +[tool:pytest] +junit_family=xunit2 diff --git a/nav2_bt_navigator/CMakeLists.txt b/nav2_bt_navigator/CMakeLists.txt index 3f56c116a1..da362c9106 100644 --- a/nav2_bt_navigator/CMakeLists.txt +++ b/nav2_bt_navigator/CMakeLists.txt @@ -60,9 +60,13 @@ ament_target_dependencies(${library_name} ${dependencies} ) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index b4ea4c516b..23fe3bf8db 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -24,30 +24,23 @@ A Behavior Tree consists of control flow nodes, such as fallback, sequence, para ## Navigation Behavior Trees -The BT Navigator package has two sample XML-based descriptions of BTs. These trees are [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml). The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs. - -### Navigate with Replanning - -[navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) implements basic navigation by continuously computing and updating the path at a rate of 1Hz. The default controller, the nav2_dwb_controller, implements path following at a rate of 10Hz. - -```XML - - - - - - - - - - - - - -``` +The BT Navigator package has three sample XML-based descriptions of BTs. +These trees are [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml), [navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) and [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml). +The user may use any of these sample trees or develop a more complex tree which could better suit the user's needs. + +### Navigate with Replanning (time-based) + +[navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) implements basic navigation by continuously computing and updating the path at a rate of 1Hz. The default controller, the nav2_dwb_controller, implements path following at a rate of 10Hz. + +![Navigation with time based replanning](./doc/navigate_w_replanning_time.png) -Navigate with replanning is composed of the following custom decorator, condition, -contro and action nodes: +### Navigate with Replanning (distace-based) + +[navigate_w_replanning_distance.xml](behavior_trees/navigate_w_replanning_distance.xml) implements basic navigation by continuously computing and updating the path after every 1 meter distance traveled by the robot. + +![Navigation with distance based replanning](./doc/navigate_w_replanning_distance.png) + +#### Navigate with replanning is composed of the following custom decorator, condition, control and action nodes: #### Control Nodes * PipelineSequence: This is a variant of a Sequence Node. When this node is ticked, @@ -60,13 +53,17 @@ returns FAILURE, all nodes are halted and this node returns FAILURE. * Recovery: This is a control flow type node with two children. It returns success if and only if the first child returns success. The second child will be executed only if the first child returns failure. The second child is responsible for recovery actions such as re-initializing system or other recovery behaviors. If the recovery behaviors are succeeded, then the first child will be executed again. The user can specify how many times the recovery actions should be taken before returning failure. The figure below depicts a simple recovery node. - +

+ +


#### Decorator Nodes * RateController: A custom control flow node, which throttles down the tick rate. This custom node has only one child and its tick rate is defined with a pre-defined frequency that the user can set. This node returns RUNNING when it is not ticking its child. Currently, in the navigation, the `RateController` is used to tick the `ComputePathToPose` and `GoalReached` node at 1 Hz. +* DistanceController: A custom control flow node, which controls the tick rate based on the distance traveled. This custom node has only one child. The user can set the distance after which the planner should replan a new path. This node returns RUNNING when it is not ticking its child. Currently, in navigation, the `DistanceController` is used to tick the `ComputePathToPose` and `GoalReached` node after every 0.5 meters. + #### Condition Nodes * GoalReached: Checks the distance to the goal, if the distance to goal is less than the pre-defined threshold, the tree returns SUCCESS, which in that case the `ComputePathToPose` action node will not get ticked. @@ -75,32 +72,52 @@ returns FAILURE, all nodes are halted and this node returns FAILURE. The graphical version of this Behavior Tree: - +

+ +


The navigate with replanning BT first ticks the `RateController` node which specifies how frequently the `GoalReached` and `ComputePathToPose` should be invoked. Then the `GoalReached` nodes check the distance to the goal to determine if the `ComputePathToPose` should be ticked or not. The `ComputePathToPose` gets the incoming goal pose from the blackboard, computes the path and puts the result back on the blackboard, where `FollowPath` picks it up. Each time a new path is computed, the blackboard gets updated and then `FollowPath` picks up the new goal to compute a control effort for. `controller_id` will specify the type of control effort you'd like to compute such as `FollowPath` `FollowPathSlow` `FollowPathExactly`, etc. ### Navigate with replanning and simple recovery actions -With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning.xml](behavior_trees/navigate_w_replanning.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps and `spin`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below. +With the recovery node, simple recoverable navigation with replanning can be implemented by utilizing the [navigate_w_replanning_time.xml](behavior_trees/navigate_w_replanning_time.xml) and a sequence of recovery actions. Our custom behavior actions for recovery are: `clearEntirelyCostmapServiceRequest` for both global and local costmaps, `spin` and `wait`. A graphical version of this simple recoverable Behavior Tree is depicted in the figure below. - +

+ +


This tree is currently our default tree in the stack and the xml file is located here: [navigate_w_replanning_and_recovery.xml](behavior_trees/navigate_w_replanning_and_recovery.xml). -## Multi-Scope Recoveries + +#### Halting recoveries on navigation goal (Preemption reference design) +In general, the recovery behaviours and any other long running process should be stopped when the navigation goal is issued (e.g. preemption). In the default tree in the stack, this behaviour is accomplished using a condition node checking the global navigation goal and a reactive fallback controller node: + +

+ +

+ +This way, the recovery actions can be interrupted if a new goal is sent to the bt_navigator. Adding other condition nodes to this structure, it is possible to halt the recoveries in other cases (e.g, giving a time limit for their execution). This is the recommended design pattern for preempting a node or tree branch under specific conditions such as a new navigation request. Please notice that the order of definition of the nodes in the xml file can alter the behaviour of this structure, and that all conditions should be placed before the recovery behaviour node or branch. + + +#### Multi-Scope Recoveries Scope-based failure handling: Utilizing Behavior Trees with a recovery node allows one to handle failures at multiple scopes. With this capability, any action in a large system can be constructed with specific recovery actions suitable for that action. Thus, failures in these actions can be handled locally within the scope. With such design, a system can be recovered at multiple levels based on the nature of the failure. Higher level recovery actions could be recovery actions such as re-initializing the system, re-calibrating the robot, bringing the system to a good known state, etc. ### Navigate with replanning and simple Multi-Scope Recoveries In the navigation stack, multi-scope recovery actions are implemented for each module. Currently, the recovery actions for the Global planner are: Clear Entire Global Costmap and Wait. The recovery actions for the Local planner are: Clear Entire Local Costmap and Spin; the recovery actions for the system level is just Wait. The figure below highlights a simple multi-scope recovery handling for the navigation task. With this tree, if the Global Planner fails, the ClearEntireCostmap which is the first recovery action for this module will be ticked, then the ComputePathToPose will be ticked again. If the ComputePathToPose fails again, the Wait which is the second recovery action for this module will be ticked. After trying the second recovery action, the ComputePathToPose will be ticked again. These actions can be repeated n times until ComputePathToPose succeeds. If the ComputePathToPose fails and the Global Planner cannot be recovered locally, the higher-level recovery actions will be ticked. In this simple example, our higher-level recovery action is just a longer wait. The same strategy is applied to the Local Planner. If the Local Planner fails, the tree will first tick the ClearEntireCostmap and then if it fails again the tree will tick the Spin. -
+

+ +

+ This tree currently is not our default tree in the stack. The xml file is located here: [navigate_w_replanning_and_round_robin_recovery.xml](behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml). +
+ ## Open Issues * **Schema definition and XML document validation** - Currently, there is no dynamic validation of incoming XML. The Behavior-Tree.CPP library is using tinyxml2, which doesn't have a validator. Instead, we can create a schema for the Mission Planning-level XML and use build-time validation of the XML input to ensure that it is well-formed and valid. diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml index d4fcc6b609..9b1311a391 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_recovery.xml @@ -18,12 +18,15 @@ - - - - - - + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml index beb246fbe9..746ca53d8d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_and_round_robin_recovery.xml @@ -10,21 +10,30 @@ - - - - + + + + + + + - - - - + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml new file mode 100644 index 0000000000..8ca5dbbc5a --- /dev/null +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml similarity index 100% rename from nav2_bt_navigator/behavior_trees/navigate_w_replanning.xml rename to nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml diff --git a/nav2_bt_navigator/doc/navigate_w_replanning_distance.png b/nav2_bt_navigator/doc/navigate_w_replanning_distance.png new file mode 100644 index 0000000000..396ebc0b2a Binary files /dev/null and b/nav2_bt_navigator/doc/navigate_w_replanning_distance.png differ diff --git a/nav2_bt_navigator/doc/simple_parallel.png b/nav2_bt_navigator/doc/navigate_w_replanning_time.png similarity index 100% rename from nav2_bt_navigator/doc/simple_parallel.png rename to nav2_bt_navigator/doc/navigate_w_replanning_time.png diff --git a/nav2_bt_navigator/doc/parallel_w_recovery.png b/nav2_bt_navigator/doc/parallel_w_recovery.png index 3201ff3403..ac1022a599 100644 Binary files a/nav2_bt_navigator/doc/parallel_w_recovery.png and b/nav2_bt_navigator/doc/parallel_w_recovery.png differ diff --git a/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png b/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png index c2a842e532..c0f67c7d5e 100644 Binary files a/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png and b/nav2_bt_navigator/doc/parallel_w_round_robin_recovery.png differ diff --git a/nav2_bt_navigator/doc/recovery_w_goal_updated.png b/nav2_bt_navigator/doc/recovery_w_goal_updated.png new file mode 100644 index 0000000000..0cfca4de34 Binary files /dev/null and b/nav2_bt_navigator/doc/recovery_w_goal_updated.png differ diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp index 0ac93a922b..0c2f7c1890 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp @@ -88,7 +88,9 @@ class BtNavigator : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; - using ActionServer = nav2_util::SimpleActionServer; + using Action = nav2_msgs::action::NavigateToPose; + + using ActionServer = nav2_util::SimpleActionServer; // Our action server implements the NavigateToPose action std::unique_ptr action_server_; @@ -110,6 +112,8 @@ class BtNavigator : public nav2_util::LifecycleNode void onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose); rclcpp::Subscription::SharedPtr goal_sub_; + BT::Tree tree_; + // The blackboard shared by all of the nodes in the tree BT::Blackboard::Ptr blackboard_; @@ -123,7 +127,7 @@ class BtNavigator : public nav2_util::LifecycleNode std::vector plugin_lib_names_; // A client that we'll use to send a command message to our own task server - rclcpp_action::Client::SharedPtr self_client_; + rclcpp_action::Client::SharedPtr self_client_; // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; @@ -131,6 +135,12 @@ class BtNavigator : public nav2_util::LifecycleNode // Spinning transform that can be used by the BT nodes std::shared_ptr tf_; std::shared_ptr tf_listener_; + + // Metrics for feedback + rclcpp::Time start_time_; + std::string robot_frame_; + std::string global_frame_; + double transform_tolerance_; }; } // namespace nav2_bt_navigator diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index dc478cb61a..2cc2d768cb 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -20,7 +20,10 @@ #include #include #include +#include +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_behavior_tree/bt_conversions.hpp" #include "nav2_bt_navigator/ros_topic_logger.hpp" @@ -28,7 +31,8 @@ namespace nav2_bt_navigator { BtNavigator::BtNavigator() -: nav2_util::LifecycleNode("bt_navigator", "", true) +: nav2_util::LifecycleNode("bt_navigator", "", false), + start_time_(0) { RCLCPP_INFO(get_logger(), "Creating"); @@ -42,17 +46,24 @@ BtNavigator::BtNavigator() "nav2_is_stuck_condition_bt_node", "nav2_goal_reached_condition_bt_node", "nav2_initial_pose_received_condition_bt_node", + "nav2_goal_updated_condition_bt_node", "nav2_reinitialize_global_localization_service_bt_node", "nav2_rate_controller_bt_node", + "nav2_distance_controller_bt_node", "nav2_recovery_node_bt_node", "nav2_pipeline_sequence_bt_node", "nav2_round_robin_node_bt_node", - "nav2_transform_available_condition_bt_node" + "nav2_transform_available_condition_bt_node", + "nav2_time_expired_condition_bt_node", + "nav2_distance_traveled_condition_bt_node" }; // Declare this node's parameters declare_parameter("bt_xml_filename"); declare_parameter("plugin_lib_names", plugin_libs); + declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter("global_frame", std::string("map")); + declare_parameter("robot_base_frame", std::string("base_link")); } BtNavigator::~BtNavigator() @@ -65,9 +76,10 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); + // use suffix '_rclcpp_node' to keep parameter file consistency #1773 auto options = rclcpp::NodeOptions().arguments( {"--ros-args", - "-r", std::string("__node:=") + get_name() + "_client_node", + "-r", std::string("__node:=") + get_name() + "_rclcpp_node", "--"}); // Support for handling the topic-based goal pose from rviz client_node_ = std::make_shared("_", options); @@ -96,6 +108,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) // Get the libraries to pull plugins from plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array(); + global_frame_ = get_parameter("global_frame").as_string(); + robot_frame_ = get_parameter("robot_base_frame").as_string(); + transform_tolerance_ = get_parameter("transform_tolerance").as_double(); // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -109,6 +124,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) blackboard_->set("server_timeout", std::chrono::milliseconds(10)); // NOLINT blackboard_->set("path_updated", false); // NOLINT blackboard_->set("initial_pose_received", false); // NOLINT + blackboard_->set("number_recoveries", 0); // NOLINT // Get the BT filename to use from the node parameter std::string bt_xml_filename; @@ -129,6 +145,9 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_DEBUG(get_logger(), "Behavior Tree file: '%s'", bt_xml_filename.c_str()); RCLCPP_DEBUG(get_logger(), "Behavior Tree XML: %s", xml_string_.c_str()); + // Create the Behavior Tree from the XML input + tree_ = bt_->buildTreeFromText(xml_string_, blackboard_); + return nav2_util::CallbackReturn::SUCCESS; } @@ -159,7 +178,6 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // TODO(orduno) Fix the race condition between the worker thread ticking the tree // and the main thread resetting the resources, see #1344 - goal_sub_.reset(); client_node_.reset(); self_client_.reset(); @@ -172,6 +190,7 @@ BtNavigator::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plugin_lib_names_.clear(); xml_string_.clear(); blackboard_.reset(); + bt_->haltAllActions(tree_.rootNode()); bt_.reset(); RCLCPP_INFO(get_logger(), "Completed Cleaning up"); @@ -211,11 +230,8 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; - - // Create the Behavior Tree from the XML input - BT::Tree tree = bt_->buildTreeFromText(xml_string_, blackboard_); - - RosTopicLogger topic_logger(client_node_, tree); + RosTopicLogger topic_logger(client_node_, tree_); + std::shared_ptr feedback_msg = std::make_shared(); auto on_loop = [&]() { if (action_server_->is_preempt_requested()) { @@ -224,10 +240,30 @@ BtNavigator::navigateToPose() initializeGoalPose(); } topic_logger.flush(); + + // action server feedback (pose, duration of task, + // number of recoveries, and distance remaining to goal) + nav2_util::getCurrentPose( + feedback_msg->current_pose, *tf_, global_frame_, robot_frame_, transform_tolerance_); + + geometry_msgs::msg::PoseStamped goal_pose; + blackboard_->get("goal", goal_pose); + + feedback_msg->distance_remaining = nav2_util::geometry_utils::euclidean_distance( + feedback_msg->current_pose.pose, goal_pose.pose); + + int recovery_count = 0; + blackboard_->get("number_recoveries", recovery_count); + feedback_msg->number_of_recoveries = recovery_count; + feedback_msg->navigation_time = now() - start_time_; + action_server_->publish_feedback(feedback_msg); }; // Execute the BT that was previously created in the configure step - nav2_behavior_tree::BtStatus rc = bt_->run(&tree, on_loop, is_canceling); + nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); + // Make sure that the Bt is not in a running state from a previous execution + // note: if all the ControlNodes are implemented correctly, this is not needed. + bt_->haltAllActions(tree_.rootNode()); switch (rc) { case nav2_behavior_tree::BtStatus::SUCCEEDED: @@ -259,6 +295,10 @@ BtNavigator::initializeGoalPose() get_logger(), "Begin navigating from current location to (%.2f, %.2f)", goal->pose.pose.position.x, goal->pose.pose.position.y); + // Reset state for new action feedback + start_time_ = now(); + blackboard_->set("number_recoveries", 0); // NOLINT + // Update the goal pose on the blackboard blackboard_->set("goal", goal->pose); } diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index e5c0c39e0b..472b30d323 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "nav2_bt_navigator/ros_topic_logger.hpp" #include "tf2_ros/buffer_interface.h" @@ -21,7 +22,7 @@ namespace nav2_bt_navigator RosTopicLogger::RosTopicLogger( const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree) -: StatusChangeLogger(tree.root_node), ros_node_(ros_node) +: StatusChangeLogger(tree.rootNode()), ros_node_(ros_node) { log_pub_ = ros_node_->create_publisher( "behavior_tree_log", @@ -38,8 +39,12 @@ void RosTopicLogger::callback( // BT timestamps are a duration since the epoch. Need to convert to a time_point // before converting to a msg. +#ifndef _WIN32 event.timestamp = tf2_ros::toMsg(std::chrono::time_point(timestamp)); +#else + event.timestamp = tf2_ros::toMsg(timestamp); +#endif event.node_name = node.name(); event.previous_status = toStr(prev_status, false); event.current_status = toStr(status, false); @@ -56,10 +61,10 @@ void RosTopicLogger::callback( void RosTopicLogger::flush() { if (event_log_.size() > 0) { - nav2_msgs::msg::BehaviorTreeLog log_msg; - log_msg.timestamp = ros_node_->now(); - log_msg.event_log = event_log_; - log_pub_->publish(log_msg); + auto log_msg = std::make_unique(); + log_msg->timestamp = ros_node_->now(); + log_msg->event_log = event_log_; + log_pub_->publish(std::move(log_msg)); event_log_.clear(); } } diff --git a/nav2_common/cmake/nav2_package.cmake b/nav2_common/cmake/nav2_package.cmake index 61dd187cfe..ee8555f799 100644 --- a/nav2_common/cmake/nav2_package.cmake +++ b/nav2_common/cmake/nav2_package.cmake @@ -42,4 +42,16 @@ macro(nav2_package) set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --coverage") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} --coverage") endif() + + # Defaults for Microsoft C++ compiler + if(MSVC) + # https://blog.kitware.com/create-dlls-on-windows-without-declspec-using-new-cmake-export-all-feature/ + set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + + # Enable Math Constants + # https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019 + add_compile_definitions( + _USE_MATH_DEFINES + ) + endif() endmacro() diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt index 90dc9fd172..b4cb1b50d0 100644 --- a/nav2_controller/CMakeLists.txt +++ b/nav2_controller/CMakeLists.txt @@ -59,9 +59,13 @@ ament_target_dependencies(${executable_name} target_link_libraries(${executable_name} ${library_name}) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_controller/include/nav2_controller/nav2_controller.hpp b/nav2_controller/include/nav2_controller/nav2_controller.hpp index 3187655d56..d5e1b0366d 100644 --- a/nav2_controller/include/nav2_controller/nav2_controller.hpp +++ b/nav2_controller/include/nav2_controller/nav2_controller.hpp @@ -107,7 +107,8 @@ class ControllerServer : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; - using ActionServer = nav2_util::SimpleActionServer; + using Action = nav2_msgs::action::FollowPath; + using ActionServer = nav2_util::SimpleActionServer; // Our action server implements the FollowPath action std::unique_ptr action_server_; @@ -215,6 +216,7 @@ class ControllerServer : public nav2_util::LifecycleNode // Whether we've published the single controller warning yet bool single_controller_warning_given_{false}; + geometry_msgs::msg::Pose end_pose_; }; } // namespace nav2_controller diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index bf9e5c9a24..ee9a162547 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -16,10 +16,12 @@ #include #include #include +#include #include "nav2_core/exceptions.hpp" #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" #include "nav2_controller/progress_checker.hpp" #include "nav2_controller/nav2_controller.hpp" @@ -36,8 +38,8 @@ ControllerServer::ControllerServer() declare_parameter("controller_frequency", 20.0); std::vector default_id, default_type; - default_type.push_back("dwb_core::DWBLocalPlanner"); - default_id.push_back("FollowPath"); + default_type.emplace_back("dwb_core::DWBLocalPlanner"); + default_id.emplace_back("FollowPath"); declare_parameter("controller_plugin_ids", default_id); declare_parameter("controller_plugin_types", default_type); @@ -87,7 +89,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) exit(-1); } - for (uint i = 0; i != controller_types_.size(); i++) { + for (size_t i = 0; i != controller_types_.size(); i++) { try { nav2_core::Controller::Ptr controller = lp_loader_.createUniqueInstance(controller_types_[i]); @@ -104,7 +106,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state) } } - for (uint i = 0; i != controller_ids_.size(); i++) { + for (size_t i = 0; i != controller_ids_.size(); i++) { controller_ids_concat_ += controller_ids_[i] + std::string(" "); } @@ -291,7 +293,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) { RCLCPP_DEBUG( get_logger(), - "Providing path to the controller %s", current_controller_); + "Providing path to the controller %s", current_controller_.c_str()); if (path.poses.empty()) { throw nav2_core::PlannerException("Invalid path, Path is empty."); } @@ -302,6 +304,7 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path) RCLCPP_DEBUG( get_logger(), "Path end point is (%.2f, %.2f)", end_pose.pose.position.x, end_pose.pose.position.y); + end_pose_ = end_pose.pose; } void ControllerServer::computeAndPublishVelocity() @@ -321,6 +324,11 @@ void ControllerServer::computeAndPublishVelocity() pose, nav_2d_utils::twist2Dto3D(twist)); + std::shared_ptr feedback = std::make_shared(); + feedback->speed = std::hypot(cmd_vel_2d.twist.linear.x, cmd_vel_2d.twist.linear.y); + feedback->distance_to_goal = nav2_util::geometry_utils::euclidean_distance(end_pose_, pose.pose); + action_server_->publish_feedback(feedback); + RCLCPP_DEBUG(get_logger(), "Publishing velocity at time %.2f", now().seconds()); publishVelocity(cmd_vel_2d); } @@ -336,7 +344,7 @@ void ControllerServer::updateGlobalPath() } else { RCLCPP_INFO( get_logger(), "Terminating action, invalid controller %s requested.", - goal->controller_id); + goal->controller_id.c_str()); action_server_->terminate_current(); return; } @@ -346,8 +354,8 @@ void ControllerServer::updateGlobalPath() void ControllerServer::publishVelocity(const geometry_msgs::msg::TwistStamped & velocity) { - auto cmd_vel = velocity.twist; - vel_publisher_->publish(cmd_vel); + auto cmd_vel = std::make_unique(velocity.twist); + vel_publisher_->publish(std::move(cmd_vel)); } void ControllerServer::publishZeroVelocity() diff --git a/nav2_controller/src/progress_checker.cpp b/nav2_controller/src/progress_checker.cpp index def27aaca5..68fe96de50 100644 --- a/nav2_controller/src/progress_checker.cpp +++ b/nav2_controller/src/progress_checker.cpp @@ -33,7 +33,7 @@ ProgressChecker::ProgressChecker(const rclcpp_lifecycle::LifecycleNode::SharedPt nh_, "movement_time_allowance", rclcpp::ParameterValue(10.0)); // Scale is set to 0 by default, so if it was not set otherwise, set to 0 nh_->get_parameter_or("required_movement_radius", radius_, 0.5); - double time_allowance_param; + double time_allowance_param = 0.0; nh_->get_parameter_or("movement_time_allowance", time_allowance_param, 10.0); time_allowance_ = rclcpp::Duration::from_seconds(time_allowance_param); } diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index 1041d74d8e..5d3fa6de52 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/recovery.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" #include "tf2_ros/buffer.h" -#include "nav2_costmap_2d/collision_checker.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" namespace nav2_core { @@ -49,7 +49,7 @@ class Recovery virtual void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, const std::string & name, std::shared_ptr tf, - std::shared_ptr collision_checker) = 0; + std::shared_ptr collision_checker) = 0; /** * @brief Method to cleanup resources used on shutdown. diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index abd70ac461..d7508415ed 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -47,6 +47,7 @@ add_library(nav2_costmap_2d_core SHARED src/costmap_layer.cpp src/observation_buffer.cpp src/clear_costmap_service.cpp + src/footprint_collision_checker.cpp ) # prevent pluginlib from using boost @@ -94,7 +95,7 @@ target_link_libraries(layers add_library(nav2_costmap_2d_client SHARED src/footprint_subscriber.cpp src/costmap_subscriber.cpp - src/collision_checker.cpp + src/costmap_topic_collision_checker.cpp ) ament_target_dependencies(nav2_costmap_2d_client @@ -131,13 +132,17 @@ target_link_libraries(nav2_costmap_2d install(TARGETS nav2_costmap_2d_core - nav2_costmap_2d layers nav2_costmap_2d_client - nav2_costmap_2d_markers - nav2_costmap_2d_cloud ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS + nav2_costmap_2d + nav2_costmap_2d_markers + nav2_costmap_2d_cloud RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index 6452110c85..45725757f2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -39,9 +39,9 @@ /** Provides a mapping for often used cost values */ namespace nav2_costmap_2d { -static const unsigned char NO_INFORMATION = 255; -static const unsigned char LETHAL_OBSTACLE = 254; -static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; -static const unsigned char FREE_SPACE = 0; +static constexpr unsigned char NO_INFORMATION = 255; +static constexpr unsigned char LETHAL_OBSTACLE = 254; +static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; +static constexpr unsigned char FREE_SPACE = 0; } #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index bbd8e6988d..83349d4470 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp @@ -151,8 +151,10 @@ class Costmap2DPublisher // Service for getting the costmaps rclcpp::Service::SharedPtr costmap_service_; - nav_msgs::msg::OccupancyGrid grid_; - nav2_msgs::msg::Costmap costmap_raw_; + float grid_resolution; + unsigned int grid_width, grid_height; + std::unique_ptr grid_; + std::unique_ptr costmap_raw_; // Translate from 0-255 values in costmap to -1 to 100 values in message. static char * cost_translation_table_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp similarity index 74% rename from nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp rename to nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp index 13139e6cd7..636a907f07 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp @@ -11,9 +11,11 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +// +// Modified by: Shivang Patel (shivaan14@gmail.com) -#ifndef NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_ -#define NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_ +#ifndef NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_ +#define NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_ #include #include @@ -24,6 +26,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose2_d.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/footprint_subscriber.hpp" #include "nav2_util/robot_utils.hpp" @@ -34,41 +37,40 @@ namespace nav2_costmap_2d { -typedef std::vector Footprint; -class CollisionChecker +class CostmapTopicCollisionChecker { public: - CollisionChecker( + CostmapTopicCollisionChecker( CostmapSubscriber & costmap_sub, FootprintSubscriber & footprint_sub, tf2_ros::Buffer & tf, std::string name = "collision_checker", - std::string global_frame = "map"); + std::string global_frame = "map", + std::string robot_base_frame = "base_link", + double transform_tolerance = 0.1); - ~CollisionChecker(); + ~CostmapTopicCollisionChecker() = default; // Returns the obstacle footprint score for a particular pose double scorePose(const geometry_msgs::msg::Pose2D & pose); bool isCollisionFree(const geometry_msgs::msg::Pose2D & pose); protected: - double lineCost(int x0, int x1, int y0, int y1) const; - double pointCost(int x, int y) const; void unorientFootprint(const Footprint & oriented_footprint, Footprint & reset_footprint); - void worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); Footprint getFootprint(const geometry_msgs::msg::Pose2D & pose); - double footprintCost(const Footprint footprint); - - std::shared_ptr costmap_; // Name used for logging std::string name_; std::string global_frame_; + std::string robot_base_frame_; tf2_ros::Buffer & tf_; CostmapSubscriber & costmap_sub_; FootprintSubscriber & footprint_sub_; + double transform_tolerance_; + FootprintCollisionChecker collision_checker_; }; + } // namespace nav2_costmap_2d -#endif // NAV2_COSTMAP_2D__COLLISION_CHECKER_HPP_ +#endif // NAV2_COSTMAP_2D__COSTMAP_TOPIC_COLLISION_CHECKER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp new file mode 100644 index 0000000000..24f35aa135 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_collision_checker.hpp @@ -0,0 +1,53 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Modified by: Shivang Patel (shivaang14@gmail.com) + +#ifndef NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ +#define NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_util/robot_utils.hpp" + +namespace nav2_costmap_2d +{ +typedef std::vector Footprint; + +class FootprintCollisionChecker +{ +public: + FootprintCollisionChecker(); + explicit FootprintCollisionChecker(std::shared_ptr costmap); + double footprintCost(const Footprint footprint); + double footprintCostAtPose(double x, double y, double theta, const Footprint footprint); + double lineCost(int x0, int x1, int y0, int y1) const; + bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my); + double pointCost(int x, int y) const; + void setCostmap(std::shared_ptr costmap); + +private: + std::shared_ptr costmap_; +}; + +} // namespace nav2_costmap_2d + +#endif // NAV2_COSTMAP_2D__FOOTPRINT_COLLISION_CHECKER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index edc4803061..08fed09145 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -64,7 +64,7 @@ class CellData * @return */ CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) - : index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) + : index_(static_cast(i)), x_(x), y_(y), src_x_(sx), src_y_(sy) { } unsigned int index_; @@ -77,27 +77,21 @@ class InflationLayer : public Layer public: InflationLayer(); - virtual ~InflationLayer() - { - deleteKernels(); - } + ~InflationLayer() override = default; - virtual void onInitialize(); - virtual void updateBounds( + void onInitialize() override; + void updateBounds( double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, - double * max_y); - virtual void updateCosts( + double * max_y) override; + void updateCosts( nav2_costmap_2d::Costmap2D & master_grid, - int min_i, int min_j, int max_i, int max_j); - virtual bool isDiscretized() - { - return true; - } - virtual void matchSize(); + int min_i, int min_j, int max_i, int max_j) override; + + void matchSize() override; - virtual void reset() + void reset() override { matchSize(); } @@ -114,15 +108,15 @@ class InflationLayer : public Layer cost = INSCRIBED_INFLATED_OBSTACLE; } else { // make sure cost falls off by Euclidean distance - double euclidean_distance = distance * resolution_; - double factor = exp(-1.0 * cost_scaling_factor_ * (euclidean_distance - inscribed_radius_)); - cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); + double factor = + exp(-1.0 * cost_scaling_factor_ * (distance * resolution_ - inscribed_radius_)); + cost = static_cast((INSCRIBED_INFLATED_OBSTACLE - 1) * factor); } return cost; } protected: - virtual void onFootprintChanged(); + void onFootprintChanged() override; private: /** @@ -133,11 +127,13 @@ class InflationLayer : public Layer * @param src_y The y coordinate of the source cell * @return */ - inline double distanceLookup(int mx, int my, int src_x, int src_y) + inline double distanceLookup( + unsigned int mx, unsigned int my, unsigned int src_x, + unsigned int src_y) { - unsigned int dx = abs(mx - src_x); - unsigned int dy = abs(my - src_y); - return cached_distances_[dx][dy]; + unsigned int dx = (mx > src_x) ? mx - src_x : src_x - mx; + unsigned int dy = (my > src_y) ? my - src_y : src_y - my; + return cached_distances_[dx * cache_length_ + dy]; } /** @@ -148,16 +144,18 @@ class InflationLayer : public Layer * @param src_y The y coordinate of the source cell * @return */ - inline unsigned char costLookup(int mx, int my, int src_x, int src_y) + inline unsigned char costLookup( + unsigned int mx, unsigned int my, unsigned int src_x, + unsigned int src_y) { - unsigned int dx = abs(mx - src_x); - unsigned int dy = abs(my - src_y); - return cached_costs_[dx][dy]; + unsigned int dx = (mx > src_x) ? mx - src_x : src_x - mx; + unsigned int dy = (my > src_y) ? my - src_y : src_y - my; + return cached_costs_[dx * cache_length_ + dy]; } void computeCaches(); - void deleteKernels(); - void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char * master_grid); + + int generateIntegerDistances(); unsigned int cellDistance(double world_dist) { @@ -169,17 +167,19 @@ class InflationLayer : public Layer unsigned int src_x, unsigned int src_y); double inflation_radius_, inscribed_radius_, cost_scaling_factor_; - bool inflate_unknown_; + bool inflate_unknown_, inflate_around_unknown_; unsigned int cell_inflation_radius_; unsigned int cached_cell_inflation_radius_; - std::map> inflation_cells_; + std::vector> inflation_cells_; double resolution_; std::vector seen_; - unsigned char ** cached_costs_; - double ** cached_distances_; + std::vector cached_costs_; + std::vector cached_distances_; + std::vector> distance_matrix_; + unsigned int cache_length_; double last_min_x_, last_min_y_, last_max_x_, last_max_y_; // Indicates that the entire costmap should be reinflated next time around. diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 05a5185583..f6f01e8c56 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -110,6 +110,7 @@ class StaticLayer : public CostmapLayer unsigned char unknown_cost_value_; bool trinary_costmap_; bool map_received_{false}; + tf2::Duration transform_tolerance_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp index 99cf9e1777..013341adf9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp @@ -77,29 +77,29 @@ unsigned int countValues( return count; } -nav2_costmap_2d::StaticLayer * addStaticLayer( +void addStaticLayer( nav2_costmap_2d::LayeredCostmap & layers, - tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node) + tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr & slayer) { - nav2_costmap_2d::StaticLayer * slayer = new nav2_costmap_2d::StaticLayer(); + slayer = std::make_shared(); layers.addPlugin(std::shared_ptr(slayer)); slayer->initialize(&layers, "static", &tf, node, nullptr, nullptr /*TODO*/); - return slayer; } -nav2_costmap_2d::ObstacleLayer * addObstacleLayer( +void addObstacleLayer( nav2_costmap_2d::LayeredCostmap & layers, - tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node) + tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr & olayer) { - nav2_costmap_2d::ObstacleLayer * olayer = new nav2_costmap_2d::ObstacleLayer(); + olayer = std::make_shared(); olayer->initialize(&layers, "obstacles", &tf, node, nullptr, nullptr /*TODO*/); layers.addPlugin(std::shared_ptr(olayer)); - return olayer; } void addObservation( - nav2_costmap_2d::ObstacleLayer * olayer, double x, double y, double z = 0.0, - double ox = 0.0, double oy = 0.0, double oz = MAX_Z) + std::shared_ptr olayer, double x, double y, double z = 0.0, + double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true) { sensor_msgs::msg::PointCloud2 cloud; sensor_msgs::PointCloud2Modifier modifier(cloud); @@ -119,18 +119,18 @@ void addObservation( // obstacle range = raytrace range = 100.0 nav2_costmap_2d::Observation obs(p, cloud, 100.0, 100.0); - olayer->addStaticObservation(obs, true, true); + olayer->addStaticObservation(obs, marking, clearing); } -nav2_costmap_2d::InflationLayer * addInflationLayer( +void addInflationLayer( nav2_costmap_2d::LayeredCostmap & layers, - tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node) + tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node, + std::shared_ptr & ilayer) { - nav2_costmap_2d::InflationLayer * ilayer = new nav2_costmap_2d::InflationLayer(); + ilayer = std::make_shared(); ilayer->initialize(&layers, "inflation", &tf, node, nullptr, nullptr /*TODO*/); std::shared_ptr ipointer(ilayer); layers.addPlugin(ipointer); - return ilayer; } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 764496cc68..92445cd71f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -99,7 +99,6 @@ class VoxelLayer : public ObstacleLayer double z_resolution_, origin_z_; int unknown_threshold_, mark_threshold_, size_z_; rclcpp::Publisher::SharedPtr clearing_endpoints_pub_; - sensor_msgs::msg::PointCloud clearing_endpoints_; inline bool worldToMap3DFloat( double wx, double wy, double wz, double & mx, double & my, diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 574a155779..86561d334b 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -37,7 +37,6 @@ *********************************************************************/ #include "nav2_costmap_2d/inflation_layer.hpp" -#include #include #include #include @@ -61,14 +60,14 @@ InflationLayer::InflationLayer() inscribed_radius_(0), cost_scaling_factor_(0), inflate_unknown_(false), + inflate_around_unknown_(false), cell_inflation_radius_(0), cached_cell_inflation_radius_(0), - cached_costs_(nullptr), - cached_distances_(nullptr), - last_min_x_(-std::numeric_limits::max()), - last_min_y_(-std::numeric_limits::max()), - last_max_x_(std::numeric_limits::max()), - last_max_y_(std::numeric_limits::max()) + cache_length_(0), + last_min_x_(std::numeric_limits::lowest()), + last_min_y_(std::numeric_limits::lowest()), + last_max_x_(std::numeric_limits::max()), + last_max_y_(std::numeric_limits::max()) { } @@ -79,14 +78,18 @@ InflationLayer::onInitialize() declareParameter("inflation_radius", rclcpp::ParameterValue(0.55)); declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0)); declareParameter("inflate_unknown", rclcpp::ParameterValue(false)); + declareParameter("inflate_around_unknown", rclcpp::ParameterValue(false)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "inflation_radius", inflation_radius_); node_->get_parameter(name_ + "." + "cost_scaling_factor", cost_scaling_factor_); node_->get_parameter(name_ + "." + "inflate_unknown", inflate_unknown_); + node_->get_parameter(name_ + "." + "inflate_around_unknown", inflate_around_unknown_); current_ = true; seen_.clear(); + cached_distances_.clear(); + cached_costs_.clear(); need_reinflation_ = false; cell_inflation_radius_ = cellDistance(inflation_radius_); matchSize(); @@ -112,13 +115,11 @@ InflationLayer::updateBounds( last_min_y_ = *min_y; last_max_x_ = *max_x; last_max_y_ = *max_y; - // For some reason when I make these -::max() it does not - // work with Costmap2D::worldToMapEnforceBounds(), so I'm using - // -::max() instead. - *min_x = -std::numeric_limits::max(); - *min_y = -std::numeric_limits::max(); - *max_x = std::numeric_limits::max(); - *max_y = std::numeric_limits::max(); + + *min_x = std::numeric_limits::lowest(); + *min_y = std::numeric_limits::lowest(); + *max_x = std::numeric_limits::max(); + *max_y = std::numeric_limits::max(); need_reinflation_ = false; } else { double tmp_min_x = last_min_x_; @@ -162,9 +163,11 @@ InflationLayer::updateCosts( } // make sure the inflation list is empty at the beginning of the cycle (should always be true) - RCLCPP_FATAL_EXPRESSION( - rclcpp::get_logger("nav2_costmap_2d"), - !inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation"); + for (auto & dist:inflation_cells_) { + RCLCPP_FATAL_EXPRESSION( + rclcpp::get_logger("nav2_costmap_2d"), + !dist.empty(), "The inflation list must be empty at the beginning of inflation"); + } unsigned char * master_array = master_grid.getCharMap(); unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); @@ -182,10 +185,10 @@ InflationLayer::updateCosts( // box min_i...max_j, by the amount cell_inflation_radius_. Cells // up to that distance outside the box can still influence the costs // stored in cells inside the box. - min_i -= cell_inflation_radius_; - min_j -= cell_inflation_radius_; - max_i += cell_inflation_radius_; - max_j += cell_inflation_radius_; + min_i -= static_cast(cell_inflation_radius_); + min_j -= static_cast(cell_inflation_radius_); + max_i += static_cast(cell_inflation_radius_); + max_j += static_cast(cell_inflation_radius_); min_i = std::max(0, min_i); min_j = std::max(0, min_j); @@ -198,13 +201,13 @@ InflationLayer::updateCosts( // with a notable performance boost // Start with lethal obstacles: by definition distance is 0.0 - std::vector & obs_bin = inflation_cells_[0.0]; + auto & obs_bin = inflation_cells_[0]; for (int j = min_j; j < max_j; j++) { for (int i = min_i; i < max_i; i++) { - int index = master_grid.getIndex(i, j); + int index = static_cast(master_grid.getIndex(i, j)); unsigned char cost = master_array[index]; - if (cost == LETHAL_OBSTACLE) { - obs_bin.push_back(CellData(index, i, j, i, j)); + if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) { + obs_bin.emplace_back(index, i, j, i, j); } } } @@ -212,13 +215,11 @@ InflationLayer::updateCosts( // Process cells by increasing distance; new cells are appended to the // corresponding distance bin, so they // can overtake previously inserted but farther away cells - std::map>::iterator bin; - for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) { - for (unsigned int i = 0; i < bin->second.size(); ++i) { - // process all cells at distance dist_bin.first - const CellData & cell = bin->second[i]; - - unsigned int index = cell.index_; + for (const auto & dist_bin: inflation_cells_) { + for (std::size_t i = 0; i < dist_bin.size(); ++i) { + // Do not use iterator or for-range based loops to iterate though dist_bin, since it's size might + // change when a new cell is enqueued, invalidating all iterators + unsigned int index = dist_bin[i].index_; // ignore if already visited if (seen_[index]) { @@ -227,10 +228,10 @@ InflationLayer::updateCosts( seen_[index] = true; - unsigned int mx = cell.x_; - unsigned int my = cell.y_; - unsigned int sx = cell.src_x_; - unsigned int sy = cell.src_y_; + unsigned int mx = dist_bin[i].x_; + unsigned int my = dist_bin[i].y_; + unsigned int sx = dist_bin[i].src_x_; + unsigned int sy = dist_bin[i].src_y_; // assign the cost associated with the distance from an obstacle to the cell unsigned char cost = costLookup(mx, my, sx, sy); @@ -259,7 +260,10 @@ InflationLayer::updateCosts( } } - inflation_cells_.clear(); + for (auto & dist:inflation_cells_) { + dist.clear(); + dist.reserve(200); + } } /** @@ -287,8 +291,11 @@ InflationLayer::enqueue( return; } + const unsigned int r = cell_inflation_radius_ + 2; + // push the cell data onto the inflation list and mark - inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y)); + inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back( + index, mx, my, src_x, src_y); } } @@ -299,55 +306,74 @@ InflationLayer::computeCaches() return; } + cache_length_ = cell_inflation_radius_ + 2; + // based on the inflation radius... compute distance and cost caches if (cell_inflation_radius_ != cached_cell_inflation_radius_) { - deleteKernels(); + cached_costs_.resize(cache_length_ * cache_length_); + cached_distances_.resize(cache_length_ * cache_length_); - cached_costs_ = new unsigned char *[cell_inflation_radius_ + 2]; - cached_distances_ = new double *[cell_inflation_radius_ + 2]; - - for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) { - cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2]; - cached_distances_[i] = new double[cell_inflation_radius_ + 2]; - for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) { - cached_distances_[i][j] = hypot(i, j); + for (unsigned int i = 0; i < cache_length_; ++i) { + for (unsigned int j = 0; j < cache_length_; ++j) { + cached_distances_[i * cache_length_ + j] = hypot(i, j); } } cached_cell_inflation_radius_ = cell_inflation_radius_; } - for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i) { - for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j) { - cached_costs_[i][j] = computeCost(cached_distances_[i][j]); + for (unsigned int i = 0; i < cache_length_; ++i) { + for (unsigned int j = 0; j < cache_length_; ++j) { + cached_costs_[i * cache_length_ + j] = computeCost(cached_distances_[i * cache_length_ + j]); } } + + int max_dist = generateIntegerDistances(); + inflation_cells_.clear(); + inflation_cells_.resize(max_dist + 1); + for (auto & dist : inflation_cells_) { + dist.reserve(200); + } } -void -InflationLayer::deleteKernels() +int +InflationLayer::generateIntegerDistances() { - if (cached_distances_ != NULL) { - for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) { - if (cached_distances_[i]) { - delete[] cached_distances_[i]; + const int r = cell_inflation_radius_ + 2; + const int size = r * 2 + 1; + + std::vector> points; + + for (int y = -r; y <= r; y++) { + for (int x = -r; x <= r; x++) { + if (x * x + y * y <= r * r) { + points.emplace_back(x, y); } } - if (cached_distances_) { - delete[] cached_distances_; - } - cached_distances_ = NULL; } - if (cached_costs_ != NULL) { - for (unsigned int i = 0; i <= cached_cell_inflation_radius_ + 1; ++i) { - if (cached_costs_[i]) { - delete[] cached_costs_[i]; - } + std::sort( + points.begin(), points.end(), + [](const std::pair & a, const std::pair & b) -> bool { + return a.first * a.first + a.second * a.second < b.first * b.first + b.second * b.second; } - delete[] cached_costs_; - cached_costs_ = NULL; + ); + + std::vector> distance_matrix(size, std::vector(size, 0)); + std::pair last = {0, 0}; + int level = 0; + for (auto const & p : points) { + if (p.first * p.first + p.second * p.second != + last.first * last.first + last.second * last.second) + { + level++; + } + distance_matrix[p.first + r][p.second + r] = level; + last = p; } + + distance_matrix_ = distance_matrix; + return level; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index f84d8d1f40..8180e3f6ba 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -117,10 +117,12 @@ void StaticLayer::getParameters() { int temp_lethal_threshold = 0; + double temp_tf_tol = 0.0; declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); + declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); @@ -133,10 +135,13 @@ StaticLayer::getParameters() node_->get_parameter("lethal_cost_threshold", temp_lethal_threshold); node_->get_parameter("unknown_cost_value", unknown_cost_value_); node_->get_parameter("trinary_costmap", trinary_costmap_); + node_->get_parameter("transform_tolerance", temp_tf_tol); // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; + + transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); } void @@ -359,7 +364,9 @@ StaticLayer::updateCosts( // Might even be in a different frame geometry_msgs::msg::TransformStamped transform; try { - transform = tf_->lookupTransform(map_frame_, global_frame_, tf2::TimePointZero); + transform = tf_->lookupTransform( + map_frame_, global_frame_, tf2::TimePointZero, + transform_tolerance_); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what()); return; diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index b9f3f224b0..5235e23675 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -41,6 +41,8 @@ #include #include #include +#include +#include #include "pluginlib/class_list_macros.hpp" #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -207,24 +209,25 @@ void VoxelLayer::updateBounds( } if (publish_voxel_) { - nav2_msgs::msg::VoxelGrid grid_msg; + auto grid_msg = std::make_unique(); unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY(); - grid_msg.size_x = voxel_grid_.sizeX(); - grid_msg.size_y = voxel_grid_.sizeY(); - grid_msg.size_z = voxel_grid_.sizeZ(); - grid_msg.data.resize(size); - memcpy(&grid_msg.data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); - - grid_msg.origin.x = origin_x_; - grid_msg.origin.y = origin_y_; - grid_msg.origin.z = origin_z_; - - grid_msg.resolutions.x = resolution_; - grid_msg.resolutions.y = resolution_; - grid_msg.resolutions.z = z_resolution_; - grid_msg.header.frame_id = global_frame_; - grid_msg.header.stamp = node_->now(); - voxel_pub_->publish(grid_msg); + grid_msg->size_x = voxel_grid_.sizeX(); + grid_msg->size_y = voxel_grid_.sizeY(); + grid_msg->size_z = voxel_grid_.sizeZ(); + grid_msg->data.resize(size); + memcpy(&grid_msg->data[0], voxel_grid_.getData(), size * sizeof(unsigned int)); + + grid_msg->origin.x = origin_x_; + grid_msg->origin.y = origin_y_; + grid_msg->origin.z = origin_z_; + + grid_msg->resolutions.x = resolution_; + grid_msg->resolutions.y = resolution_; + grid_msg->resolutions.z = z_resolution_; + grid_msg->header.frame_id = global_frame_; + grid_msg->header.stamp = node_->now(); + + voxel_pub_->publish(std::move(grid_msg)); } updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); @@ -289,6 +292,8 @@ void VoxelLayer::raytraceFreespace( double * max_x, double * max_y) { + auto clearing_endpoints_ = std::make_unique(); + size_t clearing_observation_cloud_size = clearing_observation.cloud_->height * clearing_observation.cloud_->width; if (clearing_observation_cloud_size == 0) { @@ -310,8 +315,8 @@ void VoxelLayer::raytraceFreespace( bool publish_clearing_points = (node_->count_subscribers("clearing_endpoints") > 0); if (publish_clearing_points) { - clearing_endpoints_.points.clear(); - clearing_endpoints_.points.reserve(clearing_observation_cloud_size); + clearing_endpoints_->points.clear(); + clearing_endpoints_->points.reserve(clearing_observation_cloud_size); } // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later @@ -391,16 +396,16 @@ void VoxelLayer::raytraceFreespace( point.x = wpx; point.y = wpy; point.z = wpz; - clearing_endpoints_.points.push_back(point); + clearing_endpoints_->points.push_back(point); } } } if (publish_clearing_points) { - clearing_endpoints_.header.frame_id = global_frame_; - clearing_endpoints_.header.stamp = clearing_observation.cloud_->header.stamp; + clearing_endpoints_->header.frame_id = global_frame_; + clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp; - clearing_endpoints_pub_->publish(clearing_endpoints_); + clearing_endpoints_pub_->publish(std::move(clearing_endpoints_)); } } diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp deleted file mode 100644 index 69b40f4466..0000000000 --- a/nav2_costmap_2d/src/collision_checker.cpp +++ /dev/null @@ -1,189 +0,0 @@ -// Copyright (c) 2019 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "nav2_costmap_2d/collision_checker.hpp" - -#include "nav2_costmap_2d/cost_values.hpp" -#include "nav2_costmap_2d/exceptions.hpp" -#include "nav2_costmap_2d/footprint.hpp" -#include "nav2_util/line_iterator.hpp" - -using namespace std::chrono_literals; - -namespace nav2_costmap_2d -{ - -CollisionChecker::CollisionChecker( - CostmapSubscriber & costmap_sub, - FootprintSubscriber & footprint_sub, - tf2_ros::Buffer & tf, - std::string name, - std::string global_frame) -: name_(name), - global_frame_(global_frame), - tf_(tf), - costmap_sub_(costmap_sub), - footprint_sub_(footprint_sub) -{ -} - -CollisionChecker::~CollisionChecker() -{ -} - -bool CollisionChecker::isCollisionFree( - const geometry_msgs::msg::Pose2D & pose) -{ - try { - if (scorePose(pose) < 0) { - return false; - } - return true; - } catch (const IllegalPoseException & e) { - RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what()); - return false; - } catch (const CollisionCheckerException & e) { - RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what()); - return false; - } catch (...) { - RCLCPP_ERROR(rclcpp::get_logger(name_), "Failed to check pose score!"); - return false; - } -} - -double CollisionChecker::scorePose( - const geometry_msgs::msg::Pose2D & pose) -{ - try { - costmap_ = costmap_sub_.getCostmap(); - } catch (const std::runtime_error & e) { - throw CollisionCheckerException(e.what()); - } - - unsigned int cell_x, cell_y; - if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) { - RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", cell_x, cell_y); - throw IllegalPoseException(name_, "Pose Goes Off Grid."); - } - - return footprintCost(getFootprint(pose)); -} - -void CollisionChecker::worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) -{ - if (!costmap_->worldToMap(wx, wy, mx, my)) { - RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", mx, my); - throw IllegalPoseException(name_, "Footprint Goes Off Grid."); - } -} - -Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) -{ - Footprint footprint; - if (!footprint_sub_.getFootprint(footprint)) { - throw CollisionCheckerException("Current footprint not available."); - } - - Footprint footprint_spec; - unorientFootprint(footprint, footprint_spec); - transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint); - - return footprint; -} - -double CollisionChecker::footprintCost(const Footprint footprint) -{ - // now we really have to lay down the footprint in the costmap_ grid - unsigned int x0, x1, y0, y1; - double footprint_cost = 0.0; - - // we need to rasterize each line in the footprint - for (unsigned int i = 0; i < footprint.size() - 1; ++i) { - // get the cell coord of the first point - worldToMap(footprint[i].x, footprint[i].y, x0, y0); - - // get the cell coord of the second point - worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1); - - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - } - - // we also need to connect the first point in the footprint to the last point - // get the cell coord of the last point - worldToMap(footprint.back().x, footprint.back().y, x0, y0); - - // get the cell coord of the first point - worldToMap(footprint.front().x, footprint.front().y, x1, y1); - - footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); - - // if all line costs are legal... then we can return that the footprint is legal - return footprint_cost; -} - -double CollisionChecker::lineCost(int x0, int x1, int y0, int y1) const -{ - double line_cost = 0.0; - double point_cost = -1.0; - - for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { - point_cost = pointCost(line.getX(), line.getY()); // Score the current point - - if (line_cost < point_cost) { - line_cost = point_cost; - } - } - - return line_cost; -} - -double CollisionChecker::pointCost(int x, int y) const -{ - unsigned char cost = costmap_->getCost(x, y); - // if the cell is in an obstacle the path is invalid or unknown - if (cost == LETHAL_OBSTACLE) { - RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y); - throw IllegalPoseException(name_, "Footprint Hits Obstacle."); - } else if (cost == NO_INFORMATION) { - RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", x, y); - throw IllegalPoseException(name_, "Footprint Hits Unknown Region."); - } - - return cost; -} - -void CollisionChecker::unorientFootprint( - const std::vector & oriented_footprint, - std::vector & reset_footprint) -{ - geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, tf_, global_frame_)) { - throw CollisionCheckerException("Robot pose unavailable."); - } - - double x = current_pose.pose.position.x; - double y = current_pose.pose.position.y; - double theta = tf2::getYaw(current_pose.pose.orientation); - - Footprint temp; - transformFootprint(-x, -y, 0, oriented_footprint, temp); - transformFootprint(0, 0, -theta, temp, reset_footprint); -} - -} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_cloud.cpp b/nav2_costmap_2d/src/costmap_2d_cloud.cpp index ac4f091c76..52157b3b7a 100644 --- a/nav2_costmap_2d/src/costmap_2d_cloud.cpp +++ b/nav2_costmap_2d/src/costmap_2d_cloud.cpp @@ -27,6 +27,8 @@ #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud.hpp" @@ -132,17 +134,17 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) } { - sensor_msgs::msg::PointCloud cloud; - cloud.points.resize(num_marked); - cloud.channels.resize(1); - cloud.channels[0].values.resize(num_marked); - cloud.channels[0].name = "rgb"; - cloud.header.frame_id = frame_id; - cloud.header.stamp = stamp; - - sensor_msgs::msg::ChannelFloat32 & chan = cloud.channels[0]; + auto cloud = std::make_unique(); + cloud->points.resize(num_marked); + cloud->channels.resize(1); + cloud->channels[0].values.resize(num_marked); + cloud->channels[0].name = "rgb"; + cloud->header.frame_id = frame_id; + cloud->header.stamp = stamp; + + sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0]; for (uint32_t i = 0; i < num_marked; ++i) { - geometry_msgs::msg::Point32 & p = cloud.points[i]; + geometry_msgs::msg::Point32 & p = cloud->points[i]; float & cval = chan.values[i]; Cell & c = g_marked[i]; @@ -159,21 +161,21 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) memcpy(&cval, &col, sizeof col); } - pub_marked->publish(cloud); + pub_marked->publish(std::move(cloud)); } { - sensor_msgs::msg::PointCloud cloud; - cloud.points.resize(num_unknown); - cloud.channels.resize(1); - cloud.channels[0].values.resize(num_unknown); - cloud.channels[0].name = "rgb"; - cloud.header.frame_id = frame_id; - cloud.header.stamp = stamp; - - sensor_msgs::msg::ChannelFloat32 & chan = cloud.channels[0]; + auto cloud = std::make_unique(); + cloud->points.resize(num_unknown); + cloud->channels.resize(1); + cloud->channels[0].values.resize(num_unknown); + cloud->channels[0].name = "rgb"; + cloud->header.frame_id = frame_id; + cloud->header.stamp = stamp; + + sensor_msgs::msg::ChannelFloat32 & chan = cloud->channels[0]; for (uint32_t i = 0; i < num_unknown; ++i) { - geometry_msgs::msg::Point32 & p = cloud.points[i]; + geometry_msgs::msg::Point32 & p = cloud->points[i]; float & cval = chan.values[i]; Cell & c = g_unknown[i]; @@ -190,7 +192,7 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) memcpy(&cval, &col, sizeof col); } - pub_unknown->publish(cloud); + pub_unknown->publish(std::move(cloud)); } timer.end(); diff --git a/nav2_costmap_2d/src/costmap_2d_markers.cpp b/nav2_costmap_2d/src/costmap_2d_markers.cpp index 60e8ab1b3d..281150bbf2 100644 --- a/nav2_costmap_2d/src/costmap_2d_markers.cpp +++ b/nav2_costmap_2d/src/costmap_2d_markers.cpp @@ -38,6 +38,8 @@ *********************************************************************/ #include #include +#include +#include #include "rclcpp/rclcpp.hpp" #include "visualization_msgs/msg/marker.hpp" @@ -111,31 +113,31 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid) } } - visualization_msgs::msg::Marker m; - m.header.frame_id = frame_id; - m.header.stamp = stamp; - m.ns = g_node->get_namespace(); - m.id = 0; - m.type = visualization_msgs::msg::Marker::CUBE_LIST; - m.action = visualization_msgs::msg::Marker::ADD; - m.pose.orientation.w = 1.0; - m.scale.x = x_res; - m.scale.y = y_res; - m.scale.z = z_res; - m.color.r = g_colors_r[nav2_voxel_grid::MARKED]; - m.color.g = g_colors_g[nav2_voxel_grid::MARKED]; - m.color.b = g_colors_b[nav2_voxel_grid::MARKED]; - m.color.a = g_colors_a[nav2_voxel_grid::MARKED]; - m.points.resize(num_markers); + auto m = std::make_unique(); + m->header.frame_id = frame_id; + m->header.stamp = stamp; + m->ns = g_node->get_namespace(); + m->id = 0; + m->type = visualization_msgs::msg::Marker::CUBE_LIST; + m->action = visualization_msgs::msg::Marker::ADD; + m->pose.orientation.w = 1.0; + m->scale.x = x_res; + m->scale.y = y_res; + m->scale.z = z_res; + m->color.r = g_colors_r[nav2_voxel_grid::MARKED]; + m->color.g = g_colors_g[nav2_voxel_grid::MARKED]; + m->color.b = g_colors_b[nav2_voxel_grid::MARKED]; + m->color.a = g_colors_a[nav2_voxel_grid::MARKED]; + m->points.resize(num_markers); for (uint32_t i = 0; i < num_markers; ++i) { Cell & c = g_cells[i]; - geometry_msgs::msg::Point & p = m.points[i]; + geometry_msgs::msg::Point & p = m->points[i]; p.x = c.x; p.y = c.y; p.z = c.z; } - pub->publish(m); + pub->publish(std::move(m)); timer.end(); RCLCPP_INFO( diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index e971cc77f6..56797e566b 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -40,6 +40,7 @@ #include #include +#include #include "nav2_costmap_2d/cost_values.hpp" @@ -110,30 +111,34 @@ void Costmap2DPublisher::onNewSubscription(const ros::SingleSubscriberPublisher& void Costmap2DPublisher::prepareGrid() { std::unique_lock lock(*(costmap_->getMutex())); - double resolution = costmap_->getResolution(); + grid_resolution = costmap_->getResolution(); + grid_width = costmap_->getSizeInCellsX(); + grid_height = costmap_->getSizeInCellsY(); + + grid_ = std::make_unique(); - grid_.header.frame_id = global_frame_; - grid_.header.stamp = rclcpp::Time(); + grid_->header.frame_id = global_frame_; + grid_->header.stamp = rclcpp::Time(); - grid_.info.resolution = resolution; + grid_->info.resolution = grid_resolution; - grid_.info.width = costmap_->getSizeInCellsX(); - grid_.info.height = costmap_->getSizeInCellsY(); + grid_->info.width = grid_width; + grid_->info.height = grid_height; double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - grid_.info.origin.position.x = wx - resolution / 2; - grid_.info.origin.position.y = wy - resolution / 2; - grid_.info.origin.position.z = 0.0; - grid_.info.origin.orientation.w = 1.0; + grid_->info.origin.position.x = wx - grid_resolution / 2; + grid_->info.origin.position.y = wy - grid_resolution / 2; + grid_->info.origin.position.z = 0.0; + grid_->info.origin.orientation.w = 1.0; saved_origin_x_ = costmap_->getOriginX(); saved_origin_y_ = costmap_->getOriginY(); - grid_.data.resize(grid_.info.width * grid_.info.height); + grid_->data.resize(grid_->info.width * grid_->info.height); unsigned char * data = costmap_->getCharMap(); - for (unsigned int i = 0; i < grid_.data.size(); i++) { - grid_.data[i] = cost_translation_table_[data[i]]; + for (unsigned int i = 0; i < grid_->data.size(); i++) { + grid_->data[i] = cost_translation_table_[data[i]]; } } @@ -142,27 +147,29 @@ void Costmap2DPublisher::prepareCostmap() std::unique_lock lock(*(costmap_->getMutex())); double resolution = costmap_->getResolution(); - costmap_raw_.header.frame_id = global_frame_; - costmap_raw_.header.stamp = node_->now(); + costmap_raw_ = std::make_unique(); + + costmap_raw_->header.frame_id = global_frame_; + costmap_raw_->header.stamp = node_->now(); - costmap_raw_.metadata.layer = "master"; - costmap_raw_.metadata.resolution = resolution; + costmap_raw_->metadata.layer = "master"; + costmap_raw_->metadata.resolution = resolution; - costmap_raw_.metadata.size_x = costmap_->getSizeInCellsX(); - costmap_raw_.metadata.size_y = costmap_->getSizeInCellsY(); + costmap_raw_->metadata.size_x = costmap_->getSizeInCellsX(); + costmap_raw_->metadata.size_y = costmap_->getSizeInCellsY(); double wx, wy; costmap_->mapToWorld(0, 0, wx, wy); - costmap_raw_.metadata.origin.position.x = wx - resolution / 2; - costmap_raw_.metadata.origin.position.y = wy - resolution / 2; - costmap_raw_.metadata.origin.position.z = 0.0; - costmap_raw_.metadata.origin.orientation.w = 1.0; + costmap_raw_->metadata.origin.position.x = wx - resolution / 2; + costmap_raw_->metadata.origin.position.y = wy - resolution / 2; + costmap_raw_->metadata.origin.position.z = 0.0; + costmap_raw_->metadata.origin.orientation.w = 1.0; - costmap_raw_.data.resize(costmap_raw_.metadata.size_x * costmap_raw_.metadata.size_y); + costmap_raw_->data.resize(costmap_raw_->metadata.size_x * costmap_raw_->metadata.size_y); unsigned char * data = costmap_->getCharMap(); - for (unsigned int i = 0; i < costmap_raw_.data.size(); i++) { - costmap_raw_.data[i] = data[i]; + for (unsigned int i = 0; i < costmap_raw_->data.size(); i++) { + costmap_raw_->data[i] = data[i]; } } @@ -170,40 +177,40 @@ void Costmap2DPublisher::publishCostmap() { if (node_->count_subscribers(costmap_raw_pub_->get_topic_name()) > 0) { prepareCostmap(); - costmap_raw_pub_->publish(costmap_raw_); + costmap_raw_pub_->publish(std::move(costmap_raw_)); } float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_.info.resolution != resolution || - grid_.info.width != costmap_->getSizeInCellsX() || - grid_.info.height != costmap_->getSizeInCellsY() || + if (always_send_full_costmap_ || grid_resolution != resolution || + grid_width != costmap_->getSizeInCellsX() || + grid_height != costmap_->getSizeInCellsY() || saved_origin_x_ != costmap_->getOriginX() || saved_origin_y_ != costmap_->getOriginY()) { if (node_->count_subscribers(costmap_pub_->get_topic_name()) > 0) { prepareGrid(); - costmap_pub_->publish(grid_); + costmap_pub_->publish(std::move(grid_)); } } else if (x0_ < xn_) { if (node_->count_subscribers(costmap_update_pub_->get_topic_name()) > 0) { std::unique_lock lock(*(costmap_->getMutex())); // Publish Just an Update - map_msgs::msg::OccupancyGridUpdate update; - update.header.stamp = rclcpp::Time(); - update.header.frame_id = global_frame_; - update.x = x0_; - update.y = y0_; - update.width = xn_ - x0_; - update.height = yn_ - y0_; - update.data.resize(update.width * update.height); + auto update = std::make_unique(); + update->header.stamp = rclcpp::Time(); + update->header.frame_id = global_frame_; + update->x = x0_; + update->y = y0_; + update->width = xn_ - x0_; + update->height = yn_ - y0_; + update->data.resize(update->width * update->height); unsigned int i = 0; for (unsigned int y = y0_; y < yn_; y++) { for (unsigned int x = x0_; x < xn_; x++) { unsigned char cost = costmap_->getCost(x, y); - update.data[i++] = cost_translation_table_[cost]; + update->data[i++] = cost_translation_table_[cost]; } } - costmap_update_pub_->publish(update); + costmap_update_pub_->publish(std::move(update)); } } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 4a56cd178d..b9aaf8f4f7 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -41,6 +41,7 @@ #include #include #include +#include #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_util/execution_timer.hpp" @@ -441,13 +442,13 @@ Costmap2DROS::updateMap() const double yaw = tf2::getYaw(pose.pose.orientation); layered_costmap_->updateMap(x, y, yaw); - geometry_msgs::msg::PolygonStamped footprint; - footprint.header.frame_id = global_frame_; - footprint.header.stamp = now(); - transformFootprint(x, y, yaw, padded_footprint_, footprint); + auto footprint = std::make_unique(); + footprint->header.frame_id = global_frame_; + footprint->header.stamp = now(); + transformFootprint(x, y, yaw, padded_footprint_, *footprint); RCLCPP_DEBUG(get_logger(), "Publishing footprint"); - footprint_pub_->publish(footprint); + footprint_pub_->publish(std::move(footprint)); initialized_ = true; } } diff --git a/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp new file mode 100644 index 0000000000..225e6ac3fb --- /dev/null +++ b/nav2_costmap_2d/src/costmap_topic_collision_checker.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Modified by: Shivang Patel (shivaan14@gmail.com) + +#include +#include +#include +#include +#include + +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/exceptions.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/line_iterator.hpp" + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +CostmapTopicCollisionChecker::CostmapTopicCollisionChecker( + CostmapSubscriber & costmap_sub, + FootprintSubscriber & footprint_sub, + tf2_ros::Buffer & tf, + std::string name, + std::string global_frame, + std::string robot_base_frame, + double transform_tolerance) +: name_(name), + global_frame_(global_frame), + robot_base_frame_(robot_base_frame), + tf_(tf), + costmap_sub_(costmap_sub), + footprint_sub_(footprint_sub), + transform_tolerance_(transform_tolerance), + collision_checker_(nullptr) +{ +} + +bool CostmapTopicCollisionChecker::isCollisionFree( + const geometry_msgs::msg::Pose2D & pose) +{ + try { + if (scorePose(pose) >= LETHAL_OBSTACLE) { + return false; + } + return true; + } catch (const IllegalPoseException & e) { + RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what()); + return false; + } catch (const CollisionCheckerException & e) { + RCLCPP_ERROR(rclcpp::get_logger(name_), "%s", e.what()); + return false; + } catch (...) { + RCLCPP_ERROR(rclcpp::get_logger(name_), "Failed to check pose score!"); + return false; + } +} + +double CostmapTopicCollisionChecker::scorePose( + const geometry_msgs::msg::Pose2D & pose) +{ + try { + collision_checker_.setCostmap(costmap_sub_.getCostmap()); + } catch (const std::runtime_error & e) { + throw CollisionCheckerException(e.what()); + } + + unsigned int cell_x, cell_y; + if (!collision_checker_.worldToMap(pose.x, pose.y, cell_x, cell_y)) { + RCLCPP_DEBUG(rclcpp::get_logger(name_), "Map Cell: [%d, %d]", cell_x, cell_y); + throw IllegalPoseException(name_, "Pose Goes Off Grid."); + } + + return collision_checker_.footprintCost(getFootprint(pose)); +} + +Footprint CostmapTopicCollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose) +{ + Footprint footprint; + if (!footprint_sub_.getFootprint(footprint)) { + throw CollisionCheckerException("Current footprint not available."); + } + + Footprint footprint_spec; + unorientFootprint(footprint, footprint_spec); + transformFootprint(pose.x, pose.y, pose.theta, footprint_spec, footprint); + + return footprint; +} + +void CostmapTopicCollisionChecker::unorientFootprint( + const std::vector & oriented_footprint, + std::vector & reset_footprint) +{ + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose( + current_pose, tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { + throw CollisionCheckerException("Robot pose unavailable."); + } + + double x = current_pose.pose.position.x; + double y = current_pose.pose.position.y; + double theta = tf2::getYaw(current_pose.pose.orientation); + + Footprint temp; + transformFootprint(-x, -y, 0, oriented_footprint, temp); + transformFootprint(0, 0, -theta, temp, reset_footprint); +} + + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/footprint_collision_checker.cpp b/nav2_costmap_2d/src/footprint_collision_checker.cpp new file mode 100644 index 0000000000..15adcd8665 --- /dev/null +++ b/nav2_costmap_2d/src/footprint_collision_checker.cpp @@ -0,0 +1,131 @@ +// Copyright (c) 2019 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Modified by: Shivang Patel (shivaang14@gmail.com) + +#include +#include +#include +#include + +#include "nav2_costmap_2d/footprint_collision_checker.hpp" + +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_costmap_2d/exceptions.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/line_iterator.hpp" + +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ + +FootprintCollisionChecker::FootprintCollisionChecker() +: costmap_(nullptr) +{ +} + +FootprintCollisionChecker::FootprintCollisionChecker( + std::shared_ptr costmap) +: costmap_(costmap) +{ +} + +double FootprintCollisionChecker::footprintCost(const Footprint footprint) +{ + // now we really have to lay down the footprint in the costmap_ grid + unsigned int x0, x1, y0, y1; + double footprint_cost = 0.0; + + // we need to rasterize each line in the footprint + for (unsigned int i = 0; i < footprint.size() - 1; ++i) { + // get the cell coord of the first point + if (!worldToMap(footprint[i].x, footprint[i].y, x0, y0)) { + return static_cast(LETHAL_OBSTACLE); + } + + // get the cell coord of the second point + if (!worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1)) { + return static_cast(LETHAL_OBSTACLE); + } + + footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); + } + + // we also need to connect the first point in the footprint to the last point + // get the cell coord of the last point + if (!worldToMap(footprint.back().x, footprint.back().y, x0, y0)) { + return static_cast(LETHAL_OBSTACLE); + } + + // get the cell coord of the first point + if (!worldToMap(footprint.front().x, footprint.front().y, x1, y1)) { + return static_cast(LETHAL_OBSTACLE); + } + + footprint_cost = std::max(lineCost(x0, x1, y0, y1), footprint_cost); + + // if all line costs are legal... then we can return that the footprint is legal + return footprint_cost; +} + +double FootprintCollisionChecker::lineCost(int x0, int x1, int y0, int y1) const +{ + double line_cost = 0.0; + double point_cost = -1.0; + + for (nav2_util::LineIterator line(x0, y0, x1, y1); line.isValid(); line.advance()) { + point_cost = pointCost(line.getX(), line.getY()); // Score the current point + + if (line_cost < point_cost) { + line_cost = point_cost; + } + } + + return line_cost; +} + +bool FootprintCollisionChecker::worldToMap( + double wx, double wy, unsigned int & mx, unsigned int & my) +{ + return costmap_->worldToMap(wx, wy, mx, my); +} + +double FootprintCollisionChecker::pointCost(int x, int y) const +{ + return costmap_->getCost(x, y); +} + +void FootprintCollisionChecker::setCostmap(std::shared_ptr costmap) +{ + costmap_ = costmap; +} + +double FootprintCollisionChecker::footprintCostAtPose( + double x, double y, double theta, const Footprint footprint) +{ + double cos_th = cos(theta); + double sin_th = sin(theta); + Footprint oriented_footprint; + for (unsigned int i = 0; i < footprint.size(); ++i) { + geometry_msgs::msg::Point new_pt; + new_pt.x = x + (footprint[i].x * cos_th - footprint[i].y * sin_th); + new_pt.y = y + (footprint[i].x * sin_th + footprint[i].y * cos_th); + oriented_footprint.push_back(new_pt); + } + + return footprintCost(oriented_footprint); +} + +} // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 96cd1e6052..48ba1ea467 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -129,8 +129,8 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw) return; } - minx_ = miny_ = 1e30; - maxx_ = maxy_ = -1e30; + minx_ = miny_ = std::numeric_limits::max(); + maxx_ = maxy_ = std::numeric_limits::lowest(); for (vector>::iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 4a4ec21138..f87d59bc25 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -95,11 +95,12 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame) origin.point = obs.origin_; // we need to transform the origin of the observation to the new global frame - tf2_buffer_.transform(origin, origin, new_global_frame); + tf2_buffer_.transform(origin, origin, new_global_frame, tf2::durationFromSec(tf_tolerance_)); obs.origin_ = origin.point; // we also need to transform the cloud of the observation to the new global frame - tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame); + tf2_buffer_.transform( + *(obs.cloud_), *(obs.cloud_), new_global_frame, tf2::durationFromSec(tf_tolerance_)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( rclcpp::get_logger( diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 57271a9b51..4170ad3875 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -10,7 +10,7 @@ target_link_libraries(footprint_tests_exec ) ament_add_gtest_executable(test_collision_checker_exec - test_collision_checker.cpp + test_costmap_topic_collision_checker.cpp ) ament_target_dependencies(test_collision_checker_exec ${dependencies} diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 95c8f00642..5e8c19a485 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -32,8 +32,8 @@ def main(argv=sys.argv[1:]): lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', - node_executable='lifecycle_manager', - node_name='lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', output='screen', parameters=[{'node_names': ['map_server']}, {'autostart': True}]) diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index ae3e4d6abf..40262ce983 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -72,12 +72,13 @@ class TestNode : public ::testing::Test void validatePointInflation( unsigned int mx, unsigned int my, nav2_costmap_2d::Costmap2D * costmap, - nav2_costmap_2d::InflationLayer * ilayer, + std::shared_ptr & ilayer, double inflation_radius); + void initNode(std::vector parameters); void initNode(double inflation_radius); - void waitForMap(nav2_costmap_2d::StaticLayer * slayer); + void waitForMap(std::shared_ptr & slayer); protected: nav2_util::LifecycleNode::SharedPtr node_; @@ -106,7 +107,7 @@ std::vector TestNode::setRadii( return polygon; } -void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer) +void TestNode::waitForMap(std::shared_ptr & slayer) { while (!slayer->isCurrent()) { rclcpp::spin_some(node_->get_node_base_interface()); @@ -117,7 +118,7 @@ void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer) void TestNode::validatePointInflation( unsigned int mx, unsigned int my, nav2_costmap_2d::Costmap2D * costmap, - nav2_costmap_2d::InflationLayer * ilayer, + std::shared_ptr & ilayer, double inflation_radius) { bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()]; @@ -129,7 +130,7 @@ void TestNode::validatePointInflation( bin != m.end(); ++bin) { for (unsigned int i = 0; i < bin->second.size(); ++i) { - const CellData & cell = bin->second[i]; + const CellData cell = bin->second[i]; if (!seen[cell.index_]) { seen[cell.index_] = true; unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_; @@ -169,13 +170,8 @@ void TestNode::validatePointInflation( delete[] seen; } -void TestNode::initNode(double inflation_radius) +void TestNode::initNode(std::vector parameters) { - std::vector parameters; - // Set cost_scaling_factor parameter to 1.0 for inflation layer - parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0)); - parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius)); - auto options = rclcpp::NodeOptions(); options.parameter_overrides(parameters); @@ -195,6 +191,16 @@ void TestNode::initNode(double inflation_radius) node_->declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); } +void TestNode::initNode(double inflation_radius) +{ + std::vector parameters; + // Set cost_scaling_factor parameter to 1.0 for inflation layer + parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0)); + parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius)); + + initNode(parameters); +} + TEST_F(TestNode, testAdjacentToObstacleCanStillMove) { initNode(4.1); @@ -206,8 +212,12 @@ TEST_F(TestNode, testAdjacentToObstacleCanStillMove) // circumscribed radius = 3.1 std::vector polygon = setRadii(layers, 2.1, 2.3); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); addObservation(olayer, 0, 0, MAX_Z); @@ -234,8 +244,12 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns) // circumscribed radius = 3.1 std::vector polygon = setRadii(layers, 2.1, 2.3); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); addObservation(olayer, 0, 0, MAX_Z); @@ -246,6 +260,73 @@ TEST_F(TestNode, testInflationShouldNotCreateUnknowns) EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 0u); } +TEST_F(TestNode, testInflationInUnkown) +{ + std::vector parameters; + // Set cost_scaling_factor parameter to 1.0 for inflation layer + parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0)); + parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", 4.1)); + parameters.push_back(rclcpp::Parameter("inflation.inflate_unknown", true)); + + initNode(parameters); + + node_->set_parameter(rclcpp::Parameter("track_unknown_space", true)); + + tf2_ros::Buffer tf(node_->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, true); + layers.resizeMap(9, 9, 1, 0, 0); + + // Footprint with inscribed radius = 2.1 + // circumscribed radius = 3.1 + std::vector polygon = setRadii(layers, 2.1, 2.3); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); + + addObservation(olayer, 4, 4, MAX_Z, 0.0, 0.0, MAX_Z, true, false); + + layers.updateMap(0, 0, 0); + nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); + + // Only the 4 corners of the map should remain unknown + EXPECT_EQ(countValues(*costmap, nav2_costmap_2d::NO_INFORMATION), 4u); +} + +TEST_F(TestNode, testInflationAroundUnkown) +{ + auto inflation_radius = 4.1; + std::vector parameters; + // Set cost_scaling_factor parameter to 1.0 for inflation layer + parameters.push_back(rclcpp::Parameter("inflation.cost_scaling_factor", 1.0)); + parameters.push_back(rclcpp::Parameter("inflation.inflation_radius", inflation_radius)); + parameters.push_back(rclcpp::Parameter("inflation.inflate_around_unknown", true)); + + initNode(parameters); + + node_->set_parameter(rclcpp::Parameter("track_unknown_space", true)); + + tf2_ros::Buffer tf(node_->get_clock()); + nav2_costmap_2d::LayeredCostmap layers("frame", false, false); + layers.resizeMap(10, 10, 1, 0, 0); + + // Footprint with inscribed radius = 2.1 + // circumscribed radius = 3.1 + std::vector polygon = setRadii(layers, 2.1, 2.3); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); + layers.updateMap(0, 0, 0); + + layers.getCostmap()->setCost(4, 4, nav2_costmap_2d::NO_INFORMATION); + ilayer->updateCosts(*layers.getCostmap(), 0, 0, 8, 8); + + validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius); +} + /** * Test for the cost function correctness with a larger range and different values */ @@ -260,8 +341,12 @@ TEST_F(TestNode, testCostFunctionCorrectness) // circumscribed radius = 8.0 std::vector polygon = setRadii(layers, 5.0, 6.25); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); addObservation(olayer, 50, 50, MAX_Z); @@ -331,8 +416,12 @@ TEST_F(TestNode, testInflationOrderCorrectness) // circumscribed radius = 3.1 std::vector polygon = setRadii(layers, 2.1, 2.3); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - nav2_costmap_2d::InflationLayer * ilayer = addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); // Add two diagonal cells, they would induce problems under the @@ -359,9 +448,14 @@ TEST_F(TestNode, testInflation) // circumscribed radius = 3.1 std::vector polygon = setRadii(layers, 1, 1); - auto slayer = addStaticLayer(layers, tf, node_); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr slayer = nullptr; + addStaticLayer(layers, tf, node_, slayer); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); layers.setFootprint(polygon); nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap(); @@ -432,9 +526,15 @@ TEST_F(TestNode, testInflation2) // circumscribed radius = 3.1 std::vector polygon = setRadii(layers, 1, 1); - auto slayer = addStaticLayer(layers, tf, node_); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr slayer = nullptr; + addStaticLayer(layers, tf, node_, slayer); + + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); waitForMap(slayer); @@ -464,8 +564,12 @@ TEST_F(TestNode, testInflation3) // 1 2 3 std::vector polygon = setRadii(layers, 1, 1.75); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); - addInflationLayer(layers, tf, node_); + std::shared_ptr olayer = nullptr; + addObstacleLayer(layers, tf, node_, olayer); + + std::shared_ptr ilayer = nullptr; + addInflationLayer(layers, tf, node_, ilayer); + layers.setFootprint(polygon); // There should be no occupied cells diff --git a/nav2_costmap_2d/test/integration/obstacle_tests.cpp b/nav2_costmap_2d/test/integration/obstacle_tests.cpp index b499e65398..7e1910f5f7 100644 --- a/nav2_costmap_2d/test/integration/obstacle_tests.cpp +++ b/nav2_costmap_2d/test/integration/obstacle_tests.cpp @@ -157,7 +157,7 @@ TEST_F(TestNode, testRaytracing) { nav2_costmap_2d::LayeredCostmap layers("frame", false, false); addStaticLayer(layers, tf, node_); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); + auto olayer = addObstacleLayer(layers, tf, node_); // Add a point at 0, 0, 0 addObservation(olayer, 0.0, 0.0, MAX_Z / 2, 0, 0, MAX_Z / 2); @@ -179,7 +179,7 @@ TEST_F(TestNode, testRaytracing2) { tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); addStaticLayer(layers, tf, node_); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); + auto olayer = addObstacleLayer(layers, tf, node_); // If we print map now, it is 10x10 all value 0 // printMap(*(layers.getCostmap())); @@ -236,7 +236,7 @@ TEST_F(TestNode, testWaveInterference) { // Start with an empty map, no rolling window, tracking unknown nav2_costmap_2d::LayeredCostmap layers("frame", false, true); layers.resizeMap(10, 10, 1, 0, 0); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); + auto olayer = addObstacleLayer(layers, tf, node_); // If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION) // printMap(*(layers.getCostmap())); @@ -265,7 +265,7 @@ TEST_F(TestNode, testZThreshold) { nav2_costmap_2d::LayeredCostmap layers("frame", false, true); layers.resizeMap(10, 10, 1, 0, 0); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); + auto olayer = addObstacleLayer(layers, tf, node_); // A point cloud with 2 points falling in a cell with a non-lethal cost addObservation(olayer, 0.0, 5.0, 0.4); @@ -285,7 +285,7 @@ TEST_F(TestNode, testDynamicObstacles) { nav2_costmap_2d::LayeredCostmap layers("frame", false, false); addStaticLayer(layers, tf, node_); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); + auto olayer = addObstacleLayer(layers, tf, node_); // Add a point cloud and verify its insertion. There should be only one new one addObservation(olayer, 0.0, 0.0); @@ -310,7 +310,7 @@ TEST_F(TestNode, testMultipleAdditions) { nav2_costmap_2d::LayeredCostmap layers("frame", false, false); addStaticLayer(layers, tf, node_); - nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_); + auto olayer = addObstacleLayer(layers, tf, node_); // A point cloud with one point that falls within an existing obstacle addObservation(olayer, 9.5, 0.0); @@ -327,7 +327,9 @@ TEST_F(TestNode, testMultipleAdditions) { TEST_F(TestNode, testRepeatedResets) { tf2_ros::Buffer tf(node_->get_clock()); nav2_costmap_2d::LayeredCostmap layers("frame", false, false); - addStaticLayer(layers, tf, node_); + + std::shared_ptr slayer = nullptr; + addStaticLayer(layers, tf, node_, slayer); // TODO(orduno) Add obstacle layer diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp similarity index 95% rename from nav2_costmap_2d/test/integration/test_collision_checker.cpp rename to nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp index ef58b069df..56baa03a4b 100644 --- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_topic_collision_checker.cpp @@ -18,7 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/collision_checker.hpp" +#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/static_layer.hpp" @@ -123,17 +123,20 @@ class TestCollisionChecker : public nav2_util::LifecycleNode shared_from_this(), footprint_topic); - collision_checker_ = std::make_unique( + collision_checker_ = std::make_unique( *costmap_sub_, *footprint_sub_, *tf_buffer_, get_name(), "map"); layers_ = new nav2_costmap_2d::LayeredCostmap("map", false, false); // Add Static Layer - auto slayer = addStaticLayer(*layers_, *tf_buffer_, shared_from_this()); + std::shared_ptr slayer = nullptr; + addStaticLayer(*layers_, *tf_buffer_, shared_from_this(), slayer); + while (!slayer->isCurrent()) { rclcpp::spin_some(this->get_node_base_interface()); } // Add Inflation Layer - addInflationLayer(*layers_, *tf_buffer_, shared_from_this()); + std::shared_ptr ilayer = nullptr; + addInflationLayer(*layers_, *tf_buffer_, shared_from_this(), ilayer); return nav2_util::CallbackReturn::SUCCESS; } @@ -271,7 +274,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; - std::unique_ptr collision_checker_; + std::unique_ptr collision_checker_; nav2_costmap_2d::LayeredCostmap * layers_{nullptr}; std::string global_frame_; diff --git a/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py b/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py index bc392db9b0..d4b31798f4 100644 --- a/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py +++ b/nav2_costmap_2d/test/test_launch_files/costmap_map_server.launch.py @@ -25,8 +25,8 @@ def generate_launch_description(): return LaunchDescription([ launch_ros.actions.Node( package='nav2_map_server', - node_executable='map_server', - node_name='map_server', + executable='map_server', + name='map_server', output='screen', parameters=[{'yaml_filename': mapFile}]) ]) diff --git a/nav2_costmap_2d/test/unit/CMakeLists.txt b/nav2_costmap_2d/test/unit/CMakeLists.txt index 5a952eaf28..78085141b3 100644 --- a/nav2_costmap_2d/test/unit/CMakeLists.txt +++ b/nav2_costmap_2d/test/unit/CMakeLists.txt @@ -2,3 +2,8 @@ ament_add_gtest(array_parser_test array_parser_test.cpp) target_link_libraries(array_parser_test nav2_costmap_2d_core ) + +ament_add_gtest(collision_footprint_test footprint_collision_checker_test.cpp) +target_link_libraries(collision_footprint_test + nav2_costmap_2d_core +) diff --git a/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp new file mode 100644 index 0000000000..f0e9c30623 --- /dev/null +++ b/nav2_costmap_2d/test/unit/footprint_collision_checker_test.cpp @@ -0,0 +1,153 @@ +// Copyright (c) 2020 Shivang Patel +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "gtest/gtest.h" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" + +TEST(collision_footprint, test_basic) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + geometry_msgs::msg::Point p1; + p1.x = -0.5; + p1.y = 0.0; + geometry_msgs::msg::Point p2; + p2.x = 0.0; + p2.y = 0.5; + geometry_msgs::msg::Point p3; + p3.x = 0.5; + p3.y = 0.0; + geometry_msgs::msg::Point p4; + p4.x = 0.0; + p4.y = -0.5; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, test_point_cost) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.pointCost(50, 50); + + EXPECT_NEAR(value, 0.0, 0.001); +} + +TEST(collision_footprint, test_world_to_map) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 0); + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + unsigned int x, y; + + collision_checker.worldToMap(1.0, 1.0, x, y); + + auto value = collision_checker.pointCost(x, y); + + EXPECT_NEAR(value, 0.0, 0.001); + + costmap_->setCost(50, 50, 200); + collision_checker.worldToMap(5.0, 5.0, x, y); + + EXPECT_NEAR(collision_checker.pointCost(x, y), 200.0, 0.001); +} + +TEST(collision_footprint, test_footprint_at_pose_with_movement) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.1, 0, 0, 254); + + for (unsigned int i = 40; i <= 60; ++i) { + for (unsigned int j = 40; j <= 60; ++j) { + costmap_->setCost(i, j, 0); + } + } + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + EXPECT_NEAR(value, 0.0, 0.001); + + auto up_value = collision_checker.footprintCostAtPose(5.0, 4.9, 0.0, footprint); + EXPECT_NEAR(up_value, 254.0, 0.001); + + auto down_value = collision_checker.footprintCostAtPose(5.0, 5.2, 0.0, footprint); + EXPECT_NEAR(down_value, 254.0, 0.001); +} + +TEST(collision_footprint, test_point_and_line_cost) +{ + std::shared_ptr costmap_ = + std::make_shared(100, 100, 0.10000, 0, 0.0, 0.0); + + costmap_->setCost(62, 50, 254); + costmap_->setCost(39, 60, 254); + + geometry_msgs::msg::Point p1; + p1.x = -1.0; + p1.y = 1.0; + geometry_msgs::msg::Point p2; + p2.x = 1.0; + p2.y = 1.0; + geometry_msgs::msg::Point p3; + p3.x = 1.0; + p3.y = -1.0; + geometry_msgs::msg::Point p4; + p4.x = -1.0; + p4.y = -1.0; + + nav2_costmap_2d::Footprint footprint = {p1, p2, p3, p4}; + + nav2_costmap_2d::FootprintCollisionChecker collision_checker(costmap_); + + auto value = collision_checker.footprintCostAtPose(5.0, 5.0, 0.0, footprint); + EXPECT_NEAR(value, 0.0, 0.001); + + auto left_value = collision_checker.footprintCostAtPose(4.9, 5.0, 0.0, footprint); + EXPECT_NEAR(left_value, 254.0, 0.001); + + auto right_value = collision_checker.footprintCostAtPose(5.2, 5.0, 0.0, footprint); + EXPECT_NEAR(right_value, 254.0, 0.001); +} diff --git a/nav2_dwb_controller/costmap_queue/CMakeLists.txt b/nav2_dwb_controller/costmap_queue/CMakeLists.txt index 489e73fa7b..4641d0b547 100644 --- a/nav2_dwb_controller/costmap_queue/CMakeLists.txt +++ b/nav2_dwb_controller/costmap_queue/CMakeLists.txt @@ -45,7 +45,7 @@ endif() install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_dwb_controller/dwb_core/CMakeLists.txt b/nav2_dwb_controller/dwb_core/CMakeLists.txt index cec51d5f9b..3b58ccfeb3 100644 --- a/nav2_dwb_controller/dwb_core/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_core/CMakeLists.txt @@ -58,7 +58,7 @@ ament_target_dependencies(dwb_core install(TARGETS dwb_core ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index aca53d1f53..011ea53042 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -74,6 +74,7 @@ void DWBLocalPlanner::configure( tf_ = tf; dwb_plugin_name_ = name; declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".critics"); + declare_parameter_if_not_declared(node_, dwb_plugin_name_ + ".default_critic_namespaces"); declare_parameter_if_not_declared( node_, dwb_plugin_name_ + ".prune_plan", rclcpp::ParameterValue(true)); diff --git a/nav2_dwb_controller/dwb_core/src/publisher.cpp b/nav2_dwb_controller/dwb_core/src/publisher.cpp index f4e562ff29..7f7ceb62c3 100644 --- a/nav2_dwb_controller/dwb_core/src/publisher.cpp +++ b/nav2_dwb_controller/dwb_core/src/publisher.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include "nav_2d_utils/conversions.hpp" #include "nav2_util/node_utils.hpp" @@ -97,7 +98,7 @@ DWBPublisher::on_configure() marker_pub_ = node_->create_publisher("marker", 1); cost_grid_pc_pub_ = node_->create_publisher("cost_cloud", 1); - double marker_lifetime; + double marker_lifetime = 0.0; node_->get_parameter(plugin_name_ + ".marker_lifetime", marker_lifetime); marker_lifetime_ = rclcpp::Duration::from_seconds(marker_lifetime); @@ -151,7 +152,8 @@ DWBPublisher::publishEvaluation(std::shared_ptrpublish(*results); + auto msg = std::make_unique(*results); + eval_pub_->publish(std::move(msg)); } publishTrajectories(*results); @@ -163,7 +165,7 @@ DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & res if (node_->count_subscribers(marker_pub_->get_topic_name()) < 1) {return;} if (!publish_trajectories_) {return;} - visualization_msgs::msg::MarkerArray ma; + auto ma = std::make_unique(); visualization_msgs::msg::Marker m; if (results.twists.size() == 0) {return;} @@ -216,9 +218,9 @@ DWBPublisher::publishTrajectories(const dwb_msgs::msg::LocalPlanEvaluation & res pt.z = 0; m.points.push_back(pt); } - ma.markers.push_back(m); + ma->markers.push_back(m); } - marker_pub_->publish(ma); + marker_pub_->publish(std::move(ma)); } void @@ -228,10 +230,13 @@ DWBPublisher::publishLocalPlan( { if (!publish_local_plan_) {return;} - nav_msgs::msg::Path path = - nav_2d_utils::poses2DToPath(traj.poses, header.frame_id, header.stamp); - if (node_->count_subscribers(local_pub_->get_topic_name()) < 1) { - local_pub_->publish(path); + auto path = + std::make_unique( + nav_2d_utils::poses2DToPath( + traj.poses, header.frame_id, + header.stamp)); + if (node_->count_subscribers(local_pub_->get_topic_name()) > 0) { + local_pub_->publish(std::move(path)); } } @@ -244,21 +249,21 @@ DWBPublisher::publishCostGrid( if (!publish_cost_grid_pc_) {return;} - sensor_msgs::msg::PointCloud cost_grid_pc; - cost_grid_pc.header.frame_id = costmap_ros->getGlobalFrameID(); - cost_grid_pc.header.stamp = node_->now(); + auto cost_grid_pc = std::make_unique(); + cost_grid_pc->header.frame_id = costmap_ros->getGlobalFrameID(); + cost_grid_pc->header.stamp = node_->now(); nav2_costmap_2d::Costmap2D * costmap = costmap_ros->getCostmap(); double x_coord, y_coord; unsigned int size_x = costmap->getSizeInCellsX(); unsigned int size_y = costmap->getSizeInCellsY(); - cost_grid_pc.points.resize(size_x * size_y); + cost_grid_pc->points.resize(size_x * size_y); unsigned int i = 0; for (unsigned int cy = 0; cy < size_y; cy++) { for (unsigned int cx = 0; cx < size_x; cx++) { costmap->mapToWorld(cx, cy, x_coord, y_coord); - cost_grid_pc.points[i].x = x_coord; - cost_grid_pc.points[i].y = y_coord; + cost_grid_pc->points[i].x = x_coord; + cost_grid_pc->points[i].y = y_coord; i++; } } @@ -268,23 +273,23 @@ DWBPublisher::publishCostGrid( totals.values.resize(size_x * size_y, 0.0); for (TrajectoryCritic::Ptr critic : critics) { - unsigned int channel_index = cost_grid_pc.channels.size(); - critic->addCriticVisualization(cost_grid_pc); - if (channel_index == cost_grid_pc.channels.size()) { + unsigned int channel_index = cost_grid_pc->channels.size(); + critic->addCriticVisualization(*cost_grid_pc); + if (channel_index == cost_grid_pc->channels.size()) { // No channels were added, so skip to next critic continue; } double scale = critic->getScale(); for (i = 0; i < size_x * size_y; i++) { - totals.values[i] += cost_grid_pc.channels[channel_index].values[i] * scale; + totals.values[i] += cost_grid_pc->channels[channel_index].values[i] * scale; } } - cost_grid_pc.channels.push_back(totals); + cost_grid_pc->channels.push_back(totals); // TODO(crdelsey): convert pc to pc2 // sensor_msgs::msg::PointCloud2 cost_grid_pc2; // convertPointCloudToPointCloud2(cost_grid_pc, cost_grid_pc2); - cost_grid_pc_pub_->publish(cost_grid_pc); + cost_grid_pc_pub_->publish(std::move(cost_grid_pc)); } void @@ -312,8 +317,8 @@ DWBPublisher::publishGenericPlan( { if (node_->count_subscribers(pub.get_topic_name()) < 1) {return;} if (!flag) {return;} - nav_msgs::msg::Path path = nav_2d_utils::pathToPath(plan); - pub.publish(path); + auto path = std::make_unique(nav_2d_utils::pathToPath(plan)); + pub.publish(std::move(path)); } } // namespace dwb_core diff --git a/nav2_dwb_controller/dwb_critics/CMakeLists.txt b/nav2_dwb_controller/dwb_critics/CMakeLists.txt index b892eb7d28..7b7cb8c70d 100644 --- a/nav2_dwb_controller/dwb_critics/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_critics/CMakeLists.txt @@ -60,7 +60,7 @@ ament_target_dependencies(${PROJECT_NAME} install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_dwb_controller/dwb_critics/src/twirling.cpp b/nav2_dwb_controller/dwb_critics/src/twirling.cpp index 2653aa65d5..3615e6f036 100644 --- a/nav2_dwb_controller/dwb_critics/src/twirling.cpp +++ b/nav2_dwb_controller/dwb_critics/src/twirling.cpp @@ -39,10 +39,6 @@ namespace dwb_critics { void TwirlingCritic::onInit() { - if (!nh_->has_parameter(dwb_plugin_name_ + "." + name_ + ".scale")) { - nh_->declare_parameter(dwb_plugin_name_ + "." + name_ + ".scale", rclcpp::ParameterValue(0.0)); - } - // Scale is set to 0 by default, so if it was not set otherwise, set to 0 nh_->get_parameter(dwb_plugin_name_ + "." + name_ + ".scale", scale_); } diff --git a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt index 64348944b5..ea4d7868ea 100644 --- a/nav2_dwb_controller/dwb_plugins/CMakeLists.txt +++ b/nav2_dwb_controller/dwb_plugins/CMakeLists.txt @@ -63,7 +63,7 @@ endif() install(TARGETS simple_goal_checker stopped_goal_checker standard_traj_generator ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp index 5ff9de0a28..577657b9a0 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp @@ -45,14 +45,12 @@ namespace dwb_plugins { /** - * @class KinematicParameters - * @brief A class containing one representation of the robot's kinematics + * @struct KinematicParameters + * @brief A struct containing one representation of the robot's kinematics */ -class KinematicParameters +struct KinematicParameters { -public: - KinematicParameters(); - void initialize(const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name); + friend class KinematicsHandler; inline double getMinX() {return min_vel_x_;} inline double getMaxX() {return max_vel_x_;} @@ -90,8 +88,6 @@ class KinematicParameters */ bool isValidSpeed(double x, double y, double theta); - using Ptr = std::shared_ptr; - protected: // For parameter descriptions, see cfg/KinematicParams.cfg double min_vel_x_{0}; @@ -112,14 +108,33 @@ class KinematicParameters // Cached square values of min_speed_xy and max_speed_xy double min_speed_xy_sq_{0}; double max_speed_xy_sq_{0}; +}; + +/** + * @class KinematicsHandler + * @brief A class managing the representation of the robot's kinematics + */ +class KinematicsHandler +{ +public: + KinematicsHandler(); + ~KinematicsHandler(); + void initialize(const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name); + + inline KinematicParameters getKinematics() {return *kinematics_.load();} + + bool isValidSpeed(double x, double y, double theta); + + using Ptr = std::shared_ptr; - void reconfigureCB(); +protected: + std::atomic kinematics_; // Subscription for parameter change rclcpp::AsyncParametersClient::SharedPtr parameters_client_; rclcpp::Subscription::SharedPtr parameter_event_sub_; void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); - + void update_kinematics(KinematicParameters kinematics); std::string plugin_name_; }; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp index a308e3543d..ec380674d4 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp @@ -113,7 +113,7 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator */ virtual std::vector getTimeSteps(const nav_2d_msgs::msg::Twist2D & cmd_vel); - KinematicParameters::Ptr kinematics_; + KinematicsHandler::Ptr kinematics_handler_; std::shared_ptr velocity_iterator_; double sim_time_; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp index e0acbfb2a5..55a663fddb 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/velocity_iterator.hpp @@ -51,7 +51,7 @@ class VelocityIterator virtual ~VelocityIterator() {} virtual void initialize( const nav2_util::LifecycleNode::SharedPtr & nh, - KinematicParameters::Ptr kinematics, + KinematicsHandler::Ptr kinematics, const std::string & plugin_name) = 0; virtual void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) = 0; virtual bool hasMoreTwists() = 0; diff --git a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp index 3b448479e8..68ac9405c2 100644 --- a/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp +++ b/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/xy_theta_iterator.hpp @@ -48,10 +48,10 @@ class XYThetaIterator : public VelocityIterator { public: XYThetaIterator() - : kinematics_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {} + : kinematics_handler_(nullptr), x_it_(nullptr), y_it_(nullptr), th_it_(nullptr) {} void initialize( const nav2_util::LifecycleNode::SharedPtr & nh, - KinematicParameters::Ptr kinematics, + KinematicsHandler::Ptr kinematics, const std::string & plugin_name) override; void startNewIteration(const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) override; bool hasMoreTwists() override; @@ -61,7 +61,7 @@ class XYThetaIterator : public VelocityIterator virtual bool isValidVelocity(); void iterateToValidVelocity(); int vx_samples_, vy_samples_, vtheta_samples_; - KinematicParameters::Ptr kinematics_; + KinematicsHandler::Ptr kinematics_handler_; std::shared_ptr x_it_, y_it_, th_it_; }; diff --git a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp index 74c415e7eb..4b4747cd67 100644 --- a/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp @@ -52,11 +52,27 @@ using std::placeholders::_1; namespace dwb_plugins { -KinematicParameters::KinematicParameters() +bool KinematicParameters::isValidSpeed(double x, double y, double theta) +{ + double vmag_sq = x * x + y * y; + if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) {return false;} + if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ && + min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) {return false;} + if (vmag_sq == 0.0 && theta == 0.0) {return false;} + return true; +} + +KinematicsHandler::KinematicsHandler() +{ + kinematics_.store(new KinematicParameters); +} + +KinematicsHandler::~KinematicsHandler() { + delete kinematics_.load(); } -void KinematicParameters::initialize( +void KinematicsHandler::initialize( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string & plugin_name) { @@ -94,20 +110,22 @@ void KinematicParameters::initialize( nh, plugin_name + ".decel_lim_theta", rclcpp::ParameterValue(0.0)); - nh->get_parameter(plugin_name + ".min_vel_x", min_vel_x_); - nh->get_parameter(plugin_name + ".min_vel_y", min_vel_y_); - nh->get_parameter(plugin_name + ".max_vel_x", max_vel_x_); - nh->get_parameter(plugin_name + ".max_vel_y", max_vel_y_); - nh->get_parameter(plugin_name + ".max_vel_theta", max_vel_theta_); - nh->get_parameter(plugin_name + ".min_speed_xy", min_speed_xy_); - nh->get_parameter(plugin_name + ".max_speed_xy", max_speed_xy_); - nh->get_parameter(plugin_name + ".min_speed_theta", min_speed_theta_); - nh->get_parameter(plugin_name + ".acc_lim_x", acc_lim_x_); - nh->get_parameter(plugin_name + ".acc_lim_y", acc_lim_y_); - nh->get_parameter(plugin_name + ".acc_lim_theta", acc_lim_theta_); - nh->get_parameter(plugin_name + ".decel_lim_x", decel_lim_x_); - nh->get_parameter(plugin_name + ".decel_lim_y", decel_lim_y_); - nh->get_parameter(plugin_name + ".decel_lim_theta", decel_lim_theta_); + KinematicParameters kinematics; + + nh->get_parameter(plugin_name + ".min_vel_x", kinematics.min_vel_x_); + nh->get_parameter(plugin_name + ".min_vel_y", kinematics.min_vel_y_); + nh->get_parameter(plugin_name + ".max_vel_x", kinematics.max_vel_x_); + nh->get_parameter(plugin_name + ".max_vel_y", kinematics.max_vel_y_); + nh->get_parameter(plugin_name + ".max_vel_theta", kinematics.max_vel_theta_); + nh->get_parameter(plugin_name + ".min_speed_xy", kinematics.min_speed_xy_); + nh->get_parameter(plugin_name + ".max_speed_xy", kinematics.max_speed_xy_); + nh->get_parameter(plugin_name + ".min_speed_theta", kinematics.min_speed_theta_); + nh->get_parameter(plugin_name + ".acc_lim_x", kinematics.acc_lim_x_); + nh->get_parameter(plugin_name + ".acc_lim_y", kinematics.acc_lim_y_); + nh->get_parameter(plugin_name + ".acc_lim_theta", kinematics.acc_lim_theta_); + nh->get_parameter(plugin_name + ".decel_lim_x", kinematics.decel_lim_x_); + nh->get_parameter(plugin_name + ".decel_lim_y", kinematics.decel_lim_y_); + nh->get_parameter(plugin_name + ".decel_lim_theta", kinematics.decel_lim_theta_); // Setup callback for changes to parameters. parameters_client_ = std::make_shared( @@ -117,26 +135,25 @@ void KinematicParameters::initialize( nh->get_node_services_interface()); parameter_event_sub_ = parameters_client_->on_parameter_event( - std::bind(&KinematicParameters::on_parameter_event_callback, this, _1)); + std::bind(&KinematicsHandler::on_parameter_event_callback, this, _1)); - min_speed_xy_sq_ = min_speed_xy_ * min_speed_xy_; - max_speed_xy_sq_ = max_speed_xy_ * max_speed_xy_; + kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; + kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; + + update_kinematics(kinematics); } -bool KinematicParameters::isValidSpeed(double x, double y, double theta) +bool KinematicsHandler::isValidSpeed(double x, double y, double theta) { - double vmag_sq = x * x + y * y; - if (max_speed_xy_ >= 0.0 && vmag_sq > max_speed_xy_sq_ + EPSILON) {return false;} - if (min_speed_xy_ >= 0.0 && vmag_sq + EPSILON < min_speed_xy_sq_ && - min_speed_theta_ >= 0.0 && fabs(theta) + EPSILON < min_speed_theta_) {return false;} - if (vmag_sq == 0.0 && theta == 0.0) {return false;} - return true; + return kinematics_.load()->isValidSpeed(x, y, theta); } void -KinematicParameters::on_parameter_event_callback( +KinematicsHandler::on_parameter_event_callback( const rcl_interfaces::msg::ParameterEvent::SharedPtr event) { + KinematicParameters kinematics(*kinematics_.load()); + for (auto & changed_parameter : event->changed_parameters) { const auto & type = changed_parameter.value.type; const auto & name = changed_parameter.name; @@ -144,36 +161,45 @@ KinematicParameters::on_parameter_event_callback( if (type == ParameterType::PARAMETER_DOUBLE) { if (name == plugin_name_ + ".min_vel_x") { - min_vel_x_ = value.double_value; + kinematics.min_vel_x_ = value.double_value; } else if (name == plugin_name_ + ".min_vel_y") { - min_vel_y_ = value.double_value; + kinematics.min_vel_y_ = value.double_value; } else if (name == plugin_name_ + ".max_vel_x") { - max_vel_x_ = value.double_value; + kinematics.max_vel_x_ = value.double_value; } else if (name == plugin_name_ + ".max_vel_y") { - max_vel_y_ = value.double_value; + kinematics.max_vel_y_ = value.double_value; } else if (name == plugin_name_ + ".max_vel_theta") { - max_vel_theta_ = value.double_value; + kinematics.max_vel_theta_ = value.double_value; } else if (name == plugin_name_ + ".min_speed_xy") { - min_speed_xy_ = value.double_value; + kinematics.min_speed_xy_ = value.double_value; + kinematics.min_speed_xy_sq_ = kinematics.min_speed_xy_ * kinematics.min_speed_xy_; } else if (name == plugin_name_ + ".max_speed_xy") { - max_speed_xy_ = value.double_value; + kinematics.max_speed_xy_ = value.double_value; } else if (name == plugin_name_ + ".min_speed_theta") { - min_speed_theta_ = value.double_value; + kinematics.min_speed_theta_ = value.double_value; + kinematics.max_speed_xy_sq_ = kinematics.max_speed_xy_ * kinematics.max_speed_xy_; } else if (name == plugin_name_ + ".acc_lim_x") { - acc_lim_x_ = value.double_value; + kinematics.acc_lim_x_ = value.double_value; } else if (name == plugin_name_ + ".acc_lim_y") { - acc_lim_y_ = value.double_value; + kinematics.acc_lim_y_ = value.double_value; } else if (name == plugin_name_ + ".acc_lim_theta") { - acc_lim_theta_ = value.double_value; + kinematics.acc_lim_theta_ = value.double_value; } else if (name == plugin_name_ + ".decel_lim_x") { - decel_lim_x_ = value.double_value; + kinematics.decel_lim_x_ = value.double_value; } else if (name == plugin_name_ + ".decel_lim_y") { - decel_lim_y_ = value.double_value; + kinematics.decel_lim_y_ = value.double_value; } else if (name == plugin_name_ + ".decel_lim_theta") { - decel_lim_theta_ = value.double_value; + kinematics.decel_lim_theta_ = value.double_value; } } } + update_kinematics(kinematics); +} + +void KinematicsHandler::update_kinematics(KinematicParameters kinematics) +{ + delete kinematics_.load(); + kinematics_.store(new KinematicParameters(kinematics)); } } // namespace dwb_plugins diff --git a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp index 8c2dd783e0..3b7e168579 100644 --- a/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/standard_traj_generator.cpp @@ -53,8 +53,8 @@ void StandardTrajectoryGenerator::initialize( const std::string & plugin_name) { plugin_name_ = plugin_name; - kinematics_ = std::make_shared(); - kinematics_->initialize(nh, plugin_name_); + kinematics_handler_ = std::make_shared(); + kinematics_handler_->initialize(nh, plugin_name_); initializeIterator(nh); nav2_util::declare_parameter_if_not_declared( @@ -97,7 +97,7 @@ void StandardTrajectoryGenerator::initializeIterator( const nav2_util::LifecycleNode::SharedPtr & nh) { velocity_iterator_ = std::make_shared(); - velocity_iterator_->initialize(nh, kinematics_, plugin_name_); + velocity_iterator_->initialize(nh, kinematics_handler_, plugin_name_); } void StandardTrajectoryGenerator::startNewIteration( @@ -185,16 +185,17 @@ nav_2d_msgs::msg::Twist2D StandardTrajectoryGenerator::computeNewVelocity( const nav_2d_msgs::msg::Twist2D & cmd_vel, const nav_2d_msgs::msg::Twist2D & start_vel, const double dt) { + KinematicParameters kinematics = kinematics_handler_->getKinematics(); nav_2d_msgs::msg::Twist2D new_vel; new_vel.x = projectVelocity( - start_vel.x, kinematics_->getAccX(), - kinematics_->getDecelX(), dt, cmd_vel.x); + start_vel.x, kinematics.getAccX(), + kinematics.getDecelX(), dt, cmd_vel.x); new_vel.y = projectVelocity( - start_vel.y, kinematics_->getAccY(), - kinematics_->getDecelY(), dt, cmd_vel.y); + start_vel.y, kinematics.getAccY(), + kinematics.getDecelY(), dt, cmd_vel.y); new_vel.theta = projectVelocity( start_vel.theta, - kinematics_->getAccTheta(), kinematics_->getDecelTheta(), + kinematics.getAccTheta(), kinematics.getDecelTheta(), dt, cmd_vel.theta); return new_vel; } diff --git a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp index db686092b3..c94948e896 100644 --- a/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/stopped_goal_checker.cpp @@ -39,6 +39,7 @@ #include "pluginlib/class_list_macros.hpp" #include "nav2_util/node_utils.hpp" +using std::hypot; using std::fabs; namespace dwb_plugins @@ -74,9 +75,9 @@ bool StoppedGoalChecker::isGoalReached( if (!ret) { return ret; } + return fabs(velocity.angular.z) <= rot_stopped_velocity_ && - fabs(velocity.linear.x) <= trans_stopped_velocity_ && - fabs(velocity.linear.y) <= trans_stopped_velocity_; + hypot(velocity.linear.x, velocity.linear.y) <= trans_stopped_velocity_; } } // namespace dwb_plugins diff --git a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp index 873f303629..0474e167be 100644 --- a/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp +++ b/nav2_dwb_controller/dwb_plugins/src/xy_theta_iterator.cpp @@ -42,10 +42,10 @@ namespace dwb_plugins { void XYThetaIterator::initialize( const nav2_util::LifecycleNode::SharedPtr & nh, - KinematicParameters::Ptr kinematics, + KinematicsHandler::Ptr kinematics, const std::string & plugin_name) { - kinematics_ = kinematics; + kinematics_handler_ = kinematics; nav2_util::declare_parameter_if_not_declared( nh, @@ -66,18 +66,19 @@ void XYThetaIterator::startNewIteration( const nav_2d_msgs::msg::Twist2D & current_velocity, double dt) { + KinematicParameters kinematics = kinematics_handler_->getKinematics(); x_it_ = std::make_shared( current_velocity.x, - kinematics_->getMinX(), kinematics_->getMaxX(), - kinematics_->getAccX(), kinematics_->getDecelX(), dt, vx_samples_); + kinematics.getMinX(), kinematics.getMaxX(), + kinematics.getAccX(), kinematics.getDecelX(), dt, vx_samples_); y_it_ = std::make_shared( current_velocity.y, - kinematics_->getMinY(), kinematics_->getMaxY(), - kinematics_->getAccY(), kinematics_->getDecelY(), dt, vy_samples_); + kinematics.getMinY(), kinematics.getMaxY(), + kinematics.getAccY(), kinematics.getDecelY(), dt, vy_samples_); th_it_ = std::make_shared( current_velocity.theta, - kinematics_->getMinTheta(), kinematics_->getMaxTheta(), - kinematics_->getAccTheta(), kinematics_->getDecelTheta(), + kinematics.getMinTheta(), kinematics.getMaxTheta(), + kinematics.getAccTheta(), kinematics.getDecelTheta(), dt, vtheta_samples_); if (!isValidVelocity()) { iterateToValidVelocity(); @@ -86,7 +87,7 @@ void XYThetaIterator::startNewIteration( bool XYThetaIterator::isValidVelocity() { - return kinematics_->isValidSpeed( + return kinematics_handler_->isValidSpeed( x_it_->getVelocity(), y_it_->getVelocity(), th_it_->getVelocity()); } diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index 6dbe3d3019..06bb2a5a19 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -46,7 +46,7 @@ ament_target_dependencies(path_ops install(TARGETS conversions path_ops ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ DESTINATION include/ diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp index 3d0ae06774..91f08b1bb5 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp @@ -65,6 +65,9 @@ class OdomSubscriber nav2_util::LifecycleNode::SharedPtr nh, std::string default_topic = "odom") { + nav2_util::declare_parameter_if_not_declared( + nh, "odom_topic", rclcpp::ParameterValue(default_topic)); + std::string odom_topic; nh->get_parameter_or("odom_topic", odom_topic, default_topic); odom_sub_ = diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index a8fad52165..a7a2f010ee 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -72,7 +72,7 @@ param_t searchAndGetParam( // nh->param(resolved_name, value, default_value); // return value; // } - param_t value; + param_t value = 0; nav2_util::declare_parameter_if_not_declared( nh, param_name, rclcpp::ParameterValue(default_value)); @@ -147,7 +147,7 @@ void moveParameter( const nav2_util::LifecycleNode::SharedPtr & nh, std::string old_name, std::string current_name, param_t default_value, bool should_delete = true) { - param_t value; + param_t value = 0; if (nh->get_parameter(current_name, value)) { if (should_delete) {nh->undeclare_parameter(old_name);} return; diff --git a/nav2_lifecycle_manager/CMakeLists.txt b/nav2_lifecycle_manager/CMakeLists.txt index c68e636f29..ff06d31cc5 100644 --- a/nav2_lifecycle_manager/CMakeLists.txt +++ b/nav2_lifecycle_manager/CMakeLists.txt @@ -57,10 +57,14 @@ ament_target_dependencies(lifecycle_manager ) install(TARGETS - lifecycle_manager ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS + lifecycle_manager RUNTIME DESTINATION lib/${PROJECT_NAME} ) @@ -69,6 +73,11 @@ install(DIRECTORY include/ DESTINATION include/) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + add_subdirectory(test) endif() ament_export_include_directories(include) diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index a6e300e77d..4a65c05b8a 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "nav2_util/geometry_utils.hpp" @@ -110,19 +111,19 @@ void LifecycleManagerClient::set_initial_pose(double x, double y, double theta) { const double PI = 3.141592653589793238463; - geometry_msgs::msg::PoseWithCovarianceStamped pose; - - pose.header.frame_id = "map"; - pose.header.stamp = node_->now(); - pose.pose.pose.position.x = x; - pose.pose.pose.position.y = y; - pose.pose.pose.position.z = 0.0; - pose.pose.pose.orientation = orientationAroundZAxis(theta); - pose.pose.covariance[6 * 0 + 0] = 0.5 * 0.5; - pose.pose.covariance[6 * 1 + 1] = 0.5 * 0.5; - pose.pose.covariance[6 * 5 + 5] = PI / 12.0 * PI / 12.0; - - initial_pose_publisher_->publish(pose); + auto pose = std::make_unique(); + + pose->header.frame_id = "map"; + pose->header.stamp = node_->now(); + pose->pose.pose.position.x = x; + pose->pose.pose.position.y = y; + pose->pose.pose.position.z = 0.0; + pose->pose.pose.orientation = orientationAroundZAxis(theta); + pose->pose.covariance[6 * 0 + 0] = 0.5 * 0.5; + pose->pose.covariance[6 * 1 + 1] = 0.5 * 0.5; + pose->pose.covariance[6 * 5 + 5] = PI / 12.0 * PI / 12.0; + + initial_pose_publisher_->publish(std::move(pose)); } bool diff --git a/nav2_lifecycle_manager/test/CMakeLists.txt b/nav2_lifecycle_manager/test/CMakeLists.txt new file mode 100644 index 0000000000..e931a5ce19 --- /dev/null +++ b/nav2_lifecycle_manager/test/CMakeLists.txt @@ -0,0 +1,20 @@ +ament_add_gtest_executable(test_lifecycle_gtest + test_lifecycle_manager.cpp +) + +target_link_libraries(test_lifecycle_gtest + ${library_name} +) + +ament_target_dependencies(test_lifecycle_gtest + ${dependencies} +) + +ament_add_test(test_lifecycle + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/launch_lifecycle_test.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_EXECUTABLE=$ +) diff --git a/nav2_lifecycle_manager/test/launch_lifecycle_test.py b/nav2_lifecycle_manager/test/launch_lifecycle_test.py new file mode 100755 index 0000000000..822a442357 --- /dev/null +++ b/nav2_lifecycle_manager/test/launch_lifecycle_test.py @@ -0,0 +1,57 @@ +#! /usr/bin/env python3 +# Copyright (c) 2020 Shivang Patel +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_test', + output='screen', + parameters=[{'use_sim_time': False}, + {'autostart': False}, + {'node_names': ['lifecycle_node_test']}]), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_lifecycle_node_gtest', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp new file mode 100644 index 0000000000..983a34ee64 --- /dev/null +++ b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp @@ -0,0 +1,126 @@ +// Copyright (c) 2020 Shivang Patel +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/node_thread.hpp" +#include "nav2_lifecycle_manager/lifecycle_manager_client.hpp" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class LifecycleNodeTest : public rclcpp_lifecycle::LifecycleNode +{ +public: + LifecycleNodeTest() + : rclcpp_lifecycle::LifecycleNode("lifecycle_node_test") {} + + CallbackReturn on_configure(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Configured!"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Activated!"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Deactivated!"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_cleanup(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Cleanup!"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is Shutdown!"); + return CallbackReturn::SUCCESS; + } + + CallbackReturn on_error(const rclcpp_lifecycle::State & /*state*/) override + { + RCLCPP_INFO(get_logger(), "Lifecycle Test node is encountered an error!"); + return CallbackReturn::SUCCESS; + } +}; + +class LifecycleClientTestFixture +{ +public: + LifecycleClientTestFixture() + { + lf_node_ = std::make_shared(); + lf_thread_ = std::make_unique(lf_node_->get_node_base_interface()); + } + +private: + std::shared_ptr lf_node_; + std::unique_ptr lf_thread_; +}; + +TEST(LifecycleClientTest, BasicTest) +{ + LifecycleClientTestFixture fix; + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test"); + EXPECT_TRUE(client.startup()); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::TIMEOUT, + client.is_active(std::chrono::nanoseconds(1000))); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::ACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::ACTIVE, + client.is_active()); + EXPECT_TRUE(client.pause()); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::INACTIVE, + client.is_active(std::chrono::nanoseconds(1000000000))); + EXPECT_TRUE(client.resume()); + EXPECT_TRUE(client.reset()); + EXPECT_TRUE(client.shutdown()); +} + +TEST(LifecycleClientTest, WithoutFixture) +{ + nav2_lifecycle_manager::LifecycleManagerClient client("lifecycle_manager_test"); + EXPECT_EQ( + nav2_lifecycle_manager::SystemStatus::TIMEOUT, + client.is_active(std::chrono::nanoseconds(1000))); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt index f34427d613..9243742486 100644 --- a/nav2_map_server/CMakeLists.txt +++ b/nav2_map_server/CMakeLists.txt @@ -21,21 +21,36 @@ include_directories(include) set(map_server_executable map_server) -set(map_saver_executable map_saver) +set(map_saver_cli_executable map_saver_cli) + +set(map_saver_server_executable map_saver_server) add_executable(${map_server_executable} - src/main.cpp) + src/map_server/main.cpp) + +add_executable(${map_saver_cli_executable} + src/map_saver/main_cli.cpp) -add_executable(${map_saver_executable} - src/map_saver_cli.cpp) +add_executable(${map_saver_server_executable} + src/map_saver/main_server.cpp) + +set(map_io_library_name map_io) set(library_name ${map_server_executable}_core) +add_library(${map_io_library_name} SHARED + src/map_mode.cpp + src/map_io.cpp) + add_library(${library_name} SHARED - src/occ_grid_loader.cpp - src/map_server.cpp - src/map_saver.cpp - src/map_mode.cpp) + src/map_server/map_server.cpp + src/map_saver/map_saver.cpp) + +set(map_io_dependencies + yaml_cpp_vendor + nav_msgs + nav2_util + tf2) set(map_server_dependencies rclcpp @@ -44,43 +59,72 @@ set(map_server_dependencies nav2_msgs yaml_cpp_vendor std_msgs - tf2 nav2_util) set(map_saver_dependencies rclcpp nav_msgs - tf2) + nav2_msgs + nav2_util) ament_target_dependencies(${map_server_executable} ${map_server_dependencies}) -ament_target_dependencies(${map_saver_executable} +ament_target_dependencies(${map_saver_cli_executable} + ${map_saver_dependencies}) + +ament_target_dependencies(${map_saver_server_executable} ${map_saver_dependencies}) ament_target_dependencies(${library_name} ${map_server_dependencies}) +ament_target_dependencies(${map_io_library_name} + ${map_io_dependencies}) + +target_link_libraries(${library_name} + ${map_io_library_name}) + target_link_libraries(${map_server_executable} ${library_name}) -target_link_libraries(${map_saver_executable} +if(WIN32) + target_compile_definitions(${map_server_executable} PRIVATE + YAML_CPP_DLL) +endif() + +target_link_libraries(${map_saver_cli_executable} + ${library_name}) + +target_link_libraries(${map_saver_server_executable} ${library_name}) -target_include_directories(${library_name} SYSTEM PRIVATE +target_include_directories(${map_io_library_name} SYSTEM PRIVATE ${GRAPHICSMAGICKCPP_INCLUDE_DIRS}) -target_link_libraries(${library_name} +target_link_libraries(${map_io_library_name} ${GRAPHICSMAGICKCPP_LIBRARIES}) -install(TARGETS ${map_server_executable} ${library_name} ${map_saver_executable} +if(WIN32) + target_compile_definitions(${map_io_library_name} PRIVATE + YAML_CPP_DLL) +endif() + +install(TARGETS + ${library_name} ${map_io_library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(TARGETS + ${map_server_executable} ${map_saver_cli_executable} ${map_saver_server_executable} RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -94,5 +138,6 @@ endif() ament_export_include_directories(include) ament_export_libraries( ${library_name} + ${map_io_library_name} ) ament_package() diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index d0a11bb991..8a81a2ddc8 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -8,21 +8,39 @@ service interfaces. While the nav2 map server provides the same general function as the nav1 map server, the new code has some changes to accomodate ROS2 as well as some architectural improvements. -In addition, there is now a new "load_map" service which can be used to dynamically load a map. +In addition, there is now two new "load_map" and "save_map" services which can be used to +dynamically load and save a map. ### Architecture In contrast to the ROS1 navigation map server, the nav2 map server will support a variety of map types, and thus some aspects of the original code have been refactored to support -this new extensible framework. In particular, there is now a `MapLoader` abstract base class -and type-specific map loaders which derive from this class. There is currently one such -derived class, the `OccGridLoader`, which converts an input image to an OccupancyGrid and -makes this available via topic and service interfaces. The `MapServer` class is a ROS2 node -that uses the appropriate loader, based on an input parameter. +this new extensible framework. -### Command-line arguments, ROS2 Node Parameters, and YAML files +Currently map server divides into tree parts: -The Map Server is a composable ROS2 node. By default, there is a map_server executable that +- `map_server` +- `map_saver` +- `map_io` library + +`map_server` is responsible for loading the map from a file through command-line interface +or by using serice requests. + +`map_saver` saves the map into a file. Like `map_server`, it has an ability to save the map from +command-line or by calling a service. + +`map_io` - is a map input-output library. The library is designed to be an object-independent +in order to allow easily save/load map from external code just by calling necessary function. +This library is also used by `map_loader` and `map_saver` to work. Currently it contains +OccupancyGrid saving/loading functions moved from the rest part of map server code. +It is designed to be replaceble for a new IO library (e.g. for library with new map encoding +method or any other library supporting costmaps, multifloor maps, etc...). + +### CLI-usage + +#### Map Server + +The `Map Server` is a composable ROS2 node. By default, there is a `map_server` executable that instances one of these nodes, but it is possible to compose multiple map server nodes into a single process, if desired. @@ -82,16 +100,45 @@ Then, one would invoke this process with the params file that contains the param $ process_with_multiple_map_servers __params:=combined_params.yaml ``` +#### Map Saver + +Like in ROS1 `map_saver` could be used as CLI-executable. It was renamed to `map_saver_cli` +and could be invoked by following command: + +``` +$ ros2 run nav2_map_server map_saver_cli [arguments] [--ros-args ROS remapping args] +``` + ## Currently Supported Map Types -- Occupancy grid (nav_msgs/msg/OccupancyGrid), via the OccGridLoader + +- Occupancy grid (nav_msgs/msg/OccupancyGrid) + +## MapIO library + +`MapIO` library contains following API functions declared in `map_io.hpp` to work with +OccupancyGrid maps: + +- loadMapYaml(): Load and parse the given YAML file +- loadMapFromFile(): Load the image from map file and generate an OccupancyGrid +- loadMapFromYaml(): Load the map YAML, image from map file and generate an OccupancyGrid +- saveMapToFile(): Write OccupancyGrid map to file ## Services -As in ROS navigation, the map_server node provides a "map" service to get the map. See the nav_msgs/srv/GetMap.srv file for details. -NEW in ROS2 Eloquent, map_server also now provides a "load_map" service. See nav2_msgs/srv/LoadMap.srv for details. +As in ROS navigation, the `map_server` node provides a "map" service to get the map. See the nav_msgs/srv/GetMap.srv file for details. + +NEW in ROS2 Eloquent, `map_server` also now provides a "load_map" service and `map_saver` - +a "save_map" service. See nav2_msgs/srv/LoadMap.srv and nav2_msgs/srv/SaveMap.srv for details. + +For using these services `map_server`/`map_saver` should be launched as a continuously running +`nav2::LifecycleNode` node. In addition to the CLI, `Map Saver` has a functionality of server +handling incoming services. To run `Map Saver` in a server mode +`nav2_map_server/launch/map_saver_server.launch.py` launch-file could be used. + +Service usage examples: -Example: ``` -$ ros2 service call /load_map nav2_msgs/srv/LoadMap "{type: 0, map_id: /ros/maps/map.yaml} +$ ros2 service call /map_server/load_map nav2_msgs/srv/LoadMap "{map_url: /ros/maps/map.yaml}" +$ ros2 service call /map_saver/save_map nav2_msgs/srv/SaveMap "{map_topic: map, map_url: my_map, image_format: pgm, map_mode: trinary, free_thresh: 0.25, occupied_thresh: 0.65}" ``` diff --git a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake index 37a2fbfb74..c04356b4aa 100644 --- a/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake +++ b/nav2_map_server/cmake_modules/FindGRAPHICSMAGICKCPP.cmake @@ -26,9 +26,9 @@ find_path(GRAPHICSMAGICKCPP_INCLUDE_DIRS PATH_SUFFIXES GraphicsMagick) find_library(GRAPHICSMAGICKCPP_LIBRARIES - NAMES "GraphicsMagick++") + NAMES "GraphicsMagick++" "graphicsmagick") find_package_handle_standard_args( GRAPHICSMAGICKCPP - GRAPHICSMAGICKCPP__LIBRARIES + GRAPHICSMAGICKCPP_LIBRARIES GRAPHICSMAGICKCPP_INCLUDE_DIRS) \ No newline at end of file diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp new file mode 100644 index 0000000000..bee7df7313 --- /dev/null +++ b/nav2_map_server/include/nav2_map_server/map_io.hpp @@ -0,0 +1,103 @@ +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* OccupancyGrid map input-output library */ + +#ifndef NAV2_MAP_SERVER__MAP_IO_HPP_ +#define NAV2_MAP_SERVER__MAP_IO_HPP_ + +#include +#include + +#include "nav2_map_server/map_mode.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" + +/* Map input part */ + +namespace nav2_map_server +{ + +struct LoadParameters +{ + std::string image_file_name; + double resolution{0}; + std::vector origin{0, 0, 0}; + double free_thresh; + double occupied_thresh; + MapMode mode; + bool negate; +}; + +typedef enum +{ + LOAD_MAP_SUCCESS, + MAP_DOES_NOT_EXIST, + INVALID_MAP_METADATA, + INVALID_MAP_DATA +} LOAD_MAP_STATUS; + +/** + * @brief Load and parse the given YAML file + * @param yaml_filename Name of the map file passed though parameter + * @return Map loading parameters obtained from YAML file + * @throw YAML::Exception + */ +LoadParameters loadMapYaml(const std::string & yaml_filename); + +/** + * @brief Load the image from map file and generate an OccupancyGrid + * @param load_parameters Parameters of loading map + * @param map Output loaded map + * @throw std::exception + */ +void loadMapFromFile( + const LoadParameters & load_parameters, + nav_msgs::msg::OccupancyGrid & map); + +/** + * @brief Load the map YAML, image from map file and + * generate an OccupancyGrid + * @param yaml_file Name of input YAML file + * @param map Output loaded map + * @return status of map loaded + */ +LOAD_MAP_STATUS loadMapFromYaml( + const std::string & yaml_file, + nav_msgs::msg::OccupancyGrid & map); + + +/* Map output part */ + +struct SaveParameters +{ + std::string map_file_name{""}; + std::string image_format{""}; + double free_thresh{0.0}; + double occupied_thresh{0.0}; + MapMode mode{MapMode::Trinary}; +}; + +/** + * @brief Write OccupancyGrid map to file + * @param map OccupancyGrid map data + * @param save_parameters Map saving parameters. + * @return true or false + */ +bool saveMapToFile( + const nav_msgs::msg::OccupancyGrid & map, + const SaveParameters & save_parameters); + +} // namespace nav2_map_server + +#endif // NAV2_MAP_SERVER__MAP_IO_HPP_ diff --git a/nav2_map_server/include/nav2_map_server/map_saver.hpp b/nav2_map_server/include/nav2_map_server/map_saver.hpp index f3e3078478..2c096e3dba 100644 --- a/nav2_map_server/include/nav2_map_server/map_saver.hpp +++ b/nav2_map_server/include/nav2_map_server/map_saver.hpp @@ -1,3 +1,4 @@ +// Copyright (c) 2020 Samsung Research Russia // Copyright (c) 2018 Intel Corporation // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -16,56 +17,103 @@ #define NAV2_MAP_SERVER__MAP_SAVER_HPP_ #include -#include "map_mode.hpp" -#include "nav_msgs/srv/get_map.hpp" +#include + #include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/srv/save_map.hpp" + +#include "map_io.hpp" namespace nav2_map_server { /** * @class nav2_map_server::MapSaver - * @brief A class that writes map to a file from occpancy grid which is - * subscribed from "/map" topic. + * @brief A class that provides map saving methods and services */ -class MapSaver : public rclcpp::Node +class MapSaver : public nav2_util::LifecycleNode { public: /** - * @brief Constructor for the MapSaver - * @param options NodeOptions for the MapSaver + * @brief Constructor for the nav2_map_server::MapSaver */ - explicit MapSaver(const rclcpp::NodeOptions & options); + MapSaver(); /** - * @brief A Map callback function calls try_write_map_to_file method to - * write map data to a file. - * @param map Occupancy Grid message data + * @brief Destructor for the nav2_map_server::MapServer */ - void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map); + ~MapSaver(); /** - * @brief Returns the promise as shared future - * @return a shared future copy of save_next_map_promise + * @brief Read a message from incoming map topic and save map to a file + * @param map_topic Incoming map topic name + * @param save_parameters Map saving parameters. + * @return true of false */ - std::shared_future map_saved_future() {return save_next_map_promise.get_future().share();} + bool saveMapTopicToFile( + const std::string & map_topic, + const SaveParameters & save_parameters); protected: - rclcpp::Subscription::ConstSharedPtr map_sub_; + /** + * @brief Sets up map saving service + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when node switched to active state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when node switched to inactive state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when it is required node clean-up + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when in Shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called when Error is raised + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; /** - * @brief Writes map data to file - * @param map Occupancy grid data + * @brief Map saving service callback + * @param request_header Service request header + * @param request Service request + * @param response Service response */ - void try_write_map_to_file(const nav_msgs::msg::OccupancyGrid & map); + void saveMapCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); - std::promise save_next_map_promise; + // The timeout for saving the map in service + std::shared_ptr save_map_timeout_; + // Default values for map thresholds + double free_thresh_default_; + double occupied_thresh_default_; - std::string image_format; - std::string mapname_; - int threshold_occupied_; - int threshold_free_; - nav2_map_server::MapMode map_mode; + // The name of the service for saving a map from topic + const std::string save_map_service_name_{"save_map"}; + // A service to save the map to a file at run time (SaveMap) + rclcpp::Service::SharedPtr save_map_service_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp index 966611854f..d3ace49e90 100644 --- a/nav2_map_server/include/nav2_map_server/map_server.hpp +++ b/nav2_map_server/include/nav2_map_server/map_server.hpp @@ -15,16 +15,23 @@ #ifndef NAV2_MAP_SERVER__MAP_SERVER_HPP_ #define NAV2_MAP_SERVER__MAP_SERVER_HPP_ +#include #include +#include + +#include "rclcpp/rclcpp.hpp" #include "nav2_util/lifecycle_node.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/srv/get_map.hpp" +#include "nav2_msgs/srv/load_map.hpp" namespace nav2_map_server { /** * @class nav2_map_server::MapServer - * @brief This class on activate, loads the OccGridLoader node to start map - * server from the yaml file which is being passed as parameter. + * @brief Parses the map yaml file and creates a service and a publisher that + * provides occupancy grid */ class MapServer : public nav2_util::LifecycleNode { @@ -41,27 +48,25 @@ class MapServer : public nav2_util::LifecycleNode protected: /** - * @brief Sets up required params and calls OccGridLoader node's configure - * state + * @brief Sets up required params and services. Loads map and its parameters from the file * @param state Lifecycle Node's state * @return Success or Failure */ nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; /** - * @brief Calls OccGridLoader node's activate state + * @brief Start publishing the map using the latched topic * @param state Lifecycle Node's state * @return Success or Failure */ nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; /** - * @brief Calls OccGridLoader node's deactivate state + * @brief Stops publishing the latched topic * @param state Lifecycle Node's state * @return Success or Failure */ nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; /** - * @brief Calls OccGridLoader node's cleaning up state and resets the member - * variables + * @brief Resets the member variables * @param state Lifecycle Node's state * @return Success or Failure */ @@ -79,8 +84,65 @@ class MapServer : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override; - // The map loader that will actually do the work - std::unique_ptr map_loader_; + /** + * @brief Load the map YAML, image from map file name and + * generate output response containing an OccupancyGrid. + * Update msg_ class variable. + * @param yaml_file name of input YAML file + * @param response Output response with loaded OccupancyGrid map + * @return true or false + */ + bool loadMapResponseFromYaml( + const std::string & yaml_file, + std::shared_ptr response); + + /** + * @brief Method correcting msg_ header when it belongs to instantiated object + */ + void updateMsgHeader(); + + /** + * @brief Map getting service callback + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void getMapCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + /** + * @brief Map loading service callback + * @param request_header Service request header + * @param request Service request + * @param response Service response + */ + void loadMapCallback( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + + // The name of the service for getting a map + const std::string service_name_{"map"}; + + // The name of the service for loading a map + const std::string load_map_service_name_{"load_map"}; + + // A service to provide the occupancy grid (GetMap) and the message to return + rclcpp::Service::SharedPtr occ_service_; + + // A service to load the occupancy grid from file at run time (LoadMap) + rclcpp::Service::SharedPtr load_map_service_; + + // A topic on which the occupancy grid will be published + rclcpp_lifecycle::LifecyclePublisher::SharedPtr occ_pub_; + + // The frame ID used in the returned OccupancyGrid message + std::string frame_id_; + + // The message to publish on the occupancy grid topic + nav_msgs::msg::OccupancyGrid msg_; }; } // namespace nav2_map_server diff --git a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp deleted file mode 100644 index 305d1251a4..0000000000 --- a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_ -#define NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_ - -#include -#include -#include -#include "nav2_map_server/occ_grid_loader.hpp" - -#include "map_mode.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "nav_msgs/srv/get_map.hpp" -#include "nav2_msgs/srv/load_map.hpp" -#include "rclcpp/rclcpp.hpp" - -namespace nav2_map_server -{ -/** - * @class nav2_map_server::OccGridLoader - * @brief Parses the map yaml file and creates a service and a publisher that - * provides occupancy grid - */ -class OccGridLoader : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface -{ -public: - /** - * @brief Constructor for OccGridLoader - * @param node - * @param Yaml_filename File that contains map data - */ - OccGridLoader(rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & yaml_filename); - /** - * @brief Disabling the use of default or empty constructor - */ - OccGridLoader() = delete; - /** - * @brief Destructor for OccGridLoader - */ - ~OccGridLoader(); - /** - * @brief Load map and its parameters from the file - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; - /** - * @brief Start publishing the map using the latched topic - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Stops publishing the latched topic - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; - /** - * @brief Resets the member variables - * @param state Lifecycle Node's state - * @return Success or Failure - */ - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; - -protected: - // The ROS node to use for ROS-related operations such as creating a service - rclcpp_lifecycle::LifecycleNode::SharedPtr node_; - - // The name of the YAML file from which to get the conversion parameters - std::string yaml_filename_; - - typedef struct - { - std::string image_file_name; - double resolution{0}; - std::vector origin{0, 0, 0}; - double free_thresh; - double occupied_thresh; - MapMode mode; - bool negate; - } LoadParameters; - - /** - * @brief Load and parse the given YAML file - * @param yaml_filename_ Name of the map file passed though parameter - * @throw YAML::Exception - */ - LoadParameters load_map_yaml(const std::string & yaml_filename_); - - // Load the image and generate an OccupancyGrid - void loadMapFromFile(const LoadParameters & loadParameters); - - // Load the map yaml and image from yaml file name - bool loadMapFromYaml( - std::string yaml_file, - std::shared_ptr response = nullptr); - - // A service to provide the occupancy grid (GetMap) and the message to return - rclcpp::Service::SharedPtr occ_service_; - - // A service to load the occupancy grid from file at run time (LoadMap) - rclcpp::Service::SharedPtr load_map_service_; - - // A topic on which the occupancy grid will be published - rclcpp_lifecycle::LifecyclePublisher::SharedPtr occ_pub_; - - // The message to publish on the occupancy grid topic - std::unique_ptr msg_; - - // The frame ID used in the returned OccupancyGrid message - static constexpr const char * frame_id_{"map"}; - - // The name for the topic on which the map will be published - static constexpr const char * topic_name_{"map"}; - - // The name of the service for getting a map - static constexpr const char * service_name_{"map"}; - - // The name of the service for loading a map - static constexpr const char * load_map_service_name_{"load_map"}; - - // Timer for republishing map - rclcpp::TimerBase::SharedPtr timer_; -}; - -} // namespace nav2_map_server - -#endif // NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_ diff --git a/nav2_map_server/launch/map_saver_server.launch.py b/nav2_map_server/launch/map_saver_server.launch.py new file mode 100644 index 0000000000..094c171a65 --- /dev/null +++ b/nav2_map_server/launch/map_saver_server.launch.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2020 Samsung Research Russia +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +import launch_ros.actions + + +def generate_launch_description(): + # Parameters + lifecycle_nodes = ['map_saver'] + use_sim_time = True + autostart = True + save_map_timeout = 2000 + free_thresh_default = 0.25 + occupied_thresh_default = 0.65 + + # Nodes launching commands + start_map_saver_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_saver_server', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'save_map_timeout': save_map_timeout}, + {'free_thresh_default': free_thresh_default}, + {'occupied_thresh_default': occupied_thresh_default}]) + + start_lifecycle_manager_cmd = launch_ros.actions.Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + ld = LaunchDescription() + + ld.add_action(start_map_saver_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + + return ld diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index ffd9f7c2b8..d1c9aa8e5e 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -22,6 +22,7 @@ launch_ros launch_testing tf2 + nav2_msgs nav2_util graphicsmagick diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp new file mode 100644 index 0000000000..6941b354fe --- /dev/null +++ b/nav2_map_server/src/map_io.cpp @@ -0,0 +1,541 @@ +/* Copyright 2019 Rover Robotics + * Copyright 2010 Brian Gerkey + * Copyright (c) 2008, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "nav2_map_server/map_io.hpp" + +#ifndef _WIN32 +#include +#endif +#include +#include +#include +#include +#include + +#include "Magick++.h" +#include "nav2_util/geometry_utils.hpp" + +#include "yaml-cpp/yaml.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Quaternion.h" + +#ifdef _WIN32 +// https://github.com/rtv/Stage/blob/master/replace/dirname.c +static +char * dirname(char * path) +{ + static const char dot[] = "."; + char * last_slash; + + if (path == NULL) { + return path; + } + + /* Find last '/'. */ + last_slash = path != NULL ? strrchr(path, '/') : NULL; + + if (last_slash != NULL && last_slash == path) { + /* The last slash is the first character in the string. We have to + return "/". */ + ++last_slash; + } else if (last_slash != NULL && last_slash[1] == '\0') { + /* The '/' is the last character, we have to look further. */ + last_slash = reinterpret_cast(memchr(path, last_slash - path, '/')); + } + + if (last_slash != NULL) { + /* Terminate the path. */ + last_slash[0] = '\0'; + } else { + /* This assignment is ill-designed but the XPG specs require to + return a string containing "." in any case no directory part is + found and so a static and constant string is required. */ + path = reinterpret_cast(dot); + } + + return path; +} +#endif + +namespace nav2_map_server +{ +using nav2_util::geometry_utils::orientationAroundZAxis; + +// === Map input part === + +/// Get the given subnode value. +/// The only reason this function exists is to wrap the exceptions in slightly nicer error messages, +/// including the name of the failed key +/// @throw YAML::Exception +template +T yaml_get_value(const YAML::Node & node, const std::string & key) +{ + try { + return node[key].as(); + } catch (YAML::Exception & e) { + std::stringstream ss; + ss << "Failed to parse YAML tag '" << key << "' for reason: " << e.msg; + throw YAML::Exception(e.mark, ss.str()); + } +} + +LoadParameters loadMapYaml(const std::string & yaml_filename) +{ + YAML::Node doc = YAML::LoadFile(yaml_filename); + LoadParameters load_parameters; + + auto image_file_name = yaml_get_value(doc, "image"); + if (image_file_name.empty()) { + throw YAML::Exception(doc["image"].Mark(), "The image tag was empty."); + } + if (image_file_name[0] != '/') { + // dirname takes a mutable char *, so we copy into a vector + std::vector fname_copy(yaml_filename.begin(), yaml_filename.end()); + fname_copy.push_back('\0'); + image_file_name = std::string(dirname(fname_copy.data())) + '/' + image_file_name; + } + load_parameters.image_file_name = image_file_name; + + load_parameters.resolution = yaml_get_value(doc, "resolution"); + load_parameters.origin = yaml_get_value>(doc, "origin"); + if (load_parameters.origin.size() != 3) { + throw YAML::Exception( + doc["origin"].Mark(), "value of the 'origin' tag should have 3 elements, not " + + std::to_string(load_parameters.origin.size())); + } + + load_parameters.free_thresh = yaml_get_value(doc, "free_thresh"); + load_parameters.occupied_thresh = yaml_get_value(doc, "occupied_thresh"); + + auto map_mode_node = doc["mode"]; + if (!map_mode_node.IsDefined()) { + load_parameters.mode = MapMode::Trinary; + } else { + load_parameters.mode = map_mode_from_string(map_mode_node.as()); + } + + try { + load_parameters.negate = yaml_get_value(doc, "negate"); + } catch (YAML::Exception &) { + load_parameters.negate = yaml_get_value(doc, "negate"); + } + + std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl; + std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl; + std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl; + std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl; + std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl; + std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh << + std::endl; + std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string(load_parameters.mode) << std::endl; + std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT + + return load_parameters; +} + +void loadMapFromFile( + const LoadParameters & load_parameters, + nav_msgs::msg::OccupancyGrid & map) +{ + Magick::InitializeMagick(nullptr); + nav_msgs::msg::OccupancyGrid msg; + + std::cout << "[INFO] [map_io]: Loading image_file: " << + load_parameters.image_file_name << std::endl; + Magick::Image img(load_parameters.image_file_name); + + // Copy the image data into the map structure + msg.info.width = img.size().width(); + msg.info.height = img.size().height(); + + msg.info.resolution = load_parameters.resolution; + msg.info.origin.position.x = load_parameters.origin[0]; + msg.info.origin.position.y = load_parameters.origin[1]; + msg.info.origin.position.z = 0.0; + msg.info.origin.orientation = orientationAroundZAxis(load_parameters.origin[2]); + + // Allocate space to hold the data + msg.data.resize(msg.info.width * msg.info.height); + + // Copy pixel data into the map structure + for (size_t y = 0; y < msg.info.height; y++) { + for (size_t x = 0; x < msg.info.width; x++) { + auto pixel = img.pixelColor(x, y); + + std::vector channels = {pixel.redQuantum(), pixel.greenQuantum(), + pixel.blueQuantum()}; + if (load_parameters.mode == MapMode::Trinary && img.matte()) { + // To preserve existing behavior, average in alpha with color channels in Trinary mode. + // CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque + channels.push_back(MaxRGB - pixel.alphaQuantum()); + } + double sum = 0; + for (auto c : channels) { + sum += c; + } + /// on a scale from 0.0 to 1.0 how bright is the pixel? + double shade = Magick::ColorGray::scaleQuantumToDouble(sum / channels.size()); + + // If negate is true, we consider blacker pixels free, and whiter + // pixels occupied. Otherwise, it's vice versa. + /// on a scale from 0.0 to 1.0, how occupied is the map cell (before thresholding)? + double occ = (load_parameters.negate ? shade : 1.0 - shade); + + int8_t map_cell; + switch (load_parameters.mode) { + case MapMode::Trinary: + if (load_parameters.occupied_thresh < occ) { + map_cell = 100; + } else if (occ < load_parameters.free_thresh) { + map_cell = 0; + } else { + map_cell = -1; + } + break; + case MapMode::Scale: + if (pixel.alphaQuantum() != OpaqueOpacity) { + map_cell = -1; + } else if (load_parameters.occupied_thresh < occ) { + map_cell = 100; + } else if (occ < load_parameters.free_thresh) { + map_cell = 0; + } else { + map_cell = std::rint( + (occ - load_parameters.free_thresh) / + (load_parameters.occupied_thresh - load_parameters.free_thresh) * 100.0); + } + break; + case MapMode::Raw: { + double occ_percent = std::round(shade * 255); + if (0 <= occ_percent && occ_percent <= 100) { + map_cell = static_cast(occ_percent); + } else { + map_cell = -1; + } + break; + } + default: + throw std::runtime_error("Invalid map mode"); + } + msg.data[msg.info.width * (msg.info.height - y - 1) + x] = map_cell; + } + } + + // Since loadMapFromFile() does not belong to any node, publishing in a system time. + rclcpp::Clock clock(RCL_SYSTEM_TIME); + msg.info.map_load_time = clock.now(); + msg.header.frame_id = "map"; + msg.header.stamp = clock.now(); + + std::cout << + "[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width << + " X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl; + + map = msg; +} + +LOAD_MAP_STATUS loadMapFromYaml( + const std::string & yaml_file, + nav_msgs::msg::OccupancyGrid & map) +{ + if (yaml_file.empty()) { + std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl; + return MAP_DOES_NOT_EXIST; + } + std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl; + LoadParameters load_parameters; + try { + load_parameters = loadMapYaml(yaml_file); + } catch (YAML::Exception & e) { + std::cerr << + "[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (" << + e.mark.line << ":" << e.mark.column << ") for reason: " << e.what() << std::endl; + return INVALID_MAP_METADATA; + } catch (std::exception & e) { + std::cerr << + "[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file << + " for reason: " << e.what() << std::endl; + return INVALID_MAP_METADATA; + } + + try { + loadMapFromFile(load_parameters, map); + } catch (std::exception & e) { + std::cerr << + "[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name << + " for reason: " << e.what() << std::endl; + return INVALID_MAP_DATA; + } + + return LOAD_MAP_SUCCESS; +} + +// === Map output part === + +/** + * @brief Checks map saving parameters for consistency + * @param save_parameters Map saving parameters. + * NOTE: save_parameters could be updated during function execution. + * @throw std::exception in case of inconsistent parameters + */ +void checkSaveParameters(SaveParameters & save_parameters) +{ + // Magick must me initialized before any activity with images + Magick::InitializeMagick(nullptr); + + // Checking map file name + if (save_parameters.map_file_name == "") { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + save_parameters.map_file_name = "map_" + + std::to_string(static_cast(clock.now().seconds())); + std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " << + save_parameters.map_file_name << " file" << std::endl; + } + + // Checking thresholds + if (save_parameters.occupied_thresh == 0.0) { + save_parameters.occupied_thresh = 0.65; + std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " << + save_parameters.occupied_thresh << std::endl; + } + if (save_parameters.free_thresh == 0.0) { + save_parameters.free_thresh = 0.25; + std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " << + save_parameters.free_thresh << std::endl; + } + if (1.0 < save_parameters.occupied_thresh) { + std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl; + throw std::runtime_error("Incorrect thresholds"); + } + if (save_parameters.free_thresh < 0.0) { + std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl; + throw std::runtime_error("Incorrect thresholds"); + } + if (save_parameters.occupied_thresh <= save_parameters.free_thresh) { + std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" << + std::endl; + throw std::runtime_error("Incorrect thresholds"); + } + + // Checking image format + if (save_parameters.image_format == "") { + save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm"; + std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " << + save_parameters.image_format << std::endl; + } + + std::transform( + save_parameters.image_format.begin(), + save_parameters.image_format.end(), + save_parameters.image_format.begin(), + [](unsigned char c) {return std::tolower(c);}); + + const std::vector BLESSED_FORMATS{"bmp", "pgm", "png"}; + if ( + std::find(BLESSED_FORMATS.begin(), BLESSED_FORMATS.end(), save_parameters.image_format) == + BLESSED_FORMATS.end()) + { + std::stringstream ss; + bool first = true; + for (auto & format_name : BLESSED_FORMATS) { + if (!first) { + ss << ", "; + } + ss << "'" << format_name << "'"; + first = false; + } + std::cout << + "[WARN] [map_io]: Requested image format '" << save_parameters.image_format << + "' is not one of the recommended formats: " << ss.str() << std::endl; + } + const std::string FALLBACK_FORMAT = "png"; + + try { + Magick::CoderInfo info(save_parameters.image_format); + if (!info.isWritable()) { + std::cout << + "[WARN] [map_io]: Format '" << save_parameters.image_format << + "' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl; + save_parameters.image_format = FALLBACK_FORMAT; + } + } catch (Magick::ErrorOption & e) { + std::cout << + "[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" << + FALLBACK_FORMAT << "' instead:" << std::endl << e.what() << std::endl; + save_parameters.image_format = FALLBACK_FORMAT; + } + + // Checking map mode + if ( + save_parameters.mode == MapMode::Scale && + (save_parameters.image_format == "pgm" || + save_parameters.image_format == "jpg" || + save_parameters.image_format == "jpeg")) + { + std::cout << + "[WARN] [map_io]: Map mode 'scale' requires transparency, but format '" << + save_parameters.image_format << + "' does not support it. Consider switching image format to 'png'." << std::endl; + } +} + +/** + * @brief Tries to write map data into a file + * @param map Occupancy grid data + * @param save_parameters Map saving parameters + * @throw std::expection in case of problem + */ +void tryWriteMapToFile( + const nav_msgs::msg::OccupancyGrid & map, + const SaveParameters & save_parameters) +{ + std::cout << + "[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " << + map.info.resolution << " m/pix" << std::endl; + + std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format; + { + // should never see this color, so the initialization value is just for debugging + Magick::Image image({map.info.width, map.info.height}, "red"); + + // In scale mode, we need the alpha (matte) channel. Else, we don't. + // NOTE: GraphicsMagick seems to have trouble loading the alpha channel when saved with + // Magick::GreyscaleMatte, so we use TrueColorMatte instead. + image.type( + save_parameters.mode == MapMode::Scale ? + Magick::TrueColorMatteType : Magick::GrayscaleType); + + // Since we only need to support 100 different pixel levels, 8 bits is fine + image.depth(8); + + int free_thresh_int = std::rint(save_parameters.free_thresh * 100.0); + int occupied_thresh_int = std::rint(save_parameters.occupied_thresh * 100.0); + + for (size_t y = 0; y < map.info.height; y++) { + for (size_t x = 0; x < map.info.width; x++) { + int8_t map_cell = map.data[map.info.width * (map.info.height - y - 1) + x]; + + Magick::Color pixel; + + switch (save_parameters.mode) { + case MapMode::Trinary: + if (map_cell < 0 || 100 < map_cell) { + pixel = Magick::ColorGray(205 / 255.0); + } else if (map_cell <= free_thresh_int) { + pixel = Magick::ColorGray(254 / 255.0); + } else if (occupied_thresh_int <= map_cell) { + pixel = Magick::ColorGray(0 / 255.0); + } else { + pixel = Magick::ColorGray(205 / 255.0); + } + break; + case MapMode::Scale: + if (map_cell < 0 || 100 < map_cell) { + pixel = Magick::ColorGray{0.5}; + pixel.alphaQuantum(TransparentOpacity); + } else { + pixel = Magick::ColorGray{(100.0 - map_cell) / 100.0}; + } + break; + case MapMode::Raw: + Magick::Quantum q; + if (map_cell < 0 || 100 < map_cell) { + q = MaxRGB; + } else { + q = map_cell / 255.0 * MaxRGB; + } + pixel = Magick::Color(q, q, q); + break; + default: + std::cerr << "[ERROR] [map_io]: Map mode should be Trinary, Scale or Raw" << std::endl; + throw std::runtime_error("Invalid map mode"); + } + image.pixelColor(x, y, pixel); + } + } + + std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl; + image.write(mapdatafile); + } + + std::string mapmetadatafile = save_parameters.map_file_name + ".yaml"; + { + std::ofstream yaml(mapmetadatafile); + + geometry_msgs::msg::Quaternion orientation = map.info.origin.orientation; + tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); + double yaw, pitch, roll; + mat.getEulerYPR(yaw, pitch, roll); + + YAML::Emitter e; + e << YAML::Precision(3); + e << YAML::BeginMap; + e << YAML::Key << "image" << YAML::Value << mapdatafile; + e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(save_parameters.mode); + e << YAML::Key << "resolution" << YAML::Value << map.info.resolution; + e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x << + map.info.origin.position.y << yaw << YAML::EndSeq; + e << YAML::Key << "negate" << YAML::Value << 0; + e << YAML::Key << "occupied_thresh" << YAML::Value << save_parameters.occupied_thresh; + e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh; + + if (!e.good()) { + std::cout << + "[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError() << + ". The map metadata may be invalid." << std::endl; + } + + std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl; + std::ofstream(mapmetadatafile) << e.c_str(); + } + std::cout << "[INFO] [map_io]: Map saved" << std::endl; +} + +bool saveMapToFile( + const nav_msgs::msg::OccupancyGrid & map, + const SaveParameters & save_parameters) +{ + // Local copy of SaveParameters that might be modified by checkSaveParameters() + SaveParameters save_parameters_loc = save_parameters; + + try { + // Checking map parameters for consistency + checkSaveParameters(save_parameters_loc); + + tryWriteMapToFile(map, save_parameters_loc); + } catch (std::exception & e) { + std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what() << std::endl; + return false; + } + return true; +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/src/map_saver.cpp b/nav2_map_server/src/map_saver.cpp deleted file mode 100644 index 5660c85c58..0000000000 --- a/nav2_map_server/src/map_saver.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/* - * Copyright 2019 Rover Robotics - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "nav2_map_server/map_saver.hpp" - -#include -#include -#include -#include -#include -#include - -#include "Magick++.h" -#include "nav2_map_server/map_mode.hpp" -#include "nav_msgs/msg/occupancy_grid.h" -#include "nav_msgs/srv/get_map.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2/LinearMath/Matrix3x3.h" -#include "tf2/LinearMath/Quaternion.h" -#include "yaml-cpp/yaml.h" - -namespace nav2_map_server -{ -MapSaver::MapSaver(const rclcpp::NodeOptions & options) -: Node("map_saver", options), save_next_map_promise{} -{ - Magick::InitializeMagick(nullptr); - { - mapname_ = declare_parameter("output_file_no_ext", "map"); - if (mapname_ == "map") { - mapname_ += "_" + std::to_string(static_cast(now().seconds())); - } - threshold_occupied_ = declare_parameter("threshold_occupied", 65); - if (100 < threshold_occupied_) { - throw std::runtime_error("Threshold_occupied must be 100 or less"); - } - threshold_free_ = declare_parameter("threshold_free", 25); - if (threshold_free_ < 0) { - throw std::runtime_error("Free threshold must be 0 or greater"); - } - if (threshold_occupied_ <= threshold_free_) { - throw std::runtime_error("Threshold_free must be smaller than threshold_occupied"); - } - - std::string mode_str = declare_parameter("map_mode", "trinary"); - try { - map_mode = map_mode_from_string(mode_str); - } catch (std::invalid_argument &) { - map_mode = MapMode::Trinary; - RCLCPP_WARN( - get_logger(), "Map mode parameter not recognized: '%s', using default value (trinary)", - mode_str.c_str()); - } - - image_format = declare_parameter("image_format", map_mode == MapMode::Scale ? "png" : "pgm"); - std::transform( - image_format.begin(), image_format.end(), image_format.begin(), - [](unsigned char c) {return std::tolower(c);}); - const std::vector BLESSED_FORMATS{"bmp", "pgm", "png"}; - if ( - std::find(BLESSED_FORMATS.begin(), BLESSED_FORMATS.end(), image_format) == - BLESSED_FORMATS.end()) - { - std::stringstream ss; - bool first = true; - for (auto & format_name : BLESSED_FORMATS) { - if (!first) { - ss << ", "; - } - ss << "'" << format_name << "'"; - first = false; - } - RCLCPP_WARN( - get_logger(), "Requested image format '%s' is not one of the recommended formats: %s", - image_format.c_str(), ss.str().c_str()); - } - const std::string FALLBACK_FORMAT = "png"; - - try { - Magick::CoderInfo info(image_format); - if (!info.isWritable()) { - RCLCPP_WARN( - get_logger(), "Format '%s' is not writable. Using '%s' instead", - image_format.c_str(), FALLBACK_FORMAT.c_str()); - image_format = FALLBACK_FORMAT; - } - } catch (Magick::ErrorOption & e) { - RCLCPP_WARN( - get_logger(), "Format '%s' is not usable. Using '%s' instead:\n%s", - image_format.c_str(), FALLBACK_FORMAT.c_str(), e.what()); - image_format = FALLBACK_FORMAT; - } - if ( - map_mode == MapMode::Scale && - (image_format == "pgm" || image_format == "jpg" || image_format == "jpeg")) - { - RCLCPP_WARN( - get_logger(), - "Map mode 'scale' requires transparency, but format '%s' does not support it. Consider " - "switching image format to 'png'.", - image_format.c_str()); - } - - RCLCPP_INFO(get_logger(), "Waiting for the map"); - map_sub_ = create_subscription( - "map", rclcpp::SystemDefaultsQoS(), - std::bind(&MapSaver::mapCallback, this, std::placeholders::_1)); - } -} - -void MapSaver::try_write_map_to_file(const nav_msgs::msg::OccupancyGrid & map) -{ - auto logger = get_logger(); - RCLCPP_INFO( - logger, "Received a %d X %d map @ %.3f m/pix", map.info.width, map.info.height, - map.info.resolution); - - std::string mapdatafile = mapname_ + "." + image_format; - { - // should never see this color, so the initialization value is just for debugging - Magick::Image image({map.info.width, map.info.height}, "red"); - - // In scale mode, we need the alpha (matte) channel. Else, we don't. - // NOTE: GraphicsMagick seems to have trouble loading the alpha channel when saved with - // Magick::GreyscaleMatte, so we use TrueColorMatte instead. - image.type(map_mode == MapMode::Scale ? Magick::TrueColorMatteType : Magick::GrayscaleType); - - // Since we only need to support 100 different pixel levels, 8 bits is fine - image.depth(8); - - for (size_t y = 0; y < map.info.height; y++) { - for (size_t x = 0; x < map.info.width; x++) { - int8_t map_cell = map.data[map.info.width * (map.info.height - y - 1) + x]; - - Magick::Color pixel; - - switch (map_mode) { - case MapMode::Trinary: - if (map_cell < 0 || 100 < map_cell) { - pixel = Magick::ColorGray(205 / 255.0); - } else if (map_cell <= threshold_free_) { - pixel = Magick::ColorGray(254 / 255.0); - } else if (threshold_occupied_ <= map_cell) { - pixel = Magick::ColorGray(0 / 255.0); - } else { - pixel = Magick::ColorGray(205 / 255.0); - } - break; - case MapMode::Scale: - if (map_cell < 0 || 100 < map_cell) { - pixel = Magick::ColorGray{0.5}; - pixel.alphaQuantum(TransparentOpacity); - } else { - pixel = Magick::ColorGray{(100.0 - map_cell) / 100.0}; - } - break; - case MapMode::Raw: - Magick::Quantum q; - if (map_cell < 0 || 100 < map_cell) { - q = MaxRGB; - } else { - q = map_cell / 255.0 * MaxRGB; - } - pixel = Magick::Color(q, q, q); - break; - default: - throw std::runtime_error("Invalid map mode"); - } - image.pixelColor(x, y, pixel); - } - } - - RCLCPP_INFO(logger, "Writing map occupancy data to %s", mapdatafile.c_str()); - image.write(mapdatafile); - } - - std::string mapmetadatafile = mapname_ + ".yaml"; - { - std::ofstream yaml(mapmetadatafile); - - geometry_msgs::msg::Quaternion orientation = map.info.origin.orientation; - tf2::Matrix3x3 mat(tf2::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w)); - double yaw, pitch, roll; - mat.getEulerYPR(yaw, pitch, roll); - - YAML::Emitter e; - e << YAML::Precision(3); - e << YAML::BeginMap; - e << YAML::Key << "image" << YAML::Value << mapdatafile; - e << YAML::Key << "mode" << YAML::Value << map_mode_to_string(map_mode); - e << YAML::Key << "resolution" << YAML::Value << map.info.resolution; - e << YAML::Key << "origin" << YAML::Flow << YAML::BeginSeq << map.info.origin.position.x << - map.info.origin.position.y << yaw << YAML::EndSeq; - e << YAML::Key << "negate" << YAML::Value << 0; - e << YAML::Key << "occupied_thresh" << YAML::Value << threshold_occupied_ / 100.0; - e << YAML::Key << "free_thresh" << YAML::Value << threshold_free_ / 100.0; - - if (!e.good()) { - RCLCPP_WARN( - logger, "YAML writer failed with an error %s. The map metadata may be invalid.", - e.GetLastError().c_str()); - } - - RCLCPP_INFO(logger, "Writing map metadata to %s", mapmetadatafile.c_str()); - std::ofstream(mapmetadatafile) << e.c_str(); - } - RCLCPP_INFO(logger, "Map saved"); -} - -void MapSaver::mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr map) -{ - auto current_promise = std::move(save_next_map_promise); - save_next_map_promise = std::promise(); - - try { - try_write_map_to_file(*map); - current_promise.set_value(); - } catch (std::exception & e) { - RCLCPP_ERROR(get_logger(), "Failed to write map for reason: %s", e.what()); - current_promise.set_exception(std::current_exception()); - } -} -} // namespace nav2_map_server diff --git a/nav2_map_server/src/map_saver/main_cli.cpp b/nav2_map_server/src/map_saver/main_cli.cpp new file mode 100644 index 0000000000..89c95e057e --- /dev/null +++ b/nav2_map_server/src/map_saver/main_cli.cpp @@ -0,0 +1,180 @@ +// Copyright 2019 Rover Robotics +// Copyright (c) 2008, Willow Garage, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "nav2_map_server/map_mode.hpp" +#include "nav2_map_server/map_saver.hpp" + +#include "rclcpp/rclcpp.hpp" + +using namespace nav2_map_server; // NOLINT + +const char * USAGE_STRING{ + "Usage:\n" + " map_saver_cli [arguments] [--ros-args ROS remapping args]\n" + "\n" + "Arguments:\n" + " -h/--help\n" + " -t \n" + " -f \n" + " --occ \n" + " --free \n" + " --fmt \n" + " --mode trinary(default)/scale/raw\n" + "\n" + "NOTE: --ros-args should be passed at the end of command line"}; + +typedef enum +{ + COMMAND_MAP_TOPIC, + COMMAND_MAP_FILE_NAME, + COMMAND_IMAGE_FORMAT, + COMMAND_OCCUPIED_THRESH, + COMMAND_FREE_THRESH, + COMMAND_MODE +} COMMAND_TYPE; + +struct cmd_struct +{ + const char * cmd; + COMMAND_TYPE command_type; +}; + +typedef enum +{ + ARGUMENTS_INVALID, + ARGUMENTS_VALID, + HELP_MESSAGE +} ARGUMENTS_STATUS; + +// Arguments parser +// Input parameters: logger, argc, argv +// Output parameters: map_topic, save_parameters +ARGUMENTS_STATUS parse_arguments( + const rclcpp::Logger & logger, int argc, char ** argv, + std::string & map_topic, SaveParameters & save_parameters) +{ + const struct cmd_struct commands[] = { + {"-t", COMMAND_MAP_TOPIC}, + {"-f", COMMAND_MAP_FILE_NAME}, + {"--occ", COMMAND_OCCUPIED_THRESH}, + {"--free", COMMAND_FREE_THRESH}, + {"--mode", COMMAND_MODE}, + {"--fmt", COMMAND_IMAGE_FORMAT}, + }; + + std::vector arguments(argv + 1, argv + argc); + std::vector params_from_args; + + + size_t cmd_size = sizeof(commands) / sizeof(commands[0]); + size_t i; + for (auto it = arguments.begin(); it != arguments.end(); it++) { + if (*it == "-h" || *it == "--help") { + std::cout << USAGE_STRING << std::endl; + return HELP_MESSAGE; + } + if (*it == "--ros-args") { + break; + } + for (i = 0; i < cmd_size; i++) { + if (commands[i].cmd == *it) { + if ((it + 1) == arguments.end()) { + RCLCPP_ERROR(logger, "Wrong argument: %s should be followed by a value.", it->c_str()); + return ARGUMENTS_INVALID; + } + it++; + switch (commands[i].command_type) { + case COMMAND_MAP_TOPIC: + map_topic = *it; + break; + case COMMAND_MAP_FILE_NAME: + save_parameters.map_file_name = *it; + break; + case COMMAND_FREE_THRESH: + save_parameters.free_thresh = atoi(it->c_str()); + break; + case COMMAND_OCCUPIED_THRESH: + save_parameters.occupied_thresh = atoi(it->c_str()); + break; + case COMMAND_IMAGE_FORMAT: + save_parameters.image_format = *it; + break; + case COMMAND_MODE: + try { + save_parameters.mode = map_mode_from_string(*it); + } catch (std::invalid_argument &) { + save_parameters.mode = MapMode::Trinary; + RCLCPP_WARN( + logger, + "Map mode parameter not recognized: %s, using default value (trinary)", + it->c_str()); + } + break; + } + break; + } + } + if (i == cmd_size) { + RCLCPP_ERROR(logger, "Wrong argument: %s", it->c_str()); + return ARGUMENTS_INVALID; + } + } + + return ARGUMENTS_VALID; +} + +int main(int argc, char ** argv) +{ + // ROS2 init + rclcpp::init(argc, argv); + auto logger = rclcpp::get_logger("map_saver_cli"); + + // Parse CLI-arguments + SaveParameters save_parameters; + std::string map_topic = "map"; + switch (parse_arguments(logger, argc, argv, map_topic, save_parameters)) { + case ARGUMENTS_INVALID: + rclcpp::shutdown(); + return -1; + case HELP_MESSAGE: + rclcpp::shutdown(); + return 0; + case ARGUMENTS_VALID: + break; + } + + // Call saveMapTopicToFile() + int retcode; + try { + auto map_saver = std::make_shared(); + if (map_saver->saveMapTopicToFile(map_topic, save_parameters)) { + retcode = 0; + } else { + retcode = 1; + } + } catch (std::exception & e) { + RCLCPP_ERROR(logger, "Unexpected problem appear: %s", e.what()); + retcode = -1; + } + + // Exit + rclcpp::shutdown(); + return retcode; +} diff --git a/nav2_map_server/src/map_saver/main_server.cpp b/nav2_map_server/src/map_saver/main_server.cpp new file mode 100644 index 0000000000..c7d2c695eb --- /dev/null +++ b/nav2_map_server/src/map_saver/main_server.cpp @@ -0,0 +1,39 @@ +// Copyright (c) 2020 Samsung Research Russia +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "nav2_map_server/map_saver.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto logger = rclcpp::get_logger("map_saver_server"); + + try { + auto service_node = std::make_shared(); + rclcpp::spin(service_node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; + } catch (std::exception & ex) { + RCLCPP_ERROR(logger, ex.what()); + RCLCPP_ERROR(logger, "Exiting"); + return -1; + } +} diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp new file mode 100644 index 0000000000..001e2ed28b --- /dev/null +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -0,0 +1,215 @@ +/* + * Copyright (c) 2020 Samsung Research Russia + * Copyright 2019 Rover Robotics + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "nav2_map_server/map_saver.hpp" + +#include +#include +#include +#include + +using namespace std::placeholders; + +namespace nav2_map_server +{ +MapSaver::MapSaver() +: nav2_util::LifecycleNode("map_saver", "", true) +{ + RCLCPP_INFO(get_logger(), "Creating"); + + save_map_timeout_ = std::make_shared( + std::chrono::milliseconds(declare_parameter("save_map_timeout", 2000))); + + free_thresh_default_ = declare_parameter("free_thresh_default", 0.25), + occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65); +} + +MapSaver::~MapSaver() +{ + RCLCPP_INFO(get_logger(), "Destroying"); +} + +nav2_util::CallbackReturn +MapSaver::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Make name prefix for services + const std::string service_prefix = get_name() + std::string("/"); + + // Create a service that saves the occupancy grid from map topic to a file + save_map_service_ = create_service( + service_prefix + save_map_service_name_, + std::bind(&MapSaver::saveMapCallback, this, _1, _2, _3)); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapSaver::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapSaver::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapSaver::on_error(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapSaver::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +void MapSaver::saveMapCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // Set input arguments and call saveMapTopicToFile() + SaveParameters save_parameters; + save_parameters.map_file_name = request->map_url; + save_parameters.image_format = request->image_format; + save_parameters.free_thresh = request->free_thresh; + save_parameters.occupied_thresh = request->occupied_thresh; + try { + save_parameters.mode = map_mode_from_string(request->map_mode); + } catch (std::invalid_argument &) { + save_parameters.mode = MapMode::Trinary; + RCLCPP_WARN( + get_logger(), "Map mode parameter not recognized: '%s', using default value (trinary)", + request->map_mode.c_str()); + } + + response->result = saveMapTopicToFile(request->map_topic, save_parameters); +} + +bool MapSaver::saveMapTopicToFile( + const std::string & map_topic, + const SaveParameters & save_parameters) +{ + // Local copies of map_topic and save_parameters that could be changed + std::string map_topic_loc = map_topic; + SaveParameters save_parameters_loc = save_parameters; + + RCLCPP_INFO( + get_logger(), "Saving map from \'%s\' topic to \'%s\' file", + map_topic_loc.c_str(), save_parameters_loc.map_file_name.c_str()); + + try { + // Pointer to map message received in the subscription callback + nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = nullptr; + + // Correct map_topic_loc if necessary + if (map_topic_loc == "") { + map_topic_loc = "map"; + RCLCPP_WARN( + get_logger(), "Map topic unspecified. Map messages will be read from \'%s\' topic", + map_topic_loc.c_str()); + } + + // Set default for MapSaver node thresholds parameters + if (save_parameters_loc.free_thresh == 0.0) { + RCLCPP_WARN( + get_logger(), + "Free threshold unspecified. Setting it to default value: %f", + free_thresh_default_); + save_parameters_loc.free_thresh = free_thresh_default_; + } + if (save_parameters_loc.occupied_thresh == 0.0) { + RCLCPP_WARN( + get_logger(), + "Occupied threshold unspecified. Setting it to default value: %f", + occupied_thresh_default_); + save_parameters_loc.occupied_thresh = occupied_thresh_default_; + } + + // A callback function that receives map message from subscribed topic + auto mapCallback = [&map_msg]( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void { + map_msg = msg; + }; + + // Add new subscription for incoming map topic. + // Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode + // as a map listener. + auto map_sub = rclcpp_node_->create_subscription( + map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback); + + rclcpp::Time start_time = now(); + while (rclcpp::ok()) { + if ((now() - start_time) > *save_map_timeout_) { + RCLCPP_ERROR(get_logger(), "Failed to save the map: timeout"); + return false; + } + + if (map_msg) { + // Map message received. Saving it to file + if (saveMapToFile(*map_msg, save_parameters_loc)) { + RCLCPP_INFO(get_logger(), "Map saved successfully"); + return true; + } else { + RCLCPP_ERROR(get_logger(), "Failed to save the map"); + return false; + } + } + + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + } catch (std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to save the map: %s", e.what()); + return false; + } + + RCLCPP_ERROR(get_logger(), "This situation should never appear"); + return false; +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/src/map_saver_cli.cpp b/nav2_map_server/src/map_saver_cli.cpp deleted file mode 100644 index 6497f77112..0000000000 --- a/nav2_map_server/src/map_saver_cli.cpp +++ /dev/null @@ -1,117 +0,0 @@ -// Copyright 2019 Rover Robotics -// Copyright (c) 2008, Willow Garage, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include - -#include "nav2_map_server/map_saver.hpp" -#include "rclcpp/rclcpp.hpp" - -const char * USAGE_STRING{ - "Usage: \n" - " map_saver -h/--help\n" - " map_saver [--occ ] [--free ] [--fmt ] " - "[--mode trinary/scale/raw] [-f ] [ROS remapping args]"}; - -typedef enum {TYPE_STR, TYPE_INT} VAR_TYPE; -struct cmd_struct -{ - const char * cmd; - const char * param_name; - VAR_TYPE type; -}; - -int main(int argc, char ** argv) -{ - const struct cmd_struct commands[] = { - {"-f", "output_file_no_ext", TYPE_STR}, - {"--occ", "threshold_occupied", TYPE_INT}, - {"--free", "threshold_free", TYPE_INT}, - {"--mode", "map_mode", TYPE_STR}, - {"--fmt", "image_format", TYPE_STR}, - }; - - rclcpp::init(argc, argv); - - auto logger = rclcpp::get_logger("map_saver_cli"); - - std::vector arguments(argv + 1, argv + argc); - std::vector params_from_args; - - size_t cmd_size = sizeof(commands) / sizeof(commands[0]); - size_t i; - for (auto it = arguments.begin(); it != arguments.end(); it++) { - if (*it == "-h" || *it == "--help") { - std::cout << USAGE_STRING << std::endl; - rclcpp::shutdown(); - return 0; - } - for (i = 0; i < cmd_size; i++) { - if (commands[i].cmd == *it) { - if ((it + 1) == arguments.end()) { - RCLCPP_ERROR(logger, "Wrong argument: %s should be followed by a value.", it->c_str()); - rclcpp::shutdown(); - return -1; - } - it++; - if (commands[i].type == TYPE_INT) { - params_from_args.emplace_back(commands[i].param_name, atoi(it->c_str())); - } else { - params_from_args.emplace_back(commands[i].param_name, *it); - } - break; - } - } - if (i == cmd_size) { - RCLCPP_ERROR(logger, "Wrong argument: %s", it->c_str()); - rclcpp::shutdown(); - return -1; - } - } - - auto node = std::make_shared( - rclcpp::NodeOptions().parameter_overrides(params_from_args)); - auto future = node->map_saved_future(); - - int retcode; - switch (rclcpp::spin_until_future_complete(node, future)) { - case rclcpp::executor::FutureReturnCode::INTERRUPTED: - std::cout << "Map saver failed: interrupted" << std::endl; - retcode = 1; - break; - case rclcpp::executor::FutureReturnCode::TIMEOUT: - std::cout << "Map saver failed: timeout" << std::endl; - retcode = 1; - break; - case rclcpp::executor::FutureReturnCode::SUCCESS: - try { - future.get(); - std::cout << "Map saver succeeded" << std::endl; - retcode = 0; - } catch (std::exception & e) { - std::cout << "Map saver failed: " << e.what() << std::endl; - retcode = 1; - } - break; - default: - std::cerr << "this should never happen" << std::endl; - retcode = -1; - break; - } - - rclcpp::shutdown(); - return retcode; -} diff --git a/nav2_map_server/src/map_server.cpp b/nav2_map_server/src/map_server.cpp deleted file mode 100644 index 00be1a1db7..0000000000 --- a/nav2_map_server/src/map_server.cpp +++ /dev/null @@ -1,125 +0,0 @@ -// Copyright (c) 2018 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "nav2_map_server/map_server.hpp" - -#include -#include -#include -#include - -#include "nav2_map_server/occ_grid_loader.hpp" -#include "nav2_util/node_utils.hpp" -#include "yaml-cpp/yaml.h" - -using namespace std::chrono_literals; - -namespace nav2_map_server -{ - -MapServer::MapServer() -: nav2_util::LifecycleNode("map_server") -{ - RCLCPP_INFO(get_logger(), "Creating"); - - // Declare the node parameters - declare_parameter("yaml_filename"); -} - -MapServer::~MapServer() -{ - RCLCPP_INFO(get_logger(), "Destroying"); -} - -nav2_util::CallbackReturn -MapServer::on_configure(const rclcpp_lifecycle::State & state) -{ - RCLCPP_INFO(get_logger(), "Configuring"); - - // Get the name of the YAML file to use - std::string yaml_filename = get_parameter("yaml_filename").as_string(); - - // Make sure that there's a valid file there and open it up - std::ifstream fin(yaml_filename.c_str()); - if (fin.fail()) { - throw std::runtime_error("Could not open '" + yaml_filename + "': file not found"); - } - - // The YAML document from which to get the conversion parameters - YAML::Node doc = YAML::LoadFile(yaml_filename); - - // Get the map type so that we can create the correct map loader - std::string map_type; - try { - map_type = doc["map_type"].as(); - } catch (YAML::Exception &) { - // Default to occupancy grid if not specified in the YAML file - map_type = "occupancy"; - } - - // Create the correct map loader for the specified map type - if (map_type == "occupancy") { - map_loader_ = std::make_unique(shared_from_this(), yaml_filename); - } else { - std::string msg = "Cannot load unknown map type: '" + map_type + "'"; - throw std::runtime_error(msg); - } - - map_loader_->on_configure(state); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -MapServer::on_activate(const rclcpp_lifecycle::State & state) -{ - RCLCPP_INFO(get_logger(), "Activating"); - map_loader_->on_activate(state); - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -MapServer::on_deactivate(const rclcpp_lifecycle::State & state) -{ - RCLCPP_INFO(get_logger(), "Deactivating"); - map_loader_->on_deactivate(state); - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -MapServer::on_cleanup(const rclcpp_lifecycle::State & state) -{ - RCLCPP_INFO(get_logger(), "Cleaning up"); - - map_loader_->on_cleanup(state); - map_loader_.reset(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -MapServer::on_error(const rclcpp_lifecycle::State &) -{ - RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn -MapServer::on_shutdown(const rclcpp_lifecycle::State &) -{ - RCLCPP_INFO(get_logger(), "Shutting down"); - return nav2_util::CallbackReturn::SUCCESS; -} - -} // namespace nav2_map_server diff --git a/nav2_map_server/src/main.cpp b/nav2_map_server/src/map_server/main.cpp similarity index 100% rename from nav2_map_server/src/main.cpp rename to nav2_map_server/src/map_server/main.cpp diff --git a/nav2_map_server/src/map_server/map_server.cpp b/nav2_map_server/src/map_server/map_server.cpp new file mode 100644 index 0000000000..2c2e174f9c --- /dev/null +++ b/nav2_map_server/src/map_server/map_server.cpp @@ -0,0 +1,239 @@ +/* Copyright (c) 2018 Intel Corporation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* Copyright 2019 Rover Robotics + * Copyright 2010 Brian Gerkey + * Copyright (c) 2008, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "nav2_map_server/map_server.hpp" + +#include +#include +#include +#include +#include + +#include "yaml-cpp/yaml.h" +#include "lifecycle_msgs/msg/state.hpp" +#include "nav2_map_server/map_io.hpp" + +using namespace std::chrono_literals; +using namespace std::placeholders; + +namespace nav2_map_server +{ + +MapServer::MapServer() +: nav2_util::LifecycleNode("map_server") +{ + RCLCPP_INFO(get_logger(), "Creating"); + + // Declare the node parameters + declare_parameter("yaml_filename"); + declare_parameter("topic_name", "map"); + declare_parameter("frame_id", "map"); +} + +MapServer::~MapServer() +{ + RCLCPP_INFO(get_logger(), "Destroying"); +} + +nav2_util::CallbackReturn +MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Get the name of the YAML file to use + std::string yaml_filename = get_parameter("yaml_filename").as_string(); + + std::string topic_name = get_parameter("topic_name").as_string(); + frame_id_ = get_parameter("frame_id").as_string(); + + // Shared pointer to LoadMap::Response is also should be initialized + // in order to avoid null-pointer dereference + std::shared_ptr rsp = + std::make_shared(); + + if (!loadMapResponseFromYaml(yaml_filename, rsp)) { + throw std::runtime_error("Failed to load map yaml file: " + yaml_filename); + } + + // Make name prefix for services + const std::string service_prefix = get_name() + std::string("/"); + + // Create a service that provides the occupancy grid + occ_service_ = create_service( + service_prefix + std::string(service_name_), + std::bind(&MapServer::getMapCallback, this, _1, _2, _3)); + + // Create a publisher using the QoS settings to emulate a ROS1 latched topic + occ_pub_ = create_publisher( + topic_name, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + // Create a service that loads the occupancy grid from a file + load_map_service_ = create_service( + service_prefix + std::string(load_map_service_name_), + std::bind(&MapServer::loadMapCallback, this, _1, _2, _3)); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + // Publish the map using the latched topic + occ_pub_->on_activate(); + auto occ_grid = std::make_unique(msg_); + occ_pub_->publish(std::move(occ_grid)); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + occ_pub_->on_deactivate(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + occ_pub_.reset(); + occ_service_.reset(); + load_map_service_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapServer::on_error(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state"); + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +MapServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + return nav2_util::CallbackReturn::SUCCESS; +} + +void MapServer::getMapCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr/*request*/, + std::shared_ptr response) +{ + // if not in ACTIVE state, ignore request + if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_WARN( + get_logger(), + "Received GetMap request but not in ACTIVE state, ignoring!"); + return; + } + RCLCPP_INFO(get_logger(), "Handling GetMap request"); + response->map = msg_; +} + +void MapServer::loadMapCallback( + const std::shared_ptr/*request_header*/, + const std::shared_ptr request, + std::shared_ptr response) +{ + // if not in ACTIVE state, ignore request + if (get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_WARN( + get_logger(), + "Received LoadMap request but not in ACTIVE state, ignoring!"); + return; + } + RCLCPP_INFO(get_logger(), "Handling LoadMap request"); + // Load from file + if (loadMapResponseFromYaml(request->map_url, response)) { + auto occ_grid = std::make_unique(msg_); + occ_pub_->publish(std::move(occ_grid)); // publish new map + } +} + +bool MapServer::loadMapResponseFromYaml( + const std::string & yaml_file, + std::shared_ptr response) +{ + switch (loadMapFromYaml(yaml_file, msg_)) { + case MAP_DOES_NOT_EXIST: + response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST; + return false; + case INVALID_MAP_METADATA: + response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA; + return false; + case INVALID_MAP_DATA: + response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA; + return false; + case LOAD_MAP_SUCCESS: + // Correcting msg_ header when it belongs to spiecific node + updateMsgHeader(); + + response->map = msg_; + response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS; + } + + return true; +} + +void MapServer::updateMsgHeader() +{ + msg_.info.map_load_time = now(); + msg_.header.frame_id = frame_id_; + msg_.header.stamp = now(); +} + +} // namespace nav2_map_server diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp deleted file mode 100644 index 4d8b816af4..0000000000 --- a/nav2_map_server/src/occ_grid_loader.cpp +++ /dev/null @@ -1,363 +0,0 @@ -/* Copyright 2019 Rover Robotics - * Copyright 2018 Brian Gerkey - * Copyright (c) 2008, Willow Garage, Inc. - * - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include "nav2_map_server/occ_grid_loader.hpp" - -#include -#include -#include -#include -#include - -#include "Magick++.h" -#include "tf2/LinearMath/Quaternion.h" -#include "yaml-cpp/yaml.h" -#include "nav2_util/geometry_utils.hpp" -#include "lifecycle_msgs/msg/state.hpp" - -using namespace std::chrono_literals; - -namespace nav2_map_server -{ -using nav2_util::geometry_utils::orientationAroundZAxis; - -OccGridLoader::OccGridLoader( - rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & yaml_filename) -: node_(node), yaml_filename_(yaml_filename) -{ - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Creating"); -} - -OccGridLoader::~OccGridLoader() {RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Destroying");} - -/// Get the given subnode value. -/// The only reason this function exists is to wrap the exceptions in slightly nicer error messages, -/// including the name of the failed key -/// @throw YAML::Exception -template -T yaml_get_value(const YAML::Node & node, const std::string & key) -{ - try { - return node[key].as(); - } catch (YAML::Exception & e) { - std::stringstream ss; - ss << "Failed to parse YAML tag '" << key << "' for reason: " << e.msg; - throw YAML::Exception(e.mark, ss.str()); - } -} - -OccGridLoader::LoadParameters OccGridLoader::load_map_yaml(const std::string & yaml_filename) -{ - YAML::Node doc = YAML::LoadFile(yaml_filename); - LoadParameters loadParameters; - - auto image_file_name = yaml_get_value(doc, "image"); - if (image_file_name.empty()) { - throw YAML::Exception(doc["image"].Mark(), "The image tag was empty."); - } - if (image_file_name[0] != '/') { - // dirname takes a mutable char *, so we copy into a vector - std::vector fname_copy(yaml_filename.begin(), yaml_filename.end()); - fname_copy.push_back('\0'); - image_file_name = std::string(dirname(fname_copy.data())) + '/' + image_file_name; - } - loadParameters.image_file_name = image_file_name; - - loadParameters.resolution = yaml_get_value(doc, "resolution"); - loadParameters.origin = yaml_get_value>(doc, "origin"); - if (loadParameters.origin.size() != 3) { - throw YAML::Exception( - doc["origin"].Mark(), "value of the 'origin' tag should have 3 elements, not " + - std::to_string(loadParameters.origin.size())); - } - - loadParameters.free_thresh = yaml_get_value(doc, "free_thresh"); - loadParameters.occupied_thresh = yaml_get_value(doc, "occupied_thresh"); - - auto map_mode_node = doc["mode"]; - if (!map_mode_node.IsDefined()) { - loadParameters.mode = MapMode::Trinary; - } else { - loadParameters.mode = map_mode_from_string(map_mode_node.as()); - } - - try { - loadParameters.negate = yaml_get_value(doc, "negate"); - } catch (YAML::Exception &) { - loadParameters.negate = yaml_get_value(doc, "negate"); - } - - RCLCPP_DEBUG(node_->get_logger(), "resolution: %f", loadParameters.resolution); - RCLCPP_DEBUG(node_->get_logger(), "origin[0]: %f", loadParameters.origin[0]); - RCLCPP_DEBUG(node_->get_logger(), "origin[1]: %f", loadParameters.origin[1]); - RCLCPP_DEBUG(node_->get_logger(), "origin[2]: %f", loadParameters.origin[2]); - RCLCPP_DEBUG(node_->get_logger(), "free_thresh: %f", loadParameters.free_thresh); - RCLCPP_DEBUG(node_->get_logger(), "occupied_thresh: %f", loadParameters.occupied_thresh); - RCLCPP_DEBUG(node_->get_logger(), "mode: %s", map_mode_to_string(loadParameters.mode)); - RCLCPP_DEBUG(node_->get_logger(), "negate: %d", loadParameters.negate); - - return loadParameters; -} - -bool OccGridLoader::loadMapFromYaml( - std::string yaml_file, - std::shared_ptr response) -{ - if (yaml_file.empty()) { - RCLCPP_ERROR(node_->get_logger(), "YAML file name is empty, can't load!"); - response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST; - return false; - } - RCLCPP_INFO(node_->get_logger(), "Loading yaml file: %s", yaml_file.c_str()); - LoadParameters loadParameters; - try { - loadParameters = load_map_yaml(yaml_file); - } catch (YAML::Exception & e) { - RCLCPP_ERROR( - node_->get_logger(), "Failed processing YAML file %s at position (%d:%d) for reason: %s", - yaml_file.c_str(), e.mark.line, e.mark.column, e.what()); - response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA; - return false; - } catch (std::exception & e) { - RCLCPP_ERROR( - node_->get_logger(), "Failed to parse map YAML loaded from file %s for reason: %s", - yaml_file.c_str(), e.what()); - response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA; - return false; - } - - try { - loadMapFromFile(loadParameters); - } catch (std::exception & e) { - RCLCPP_ERROR( - node_->get_logger(), "Failed to load image file %s for reason: %s", - loadParameters.image_file_name.c_str(), e.what()); - response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA; - return false; - } - return true; -} - -nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring"); - - // initialize Occupancy Grid msg - needed by loadMapFromYaml - msg_ = std::make_unique(); - - if (!loadMapFromYaml(yaml_filename_)) { - throw std::runtime_error("Failed to load map yaml file: " + yaml_filename_); - } - - // Create GetMap service callback handle - auto handle_occ_callback = [this]( - const std::shared_ptr/*request_header*/, - const std::shared_ptr/*request*/, - std::shared_ptr response) -> void { - if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - RCLCPP_WARN( - node_->get_logger(), - "Received GetMap request but not in ACTIVE state, ignoring!"); - return; - } - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling GetMap request"); - response->map = *msg_; - }; - - // Create a service that provides the occupancy grid - occ_service_ = node_->create_service(service_name_, handle_occ_callback); - - // Create the load_map service callback handle - auto load_map_callback = [this]( - const std::shared_ptr/*request_header*/, - const std::shared_ptr request, - std::shared_ptr response) -> void { - // if not in ACTIVE state, ignore request - if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - RCLCPP_WARN( - node_->get_logger(), - "Received LoadMap request but not in ACTIVE state, ignoring!"); - return; - } - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling LoadMap request"); - // Load from file - if (loadMapFromYaml(request->map_url, response)) { - response->map = *msg_; - response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS; - occ_pub_->publish(*msg_); // publish new map - } - }; - - // Create a publisher using the QoS settings to emulate a ROS1 latched topic - occ_pub_ = node_->create_publisher( - topic_name_, - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - - // Create a service that loads the occupancy grid from a file - load_map_service_ = node_->create_service( - load_map_service_name_, - load_map_callback); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn OccGridLoader::on_activate(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Activating"); - - // Publish the map using the latched topic - occ_pub_->on_activate(); - occ_pub_->publish(*msg_); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn OccGridLoader::on_deactivate(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Deactivating"); - - occ_pub_->on_deactivate(); - timer_.reset(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -nav2_util::CallbackReturn OccGridLoader::on_cleanup(const rclcpp_lifecycle::State & /*state*/) -{ - RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Cleaning up"); - - occ_pub_.reset(); - occ_service_.reset(); - load_map_service_.reset(); - msg_.reset(); - - return nav2_util::CallbackReturn::SUCCESS; -} - -void OccGridLoader::loadMapFromFile(const LoadParameters & loadParameters) -{ - Magick::InitializeMagick(nullptr); - nav_msgs::msg::OccupancyGrid msg; - - RCLCPP_INFO( - node_->get_logger(), "Loading image_file: %s", - loadParameters.image_file_name.c_str()); - Magick::Image img(loadParameters.image_file_name); - - // Copy the image data into the map structure - msg.info.width = img.size().width(); - msg.info.height = img.size().height(); - - msg.info.resolution = loadParameters.resolution; - msg.info.origin.position.x = loadParameters.origin[0]; - msg.info.origin.position.y = loadParameters.origin[1]; - msg.info.origin.position.z = 0.0; - msg.info.origin.orientation = orientationAroundZAxis(loadParameters.origin[2]); - - // Allocate space to hold the data - msg.data.resize(msg.info.width * msg.info.height); - - // Copy pixel data into the map structure - for (size_t y = 0; y < msg.info.height; y++) { - for (size_t x = 0; x < msg.info.width; x++) { - auto pixel = img.pixelColor(x, y); - - std::vector channels = {pixel.redQuantum(), pixel.greenQuantum(), - pixel.blueQuantum()}; - if (loadParameters.mode == MapMode::Trinary && img.matte()) { - // To preserve existing behavior, average in alpha with color channels in Trinary mode. - // CAREFUL. alpha is inverted from what you might expect. High = transparent, low = opaque - channels.push_back(MaxRGB - pixel.alphaQuantum()); - } - double sum = 0; - for (auto c : channels) { - sum += c; - } - /// on a scale from 0.0 to 1.0 how bright is the pixel? - double shade = Magick::ColorGray::scaleQuantumToDouble(sum / channels.size()); - - // If negate is true, we consider blacker pixels free, and whiter - // pixels occupied. Otherwise, it's vice versa. - /// on a scale from 0.0 to 1.0, how occupied is the map cell (before thresholding)? - double occ = (loadParameters.negate ? shade : 1.0 - shade); - - int8_t map_cell; - switch (loadParameters.mode) { - case MapMode::Trinary: - if (loadParameters.occupied_thresh < occ) { - map_cell = 100; - } else if (occ < loadParameters.free_thresh) { - map_cell = 0; - } else { - map_cell = -1; - } - break; - case MapMode::Scale: - if (pixel.alphaQuantum() != OpaqueOpacity) { - map_cell = -1; - } else if (loadParameters.occupied_thresh < occ) { - map_cell = 100; - } else if (occ < loadParameters.free_thresh) { - map_cell = 0; - } else { - map_cell = std::rint( - (occ - loadParameters.free_thresh) / - (loadParameters.occupied_thresh - loadParameters.free_thresh) * 100.0); - } - break; - case MapMode::Raw: { - double occ_percent = std::round(shade * 255); - if (0 <= occ_percent && occ_percent <= 100) { - map_cell = static_cast(occ_percent); - } else { - map_cell = -1; - } - break; - } - default: - throw std::runtime_error("Invalid map mode"); - } - msg.data[msg.info.width * (msg.info.height - y - 1) + x] = map_cell; - } - } - - msg.info.map_load_time = node_->now(); - msg.header.frame_id = frame_id_; - msg.header.stamp = node_->now(); - - RCLCPP_DEBUG( - node_->get_logger(), "Read map %s: %d X %d map @ %.3lf m/cell", - loadParameters.image_file_name.c_str(), msg.info.width, msg.info.height, msg.info.resolution); - - *msg_ = msg; -} - -} // namespace nav2_map_server diff --git a/nav2_map_server/test/component/CMakeLists.txt b/nav2_map_server/test/component/CMakeLists.txt index 85c1bba1ae..a394a5201e 100644 --- a/nav2_map_server/test/component/CMakeLists.txt +++ b/nav2_map_server/test/component/CMakeLists.txt @@ -1,22 +1,54 @@ include_directories(${PROJECT_SOURCE_DIR}/test) -ament_add_gtest_executable(test_occ_grid_node - test_occ_grid_node.cpp +# map_server component test +ament_add_gtest_executable(test_map_server_node + test_map_server_node.cpp ${PROJECT_SOURCE_DIR}/test/test_constants.cpp ) -ament_target_dependencies(test_occ_grid_node rclcpp nav_msgs) -target_link_libraries(test_occ_grid_node +ament_target_dependencies(test_map_server_node rclcpp nav_msgs) +target_link_libraries(test_map_server_node ${library_name} stdc++fs ) -# occ_grid component test -ament_add_test(test_occ_grid_node +ament_add_test(test_map_server_node GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_occ_grid_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_server_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" ENV TEST_DIR=${TEST_DIR} TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ + TEST_EXECUTABLE=$ +) + +# map_saver component test +ament_add_gtest_executable(test_map_saver_node + test_map_saver_node.cpp + ${PROJECT_SOURCE_DIR}/test/test_constants.cpp +) + +ament_target_dependencies(test_map_saver_node rclcpp nav_msgs) +target_link_libraries(test_map_saver_node + ${library_name} + stdc++fs +) + +add_executable(test_map_saver_publisher + test_map_saver_publisher.cpp + ${PROJECT_SOURCE_DIR}/test/test_constants.cpp +) + +target_link_libraries(test_map_saver_publisher + ${map_io_library_name} + stdc++fs +) + +ament_add_test(test_map_saver_node + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_saver_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_DIR=${TEST_DIR} + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ ) diff --git a/nav2_map_server/test/component/test_map_saver_launch.py b/nav2_map_server/test/component/test_map_saver_launch.py new file mode 100755 index 0000000000..a491a2d8de --- /dev/null +++ b/nav2_map_server/test/component/test_map_saver_launch.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_testing.legacy import LaunchTestService + + +def main(argv=sys.argv[1:]): + launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py') + testExecutable = os.getenv('TEST_EXECUTABLE') + ld = LaunchDescription([ + IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + ]) + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_map_saver_node', + ) + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + os.chdir(os.getenv('TEST_LAUNCH_DIR')) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_map_server/test/component/test_map_saver_node.cpp b/nav2_map_server/test/component/test_map_saver_node.cpp new file mode 100644 index 0000000000..864ca4c3e3 --- /dev/null +++ b/nav2_map_server/test/component/test_map_saver_node.cpp @@ -0,0 +1,217 @@ +// Copyright (c) 2020 Samsung Research Russia +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "test_constants/test_constants.h" +#include "nav2_map_server/map_saver.hpp" +#include "nav2_util/lifecycle_service_client.hpp" +#include "nav2_msgs/srv/save_map.hpp" + +#define TEST_DIR TEST_DIRECTORY + +using std::experimental::filesystem::path; +using lifecycle_msgs::msg::Transition; +using namespace nav2_map_server; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; + +RclCppFixture g_rclcppfixture; + +class MapSaverTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = rclcpp::Node::make_shared("map_client_test"); + lifecycle_client_ = + std::make_shared("map_saver", node_); + RCLCPP_INFO(node_->get_logger(), "Creating Test Node"); + + std::this_thread::sleep_for(std::chrono::seconds(5)); // allow node to start up + const std::chrono::seconds timeout(5); + lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout); + lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout); + } + + static void TearDownTestCase() + { + lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); + lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); + } + + template + typename T::Response::SharedPtr send_request( + + rclcpp::Node::SharedPtr node, + typename rclcpp::Client::SharedPtr client, + typename T::Request::SharedPtr request) + { + auto result = client->async_send_request(request); + + // Wait for the result + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { + return result.get(); + } else { + return nullptr; + } + } + +protected: + // Check that map_msg corresponds to reference pattern + // Input: map_msg + void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg) + { + ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); + ASSERT_EQ(map_msg.info.width, g_valid_image_width); + ASSERT_EQ(map_msg.info.height, g_valid_image_height); + for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) { + ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]); + } + } + + static rclcpp::Node::SharedPtr node_; + static std::shared_ptr lifecycle_client_; +}; + + +rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr; +std::shared_ptr MapSaverTestFixture::lifecycle_client_ = + nullptr; + +// Send map saving service request. +// Load saved map and verify obtained OccupancyGrid. +TEST_F(MapSaverTestFixture, SaveMap) +{ + RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service"); + auto req = std::make_shared(); + auto client = node_->create_client( + "/map_saver/save_map"); + + RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service"); + ASSERT_TRUE(client->wait_for_service()); + + // 1. Send valid save_map serivce request + req->map_topic = "map"; + req->map_url = path(g_tmp_dir) / path(g_valid_map_name); + req->image_format = "png"; + req->map_mode = "trinary"; + req->free_thresh = g_default_free_thresh; + req->occupied_thresh = g_default_occupied_thresh; + auto resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, true); + + // 2. Load saved map and verify it + nav_msgs::msg::OccupancyGrid map_msg; + LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + verifyMapMsg(map_msg); +} + +// Send map saving service request with default parameters. +// Load saved map and verify obtained OccupancyGrid. +TEST_F(MapSaverTestFixture, SaveMapDefaultParameters) +{ + RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service"); + auto req = std::make_shared(); + auto client = node_->create_client( + "/map_saver/save_map"); + + RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service"); + ASSERT_TRUE(client->wait_for_service()); + + // 1. Send save_map serivce request with default parameters + req->map_topic = ""; + req->map_url = path(g_tmp_dir) / path(g_valid_map_name); + req->image_format = ""; + req->map_mode = ""; + req->free_thresh = 0.0; + req->occupied_thresh = 0.0; + auto resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, true); + + // 2. Load saved map and verify it + nav_msgs::msg::OccupancyGrid map_msg; + LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + verifyMapMsg(map_msg); +} + +// Send map saving service requests with different sets of parameters. +// In case of map is expected to be saved correctly, load map from a saved +// file and verify obtained OccupancyGrid. +TEST_F(MapSaverTestFixture, SaveMapInvalidParameters) +{ + RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service"); + auto req = std::make_shared(); + auto client = node_->create_client( + "/map_saver/save_map"); + + RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service"); + ASSERT_TRUE(client->wait_for_service()); + + // 1. Trying to send save_map serivce request with different sets of parameters + // In case of map is expected to be saved correctly, verify it + req->map_topic = "invalid_map"; + req->map_url = path(g_tmp_dir) / path(g_valid_map_name); + req->image_format = "png"; + req->map_mode = "trinary"; + req->free_thresh = g_default_free_thresh; + req->occupied_thresh = g_default_occupied_thresh; + auto resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, false); + + req->map_topic = "map"; + req->image_format = "invalid_format"; + resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, true); + nav_msgs::msg::OccupancyGrid map_msg; + LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + verifyMapMsg(map_msg); + + req->image_format = "png"; + req->map_mode = "invalid_mode"; + resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, true); + status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + verifyMapMsg(map_msg); + + req->map_mode = "trinary"; + req->free_thresh = 2.0; + req->occupied_thresh = 2.0; + resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, false); + + req->free_thresh = -2.0; + req->occupied_thresh = -2.0; + resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, false); + + req->free_thresh = 0.7; + req->occupied_thresh = 0.2; + resp = send_request(node_, client, req); + ASSERT_EQ(resp->result, false); +} diff --git a/nav2_map_server/test/component/test_map_saver_publisher.cpp b/nav2_map_server/test/component/test_map_saver_publisher.cpp new file mode 100644 index 0000000000..50d66b0a5e --- /dev/null +++ b/nav2_map_server/test/component/test_map_saver_publisher.cpp @@ -0,0 +1,68 @@ +// Copyright (c) 2020 Samsung Research Russia +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_map_server/map_io.hpp" +#include "test_constants/test_constants.h" + +#define TEST_DIR TEST_DIRECTORY + +using namespace std::chrono_literals; +using namespace nav2_map_server; // NOLINT +using std::experimental::filesystem::path; + +class TestPublisher : public rclcpp::Node +{ +public: + TestPublisher() + : Node("map_publisher") + { + std::string pub_map_file = path(TEST_DIR) / path(g_valid_yaml_file); + LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg_); + if (status != LOAD_MAP_SUCCESS) { + RCLCPP_ERROR(get_logger(), "Can not load %s map file", pub_map_file); + return; + } + + map_pub_ = create_publisher( + "map", + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + timer_ = create_wall_timer(300ms, std::bind(&TestPublisher::mapPublishCallback, this)); + } + +protected: + void mapPublishCallback() + { + map_pub_->publish(msg_); + } + + rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::TimerBase::SharedPtr timer_; + nav_msgs::msg::OccupancyGrid msg_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto pub_node = std::make_shared(); + rclcpp::spin(pub_node); + rclcpp::shutdown(); + return 0; +} diff --git a/nav2_map_server/test/component/test_occ_grid_launch.py b/nav2_map_server/test/component/test_map_server_launch.py similarity index 97% rename from nav2_map_server/test/component/test_occ_grid_launch.py rename to nav2_map_server/test/component/test_map_server_launch.py index d2f15fee26..8164294085 100755 --- a/nav2_map_server/test/component/test_occ_grid_launch.py +++ b/nav2_map_server/test/component/test_map_server_launch.py @@ -33,7 +33,7 @@ def main(argv=sys.argv[1:]): ]) test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_occ_grid_node', + name='test_map_server_node', ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_map_server/test/component/test_occ_grid_node.cpp b/nav2_map_server/test/component/test_map_server_node.cpp similarity index 66% rename from nav2_map_server/test/component/test_occ_grid_node.cpp rename to nav2_map_server/test/component/test_map_server_node.cpp index 4185a1cb4d..373f878305 100644 --- a/nav2_map_server/test/component/test_occ_grid_node.cpp +++ b/nav2_map_server/test/component/test_map_server_node.cpp @@ -15,12 +15,13 @@ #include #include #include +#include #include #include "test_constants/test_constants.h" -#include "nav2_map_server/occ_grid_loader.hpp" +#include "nav2_map_server/map_server.hpp" #include "nav2_util/lifecycle_service_client.hpp" -#include "nav2_msgs/srv/load_map.hpp" +#include "nav2_msgs/srv/load_map.hpp" #define TEST_DIR TEST_DIRECTORY @@ -37,24 +38,23 @@ class RclCppFixture RclCppFixture g_rclcppfixture; -class TestNode : public ::testing::Test +class MapServerTestFixture : public ::testing::Test { public: - TestNode() + static void SetUpTestCase() { node_ = rclcpp::Node::make_shared("map_client_test"); lifecycle_client_ = std::make_shared("map_server", node_); RCLCPP_INFO(node_->get_logger(), "Creating Test Node"); - - std::this_thread::sleep_for(std::chrono::seconds(1)); // allow node to start up + std::this_thread::sleep_for(std::chrono::seconds(5)); // allow node to start up const std::chrono::seconds timeout(5); lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout); lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout); } - ~TestNode() + static void TearDownTestCase() { lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE); lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP); @@ -62,7 +62,6 @@ class TestNode : public ::testing::Test template typename T::Response::SharedPtr send_request( - rclcpp::Node::SharedPtr node, typename rclcpp::Client::SharedPtr client, typename T::Request::SharedPtr request) @@ -70,10 +69,7 @@ class TestNode : public ::testing::Test auto result = client->async_send_request(request); // Wait for the result - if ( - rclcpp::spin_until_future_complete(node, result) == - rclcpp::executor::FutureReturnCode::SUCCESS) - { + if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) { return result.get(); } else { return nullptr; @@ -81,35 +77,51 @@ class TestNode : public ::testing::Test } protected: - rclcpp::Node::SharedPtr node_; - std::shared_ptr lifecycle_client_; + // Check that map_msg corresponds to reference pattern + // Input: map_msg + void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg) + { + ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); + ASSERT_EQ(map_msg.info.width, g_valid_image_width); + ASSERT_EQ(map_msg.info.height, g_valid_image_height); + for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) { + ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]); + } + } + + static rclcpp::Node::SharedPtr node_; + static std::shared_ptr lifecycle_client_; }; -TEST_F(TestNode, GetMap) + +rclcpp::Node::SharedPtr MapServerTestFixture::node_ = nullptr; +std::shared_ptr MapServerTestFixture::lifecycle_client_ = + nullptr; + + +// Send map getting service request and verify obtained OccupancyGrid +TEST_F(MapServerTestFixture, GetMap) { RCLCPP_INFO(node_->get_logger(), "Testing GetMap service"); auto req = std::make_shared(); - auto client = node_->create_client("map"); + auto client = node_->create_client( + "/map_server/map"); RCLCPP_INFO(node_->get_logger(), "Waiting for map service"); ASSERT_TRUE(client->wait_for_service()); auto resp = send_request(node_, client, req); - ASSERT_FLOAT_EQ(resp->map.info.resolution, g_valid_image_res); - ASSERT_EQ(resp->map.info.width, g_valid_image_width); - ASSERT_EQ(resp->map.info.height, g_valid_image_height); - - for (unsigned int i = 0; i < resp->map.info.width * resp->map.info.height; i++) { - ASSERT_EQ(g_valid_image_content[i], resp->map.data[i]); - } + verifyMapMsg(resp->map); } -TEST_F(TestNode, LoadMap) +// Send map loading service request and verify obtained OccupancyGrid +TEST_F(MapServerTestFixture, LoadMap) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); - auto client = node_->create_client("load_map"); + auto client = node_->create_client( + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); @@ -118,19 +130,16 @@ TEST_F(TestNode, LoadMap) auto resp = send_request(node_, client, req); ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS); - ASSERT_FLOAT_EQ(resp->map.info.resolution, g_valid_image_res); - ASSERT_EQ(resp->map.info.height, g_valid_image_height); - - for (unsigned int i = 0; i < resp->map.info.width * resp->map.info.height; i++) { - ASSERT_EQ(g_valid_image_content[i], resp->map.data[i]); - } + verifyMapMsg(resp->map); } -TEST_F(TestNode, LoadMapNull) +// Send map loading service request without specifying which map to load +TEST_F(MapServerTestFixture, LoadMapNull) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); - auto client = node_->create_client("load_map"); + auto client = node_->create_client( + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); @@ -142,11 +151,13 @@ TEST_F(TestNode, LoadMapNull) ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST); } -TEST_F(TestNode, LoadMapInvalidYaml) +// Send map loading service request with non-existing yaml file +TEST_F(MapServerTestFixture, LoadMapInvalidYaml) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); - auto client = node_->create_client("load_map"); + auto client = node_->create_client( + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); @@ -158,11 +169,13 @@ TEST_F(TestNode, LoadMapInvalidYaml) ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA); } -TEST_F(TestNode, LoadMapInvalidImage) +// Send map loading service request with yaml file containing non-existing map +TEST_F(MapServerTestFixture, LoadMapInvalidImage) { RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service"); auto req = std::make_shared(); - auto client = node_->create_client("load_map"); + auto client = node_->create_client( + "/map_server/load_map"); RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service"); ASSERT_TRUE(client->wait_for_service()); diff --git a/nav2_map_server/test/invalid_image.yaml b/nav2_map_server/test/invalid_image.yaml index bcdc605c26..1161b0a67e 100644 --- a/nav2_map_server/test/invalid_image.yaml +++ b/nav2_map_server/test/invalid_image.yaml @@ -1,4 +1,3 @@ -map_type: "occupancy" image: "invalid.png" resolution: 0.1 origin: [2.0, 3.0, 1.0] diff --git a/nav2_map_server/test/map_saver_params.yaml b/nav2_map_server/test/map_saver_params.yaml new file mode 100644 index 0000000000..25d6b88b2c --- /dev/null +++ b/nav2_map_server/test/map_saver_params.yaml @@ -0,0 +1,5 @@ +map_saver: + ros__parameters: + save_map_timeout: 1000 + free_thresh_default: 0.196 + occupied_thresh_default: 0.65 diff --git a/nav2_map_server/test/test_constants.cpp b/nav2_map_server/test/test_constants.cpp index 5d17cb8b31..3edec23b5b 100644 --- a/nav2_map_server/test/test_constants.cpp +++ b/nav2_map_server/test/test_constants.cpp @@ -35,6 +35,8 @@ #include "test_constants/test_constants.h" +#include + const unsigned int g_valid_image_width = 10; const unsigned int g_valid_image_height = 10; // Note that the image content is given in row-major order, with the @@ -55,8 +57,14 @@ const char g_valid_image_content[] = { 100, 0, 0, 0, 0, 0, 0, 0, 0, 0, }; +const char * g_valid_map_name = "testmap"; const char * g_valid_png_file = "testmap.png"; const char * g_valid_bmp_file = "testmap.bmp"; +const char * g_valid_pgm_file = "testmap.pgm"; const char * g_valid_yaml_file = "testmap.yaml"; +const char * g_tmp_dir = "/tmp"; -const float g_valid_image_res = 0.1; +const double g_valid_image_res = 0.1; +const std::vector g_valid_origin{2.0, 3.0, 1.0}; +const double g_default_free_thresh = 0.196; +const double g_default_occupied_thresh = 0.65; diff --git a/nav2_map_server/test/test_constants/test_constants.h b/nav2_map_server/test/test_constants/test_constants.h index 08c7da9d8d..285c00ca44 100644 --- a/nav2_map_server/test/test_constants/test_constants.h +++ b/nav2_map_server/test/test_constants/test_constants.h @@ -33,13 +33,24 @@ /* This file externs global constants shared among tests */ +#include + extern const unsigned int g_valid_image_width; extern const unsigned int g_valid_image_height; extern const char g_valid_image_content[]; +extern const char * g_valid_map_name; extern const char * g_valid_png_file; extern const char * g_valid_bmp_file; +extern const char * g_valid_pgm_file; extern const char * g_valid_yaml_file; +extern const char * g_tmp_dir; -extern const float g_valid_image_res; +extern const double g_valid_image_res; +// *INDENT-OFF* +// Uncrustify may incorrectly guide to add extra spaces in < double > during CI tests +extern const std::vector g_valid_origin; +// *INDENT-ON* +extern const double g_default_free_thresh; +extern const double g_default_occupied_thresh; #endif // TEST_CONSTANTS__TEST_CONSTANTS_H_ diff --git a/nav2_map_server/test/test_launch_files/map_saver_node.launch.py b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py new file mode 100644 index 0000000000..9c061439c7 --- /dev/null +++ b/nav2_map_server/test/test_launch_files/map_saver_node.launch.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +import launch_ros.actions + + +def generate_launch_description(): + map_publisher = os.path.dirname(os.getenv('TEST_EXECUTABLE')) + '/test_map_saver_publisher' + + ld = LaunchDescription() + + map_saver_server_cmd = launch_ros.actions.Node( + package='nav2_map_server', + executable='map_saver_server', + output='screen', + parameters=[os.path.join(os.getenv('TEST_DIR'), + 'map_saver_params.yaml')]) + + map_publisher_cmd = ExecuteProcess( + cmd=[map_publisher]) + + ld.add_action(map_saver_server_cmd) + ld.add_action(map_publisher_cmd) + + return ld diff --git a/nav2_map_server/test/test_launch_files/map_server_node.launch.py b/nav2_map_server/test/test_launch_files/map_server_node.launch.py index ff9767573c..78d5e8b6d1 100644 --- a/nav2_map_server/test/test_launch_files/map_server_node.launch.py +++ b/nav2_map_server/test/test_launch_files/map_server_node.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): return LaunchDescription([ launch_ros.actions.Node( package='nav2_map_server', - node_executable='map_server', + executable='map_server', output='screen', parameters=[os.path.join(os.getenv('TEST_DIR'), 'map_server_params.yaml')]) diff --git a/nav2_map_server/test/testmap.pgm b/nav2_map_server/test/testmap.pgm new file mode 100644 index 0000000000..553ff71acd Binary files /dev/null and b/nav2_map_server/test/testmap.pgm differ diff --git a/nav2_map_server/test/testmap.yaml b/nav2_map_server/test/testmap.yaml index fe6b2a681b..1425aa7ea0 100644 --- a/nav2_map_server/test/testmap.yaml +++ b/nav2_map_server/test/testmap.yaml @@ -1,4 +1,3 @@ -map_type: "occupancy" image: "testmap.png" resolution: 0.1 origin: [2.0, 3.0, 1.0] diff --git a/nav2_map_server/test/unit/CMakeLists.txt b/nav2_map_server/test/unit/CMakeLists.txt index 173d303b11..e0326d462c 100644 --- a/nav2_map_server/test/unit/CMakeLists.txt +++ b/nav2_map_server/test/unit/CMakeLists.txt @@ -1,11 +1,11 @@ include_directories(${PROJECT_SOURCE_DIR}/test) -ament_add_gtest(test_occ_grid test_occ_grid.cpp ${PROJECT_SOURCE_DIR}/test/test_constants.cpp) +# map_io unit test +ament_add_gtest(test_map_io test_map_io.cpp ${PROJECT_SOURCE_DIR}/test/test_constants.cpp) -ament_target_dependencies(test_occ_grid rclcpp nav_msgs) +ament_target_dependencies(test_map_io rclcpp nav_msgs) -target_link_libraries(test_occ_grid - ${library_name} +target_link_libraries(test_map_io + ${map_io_library_name} stdc++fs ) - diff --git a/nav2_map_server/test/unit/test_map_io.cpp b/nav2_map_server/test/unit/test_map_io.cpp new file mode 100644 index 0000000000..419ce669ef --- /dev/null +++ b/nav2_map_server/test/unit/test_map_io.cpp @@ -0,0 +1,313 @@ +// Copyright 2019 Rover Robotics + +/* + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* Author: Brian Gerkey */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "yaml-cpp/yaml.h" +#include "nav2_map_server/map_io.hpp" +#include "nav2_map_server/map_server.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "test_constants/test_constants.h" + +#define TEST_DIR TEST_DIRECTORY + +using namespace std; // NOLINT +using namespace nav2_map_server; // NOLINT +using std::experimental::filesystem::path; + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; + +RclCppFixture g_rclcppfixture; + +class MapIOTester : public ::testing::Test +{ +protected: + // Fill LoadParameters with standard for testing values + // Input: image_file_name + // Output: load_parameters + void fillLoadParameters( + const std::string & image_file_name, + LoadParameters & load_parameters) + { + load_parameters.image_file_name = image_file_name; + load_parameters.resolution = g_valid_image_res; + load_parameters.origin = g_valid_origin; + load_parameters.free_thresh = g_default_free_thresh; + load_parameters.occupied_thresh = g_default_occupied_thresh; + load_parameters.mode = MapMode::Trinary; + load_parameters.negate = 0; + } + + // Fill SaveParameters with standard for testing values + // Input: map_file_name, image_format + // Output: save_parameters + void fillSaveParameters( + const std::string & map_file_name, + const std::string & image_format, + SaveParameters & save_parameters) + { + save_parameters.map_file_name = map_file_name; + save_parameters.image_format = image_format; + save_parameters.free_thresh = g_default_free_thresh; + save_parameters.occupied_thresh = g_default_occupied_thresh; + save_parameters.mode = MapMode::Trinary; + } + + // Check that map_msg corresponds to reference pattern + // Input: map_msg + void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg) + { + ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); + ASSERT_EQ(map_msg.info.width, g_valid_image_width); + ASSERT_EQ(map_msg.info.height, g_valid_image_height); + for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) { + ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]); + } + } +}; + +// Load a valid reference PGM file. Check obtained OccupancyGrid message for consistency: +// loaded image should match the known dimensions and content of the file. +// Save obtained OccupancyGrid message into a tmp PGM file. Then load back saved tmp file +// and check for consistency. +// Succeeds all steps were passed without a problem or expection. +TEST_F(MapIOTester, loadSaveValidPGM) +{ + // 1. Load reference map file and verify obtained OccupancyGrid + LoadParameters loadParameters; + fillLoadParameters(path(TEST_DIR) / path(g_valid_pgm_file), loadParameters); + + nav_msgs::msg::OccupancyGrid map_msg; + ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg)); + + verifyMapMsg(map_msg); + + // 2. Save OccupancyGrid into a tmp file + SaveParameters saveParameters; + fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "pgm", saveParameters); + + ASSERT_TRUE(saveMapToFile(map_msg, saveParameters)); + + // 3. Load saved map and verify it + LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + + verifyMapMsg(map_msg); +} + +// Load a valid reference PNG file. Check obtained OccupancyGrid message for consistency: +// loaded image should match the known dimensions and content of the file. +// Save obtained OccupancyGrid message into a tmp PNG file. Then load back saved tmp file +// and check for consistency. +// Succeeds all steps were passed without a problem or expection. +TEST_F(MapIOTester, loadSaveValidPNG) +{ + // 1. Load reference map file and verify obtained OccupancyGrid + LoadParameters loadParameters; + fillLoadParameters(path(TEST_DIR) / path(g_valid_png_file), loadParameters); + + nav_msgs::msg::OccupancyGrid map_msg; + ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg)); + + verifyMapMsg(map_msg); + + // 2. Save OccupancyGrid into a tmp file + SaveParameters saveParameters; + fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "png", saveParameters); + + ASSERT_TRUE(saveMapToFile(map_msg, saveParameters)); + + // 3. Load saved map and verify it + LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + + verifyMapMsg(map_msg); +} + +// Load a valid reference BMP file. Check obtained OccupancyGrid message for consistency: +// loaded image should match the known dimensions and content of the file. +// Save obtained OccupancyGrid message into a tmp BMP file. Then load back saved tmp file +// and check for consistency. +// Succeeds all steps were passed without a problem or expection. +TEST_F(MapIOTester, loadSaveValidBMP) +{ + // 1. Load reference map file and verify obtained OccupancyGrid + auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file); + + LoadParameters loadParameters; + fillLoadParameters(test_bmp, loadParameters); + + nav_msgs::msg::OccupancyGrid map_msg; + ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg)); + + verifyMapMsg(map_msg); + + // 2. Save OccupancyGrid into a tmp file + SaveParameters saveParameters; + fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "bmp", saveParameters); + + ASSERT_TRUE(saveMapToFile(map_msg, saveParameters)); + + // 3. Load saved map and verify it + LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + + verifyMapMsg(map_msg); +} + +// Load map from a valid file. Trying to save map with different modes. +// Succeeds all steps were passed without a problem or expection. +TEST_F(MapIOTester, loadSaveMapModes) +{ + // 1. Load map from YAML file + nav_msgs::msg::OccupancyGrid map_msg; + LOAD_MAP_STATUS status = loadMapFromYaml(path(TEST_DIR) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + + // No need to check Trinary mode. This already verified in previous testcases. + // 2. Save map in Scale mode. + SaveParameters saveParameters; + fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "png", saveParameters); + saveParameters.mode = MapMode::Scale; + + ASSERT_TRUE(saveMapToFile(map_msg, saveParameters)); + + // 3. Load saved map and verify it + status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + + verifyMapMsg(map_msg); + + // 4. Save map in Raw mode. + saveParameters.mode = MapMode::Raw; + + ASSERT_TRUE(saveMapToFile(map_msg, saveParameters)); + + // 5. Load saved map and verify it + status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + + verifyMapMsg(map_msg); +} + +// Try to load an invalid file with different ways. +// Succeeds if all cases are got expected fail behaviours. +TEST_F(MapIOTester, loadInvalidFile) +{ + // 1. Trying to load incorrect map by loadMapFromFile() + auto test_invalid = path(TEST_DIR) / path("foo"); + + LoadParameters loadParameters; + fillLoadParameters(test_invalid, loadParameters); + + nav_msgs::msg::OccupancyGrid map_msg; + ASSERT_ANY_THROW(loadMapFromFile(loadParameters, map_msg)); + + // 2. Trying to load incorrect map by loadMapFromYaml() + LOAD_MAP_STATUS status = loadMapFromYaml("", map_msg); + ASSERT_EQ(status, MAP_DOES_NOT_EXIST); + + status = loadMapFromYaml(std::string(test_invalid) + ".yaml", map_msg); + ASSERT_EQ(status, INVALID_MAP_METADATA); +} + +// Load map from a valid file. Trying to save map with different sets of parameters. +// Succeeds if all cases got expected behaviours. +TEST_F(MapIOTester, saveInvalidParameters) +{ + // 1. Load map from YAML file + nav_msgs::msg::OccupancyGrid map_msg; + LOAD_MAP_STATUS status = loadMapFromYaml(path(TEST_DIR) / path(g_valid_yaml_file), map_msg); + ASSERT_EQ(status, LOAD_MAP_SUCCESS); + + // 2. Trying to save map with different sets of parameters + SaveParameters saveParameters; + + saveParameters.map_file_name = path(g_tmp_dir) / path(g_valid_map_name); + saveParameters.image_format = ""; + saveParameters.free_thresh = 2.0; + saveParameters.occupied_thresh = 2.0; + saveParameters.mode = MapMode::Trinary; + ASSERT_FALSE(saveMapToFile(map_msg, saveParameters)); + + saveParameters.free_thresh = -2.0; + saveParameters.occupied_thresh = -2.0; + ASSERT_FALSE(saveMapToFile(map_msg, saveParameters)); + + saveParameters.free_thresh = 0.7; + saveParameters.occupied_thresh = 0.2; + ASSERT_FALSE(saveMapToFile(map_msg, saveParameters)); + + saveParameters.free_thresh = 0.0; + saveParameters.occupied_thresh = 0.0; + ASSERT_TRUE(saveMapToFile(map_msg, saveParameters)); + + saveParameters.map_file_name = path("/invalid_path") / path(g_valid_map_name); + ASSERT_FALSE(saveMapToFile(map_msg, saveParameters)); +} + +// Load valid YAML file and check for consistency +TEST_F(MapIOTester, loadValidYAML) +{ + LoadParameters loadParameters; + ASSERT_NO_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path(g_valid_yaml_file))); + + LoadParameters refLoadParameters; + fillLoadParameters(path(TEST_DIR) / path(g_valid_png_file), refLoadParameters); + ASSERT_EQ(loadParameters.image_file_name, refLoadParameters.image_file_name); + ASSERT_FLOAT_EQ(loadParameters.resolution, refLoadParameters.resolution); + ASSERT_EQ(loadParameters.origin, refLoadParameters.origin); + ASSERT_FLOAT_EQ(loadParameters.free_thresh, refLoadParameters.free_thresh); + ASSERT_FLOAT_EQ(loadParameters.occupied_thresh, refLoadParameters.occupied_thresh); + ASSERT_EQ(loadParameters.mode, refLoadParameters.mode); + ASSERT_EQ(loadParameters.negate, refLoadParameters.negate); +} + +// Try to load invalid YAML file +TEST_F(MapIOTester, loadInvalidYAML) +{ + LoadParameters loadParameters; + ASSERT_ANY_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path("invalid_file.yaml"))); +} diff --git a/nav2_map_server/test/unit/test_occ_grid.cpp b/nav2_map_server/test/unit/test_occ_grid.cpp deleted file mode 100644 index 0b14e3682f..0000000000 --- a/nav2_map_server/test/unit/test_occ_grid.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// Copyright 2019 Rover Robotics - -/* - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* Author: Brian Gerkey */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "yaml-cpp/yaml.h" -#include "nav2_map_server/occ_grid_loader.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "test_constants/test_constants.h" - -#define TEST_DIR TEST_DIRECTORY - -using namespace std; // NOLINT -using std::experimental::filesystem::path; - -class RclCppFixture -{ -public: - RclCppFixture() {rclcpp::init(0, nullptr);} - ~RclCppFixture() {rclcpp::shutdown();} -}; - -RclCppFixture g_rclcppfixture; - -class TestMapLoader : public nav2_map_server::OccGridLoader -{ - FRIEND_TEST(MapLoaderTest, loadValidPNG); - FRIEND_TEST(MapLoaderTest, loadValidBMP); - FRIEND_TEST(MapLoaderTest, loadInvalidFile); - -public: - explicit TestMapLoader(nav2_util::LifecycleNode::SharedPtr node, std::string yaml_filename) - : OccGridLoader(node, yaml_filename) - { - } - - nav_msgs::msg::OccupancyGrid getOccupancyGrid() - { - return *msg_; - } -}; - -class FakeMapServer : public nav2_util::LifecycleNode -{ -public: - FakeMapServer() - : nav2_util::LifecycleNode("FakeMapServer") {} - - nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override - { - return nav2_util::CallbackReturn::SUCCESS; - } - nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override - { - return nav2_util::CallbackReturn::SUCCESS; - } - nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override - { - return nav2_util::CallbackReturn::SUCCESS; - } - nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override - { - return nav2_util::CallbackReturn::SUCCESS; - } -}; - -class MapLoaderTest : public ::testing::Test -{ -public: - MapLoaderTest() - { - // Set up a fake YAML document with the required fields - - std::string temp_name("/tmp/map_unit_test.yaml"); - std::ofstream params_file; - params_file.open(temp_name, std::ofstream::out | std::ofstream::trunc); - - params_file << "resolution: 0.1" << std::endl; - params_file << "origin: [2.0, 3.0, 1.0]" << std::endl; - params_file << "negate: 0" << std::endl; - params_file << "occupied_thresh: 0.65" << std::endl; - params_file << "free_thresh: 0.196" << std::endl; - - params_file.close(); - - node_ = std::make_shared(); - map_loader_ = std::make_unique(node_, temp_name); - } - -protected: - nav2_util::LifecycleNode::SharedPtr node_; - std::unique_ptr map_loader_; -}; - -// Try to load a valid PNG file. Succeeds if no exception is thrown, and if -// the loaded image matches the known dimensions and content of the file. - -TEST_F(MapLoaderTest, loadValidPNG) -{ - auto test_png = path(TEST_DIR) / path(g_valid_png_file); - - TestMapLoader::LoadParameters loadParameters; - loadParameters.image_file_name = test_png; - loadParameters.resolution = g_valid_image_res; - loadParameters.origin[0] = 0; - loadParameters.origin[1] = 0; - loadParameters.origin[2] = 0; - loadParameters.free_thresh = 0.196; - loadParameters.occupied_thresh = 0.65; - loadParameters.mode = nav2_map_server::MapMode::Trinary; - loadParameters.negate = 0; - - // In order to loadMapFromFile without going through the Configure and Activate states, - // the msg_ member must be initialized - map_loader_->msg_ = std::make_unique(); - - ASSERT_NO_THROW(map_loader_->loadMapFromFile(loadParameters)); - nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid(); - - EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); - EXPECT_EQ(map_msg.info.width, g_valid_image_width); - EXPECT_EQ(map_msg.info.height, g_valid_image_height); - for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) { - EXPECT_EQ(g_valid_image_content[i], map_msg.data[i]); - } -} - -// Try to load a valid BMP file. Succeeds if no exception is thrown, and if -// the loaded image matches the known dimensions and content of the file. - -TEST_F(MapLoaderTest, loadValidBMP) -{ - auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file); - - TestMapLoader::LoadParameters loadParameters; - loadParameters.image_file_name = test_bmp; - loadParameters.resolution = g_valid_image_res; - loadParameters.origin[0] = 0; - loadParameters.origin[1] = 0; - loadParameters.origin[2] = 0; - loadParameters.free_thresh = 0.196; - loadParameters.occupied_thresh = 0.65; - loadParameters.mode = nav2_map_server::MapMode::Trinary; - loadParameters.negate = 0; - - // In order to loadMapFromFile without going through the Configure and Activate states, - // the msg_ member must be initialized - map_loader_->msg_ = std::make_unique(); - - ASSERT_NO_THROW(map_loader_->loadMapFromFile(loadParameters)); - nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid(); - - EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res); - EXPECT_EQ(map_msg.info.width, g_valid_image_width); - EXPECT_EQ(map_msg.info.height, g_valid_image_height); - for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) { - EXPECT_EQ(g_valid_image_content[i], map_msg.data[i]); - } -} - -// Try to load an invalid file. Succeeds if a std::runtime exception is thrown - -TEST_F(MapLoaderTest, loadInvalidFile) -{ - auto test_invalid = path(TEST_DIR) / path("foo"); - - TestMapLoader::LoadParameters loadParameters; - loadParameters.image_file_name = test_invalid; - loadParameters.resolution = g_valid_image_res; - loadParameters.origin[0] = 0; - loadParameters.origin[1] = 0; - loadParameters.origin[2] = 0; - loadParameters.free_thresh = 0.196; - loadParameters.occupied_thresh = 0.65; - loadParameters.mode = nav2_map_server::MapMode::Trinary; - loadParameters.negate = 0; - - ASSERT_ANY_THROW(map_loader_->loadMapFromFile(loadParameters)); -} diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 97a0c68e43..1ecdf58b0f 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -18,12 +18,15 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/VoxelGrid.msg" "msg/BehaviorTreeStatusChange.msg" "msg/BehaviorTreeLog.msg" + "msg/Particle.msg" + "msg/ParticleCloud.msg" "srv/GetCostmap.srv" "srv/ClearCostmapExceptRegion.srv" "srv/ClearCostmapAroundRobot.srv" "srv/ClearEntireCostmap.srv" "srv/ManageLifecycleNodes.srv" "srv/LoadMap.srv" + "srv/SaveMap.srv" "action/BackUp.action" "action/ComputePathToPose.action" "action/FollowPath.action" @@ -31,7 +34,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/Wait.action" "action/Spin.action" "action/DummyRecovery.action" - "action/RandomCrawl.action" "action/FollowWaypoints.action" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs ) diff --git a/nav2_msgs/action/BackUp.action b/nav2_msgs/action/BackUp.action index e8be2e5983..906bdd9fff 100644 --- a/nav2_msgs/action/BackUp.action +++ b/nav2_msgs/action/BackUp.action @@ -5,4 +5,4 @@ float32 speed #result definition std_msgs/Empty result --- -#feedback +float32 distance_traveled diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index e1241fe36c..8b9384492c 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -6,3 +6,5 @@ string controller_id std_msgs/Empty result --- #feedback +float32 distance_to_goal +float32 speed diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index 10c7d5de2c..d047dad866 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -4,4 +4,7 @@ geometry_msgs/PoseStamped pose #result definition std_msgs/Empty result --- -#feedback +geometry_msgs/PoseStamped current_pose +builtin_interfaces/Duration navigation_time +int16 number_of_recoveries +float32 distance_remaining diff --git a/nav2_msgs/action/RandomCrawl.action b/nav2_msgs/action/RandomCrawl.action deleted file mode 100644 index 8077c1acaa..0000000000 --- a/nav2_msgs/action/RandomCrawl.action +++ /dev/null @@ -1,7 +0,0 @@ -#goal definition -std_msgs/Empty target ---- -#result definition -std_msgs/Empty result ---- -#feedback diff --git a/nav2_msgs/action/Spin.action b/nav2_msgs/action/Spin.action index 021df0d6d7..d6c926695e 100644 --- a/nav2_msgs/action/Spin.action +++ b/nav2_msgs/action/Spin.action @@ -4,4 +4,4 @@ float32 target_yaw #result definition std_msgs/Empty result --- -#feedback +float32 angular_distance_traveled diff --git a/nav2_msgs/action/Wait.action b/nav2_msgs/action/Wait.action index 0855b72323..63dd38d411 100644 --- a/nav2_msgs/action/Wait.action +++ b/nav2_msgs/action/Wait.action @@ -5,3 +5,4 @@ builtin_interfaces/Duration time std_msgs/Empty result --- #feedback +builtin_interfaces/Duration time_left diff --git a/nav2_msgs/msg/Particle.msg b/nav2_msgs/msg/Particle.msg new file mode 100644 index 0000000000..294bbd170d --- /dev/null +++ b/nav2_msgs/msg/Particle.msg @@ -0,0 +1,5 @@ +# This represents an individual particle with weight produced by a particle filter + +geometry_msgs/Pose pose + +float64 weight diff --git a/nav2_msgs/msg/ParticleCloud.msg b/nav2_msgs/msg/ParticleCloud.msg new file mode 100644 index 0000000000..17b07ac707 --- /dev/null +++ b/nav2_msgs/msg/ParticleCloud.msg @@ -0,0 +1,6 @@ +# This represents a particle cloud containing particle poses and weights + +std_msgs/Header header + +# Array of particles in the cloud +Particle[] particles diff --git a/nav2_msgs/srv/SaveMap.srv b/nav2_msgs/srv/SaveMap.srv new file mode 100644 index 0000000000..f50a881d47 --- /dev/null +++ b/nav2_msgs/srv/SaveMap.srv @@ -0,0 +1,14 @@ +# URL of map resource +# Can be an absolute path to a file: file:///path/to/maps/floor1.yaml +# Or, relative to a ROS package: package://my_ros_package/maps/floor2.yaml +string map_topic +string map_url +# Constants for image_format. Supported formats: pgm, png, bmp +string image_format +# Map modes: trinary, scale or raw +string map_mode +# Thresholds. Values in range of [0.0 .. 1.0] +float32 free_thresh +float32 occupied_thresh +--- +bool result diff --git a/nav2_navfn_planner/CMakeLists.txt b/nav2_navfn_planner/CMakeLists.txt index 1315b65ed5..ab4815b016 100644 --- a/nav2_navfn_planner/CMakeLists.txt +++ b/nav2_navfn_planner/CMakeLists.txt @@ -60,7 +60,7 @@ pluginlib_export_plugin_description_file(nav2_core global_planner_plugin.xml) install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index 87b21b395b..29c97db634 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -72,7 +72,7 @@ NavfnPlanner::configure( // Initialize parameters // Declare this plugin's parameters - declare_parameter_if_not_declared(node_, name + ".tolerance", rclcpp::ParameterValue(2.0)); + declare_parameter_if_not_declared(node_, name + ".tolerance", rclcpp::ParameterValue(0.5)); node_->get_parameter(name + ".tolerance", tolerance_); declare_parameter_if_not_declared(node_, name + ".use_astar", rclcpp::ParameterValue(false)); node_->get_parameter(name + ".use_astar", use_astar_); @@ -213,26 +213,35 @@ NavfnPlanner::makePlan( double resolution = costmap_->getResolution(); geometry_msgs::msg::Pose p, best_pose; - p = goal; bool found_legal = false; - double best_sdist = std::numeric_limits::max(); - - p.position.y = goal.position.y - tolerance; - - while (p.position.y <= goal.position.y + tolerance) { - p.position.x = goal.position.x - tolerance; - while (p.position.x <= goal.position.x + tolerance) { - double potential = getPointPotential(p.position); - double sdist = squared_distance(p, goal); - if (potential < POT_HIGH && sdist < best_sdist) { - best_sdist = sdist; - best_pose = p; - found_legal = true; + + p = goal; + double potential = getPointPotential(p.position); + if (potential < POT_HIGH) { + // Goal is reachable by itself + best_pose = p; + found_legal = true; + } else { + // Goal is not reachable. Trying to find nearest to the goal + // reachable point within its tolerance region + double best_sdist = std::numeric_limits::max(); + + p.position.y = goal.position.y - tolerance; + while (p.position.y <= goal.position.y + tolerance) { + p.position.x = goal.position.x - tolerance; + while (p.position.x <= goal.position.x + tolerance) { + potential = getPointPotential(p.position); + double sdist = squared_distance(p, goal); + if (potential < POT_HIGH && sdist < best_sdist) { + best_sdist = sdist; + best_pose = p; + found_legal = true; + } + p.position.x += resolution; } - p.position.x += resolution; + p.position.y += resolution; } - p.position.y += resolution; } if (found_legal) { @@ -257,52 +266,20 @@ NavfnPlanner::smoothApproachToGoal( { // Replace the last pose of the computed path if it's actually further away // to the second to last pose than the goal pose. - - auto second_to_last_pose = plan.poses.end()[-2]; - auto last_pose = plan.poses.back(); - if ( - squared_distance(last_pose.pose, second_to_last_pose.pose) > - squared_distance(goal, second_to_last_pose.pose)) - { - plan.poses.back().pose = goal; - } else { - geometry_msgs::msg::PoseStamped goal_copy; - goal_copy.pose = goal; - plan.poses.push_back(goal_copy); - } -} - -bool -NavfnPlanner::computePotential(const geometry_msgs::msg::Point & world_point) -{ - // make sure to resize the underlying array that Navfn uses - planner_->setNavArr( - costmap_->getSizeInCellsX(), - costmap_->getSizeInCellsY()); - - planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_); - - unsigned int mx, my; - if (!worldToMap(world_point.x, world_point.y, mx, my)) { - return false; - } - - int map_start[2]; - map_start[0] = 0; - map_start[1] = 0; - - int map_goal[2]; - map_goal[0] = mx; - map_goal[1] = my; - - planner_->setStart(map_start); - planner_->setGoal(map_goal); - - if (use_astar_) { - return planner_->calcNavFnAstar(); + if (plan.poses.size() >= 2) { + auto second_to_last_pose = plan.poses.end()[-2]; + auto last_pose = plan.poses.back(); + if ( + squared_distance(last_pose.pose, second_to_last_pose.pose) > + squared_distance(goal, second_to_last_pose.pose)) + { + plan.poses.back().pose = goal; + return; + } } - - return planner_->calcNavFnDijkstra(); + geometry_msgs::msg::PoseStamped goal_copy; + goal_copy.pose = goal; + plan.poses.push_back(goal_copy); } bool @@ -393,18 +370,25 @@ NavfnPlanner::validPointPotential( const double resolution = costmap_->getResolution(); geometry_msgs::msg::Point p = world_point; - p.y = world_point.y - tolerance; - - while (p.y <= world_point.y + tolerance) { - p.x = world_point.x - tolerance; - while (p.x <= world_point.x + tolerance) { - double potential = getPointPotential(p); - if (potential < POT_HIGH) { - return true; + double potential = getPointPotential(p); + if (potential < POT_HIGH) { + // world_point is reachable by itself + return true; + } else { + // world_point, is not reachable. Trying to find any + // reachable point within its tolerance region + p.y = world_point.y - tolerance; + while (p.y <= world_point.y + tolerance) { + p.x = world_point.x - tolerance; + while (p.x <= world_point.x + tolerance) { + potential = getPointPotential(p); + if (potential < POT_HIGH) { + return true; + } + p.x += resolution; } - p.x += resolution; + p.y += resolution; } - p.y += resolution; } return false; diff --git a/nav2_planner/CMakeLists.txt b/nav2_planner/CMakeLists.txt index 4012f40b9d..69f4d57156 100644 --- a/nav2_planner/CMakeLists.txt +++ b/nav2_planner/CMakeLists.txt @@ -67,9 +67,13 @@ ament_target_dependencies(${executable_name} # prevent pluginlib from using boost target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index b24e34f1c1..dbb159d730 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "builtin_interfaces/msg/duration.hpp" #include "nav2_util/costmap.hpp" @@ -89,7 +90,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) exit(-1); } - for (uint i = 0; i != plugin_types_.size(); i++) { + for (size_t i = 0; i != plugin_types_.size(); i++) { try { nav2_core::GlobalPlanner::Ptr planner = gp_loader_.createUniqueInstance(plugin_types_[i]); @@ -106,7 +107,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state) } } - for (uint i = 0; i != plugin_types_.size(); i++) { + for (size_t i = 0; i != plugin_types_.size(); i++) { planner_ids_concat_ += plugin_ids_[i] + std::string(" "); } @@ -294,7 +295,8 @@ PlannerServer::computePlan() void PlannerServer::publishPlan(const nav_msgs::msg::Path & path) { - plan_publisher_->publish(path); + auto msg = std::make_unique(path); + plan_publisher_->publish(std::move(msg)); } } // namespace nav2_planner diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt index 6b0def011e..01849b8c11 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/nav2_recoveries/CMakeLists.txt @@ -103,13 +103,17 @@ ament_target_dependencies(${executable_name} ${dependencies} ) -install(TARGETS ${executable_name} - ${library_name} + +install(TARGETS ${library_name} nav2_backup_recovery nav2_spin_recovery nav2_wait_recovery ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 731188cecb..5583a19d4d 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" #include "tf2_ros/transform_listener.h" @@ -87,7 +88,7 @@ class Recovery : public nav2_core::Recovery void configure( const rclcpp_lifecycle::LifecycleNode::SharedPtr parent, const std::string & name, std::shared_ptr tf, - std::shared_ptr collision_checker) override + std::shared_ptr collision_checker) override { RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str()); @@ -96,6 +97,9 @@ class Recovery : public nav2_core::Recovery tf_ = tf; node_->get_parameter("cycle_frequency", cycle_frequency_); + node_->get_parameter("global_frame", global_frame_); + node_->get_parameter("robot_base_frame", robot_base_frame_); + node_->get_parameter("transform_tolerance", transform_tolerance_); action_server_ = std::make_shared( node_, recovery_name_, @@ -134,11 +138,14 @@ class Recovery : public nav2_core::Recovery std::string recovery_name_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr vel_pub_; std::shared_ptr action_server_; - std::shared_ptr collision_checker_; + std::shared_ptr collision_checker_; std::shared_ptr tf_; double cycle_frequency_; double enabled_; + std::string global_frame_; + std::string robot_base_frame_; + double transform_tolerance_; void execute() { @@ -203,12 +210,12 @@ class Recovery : public nav2_core::Recovery void stopRobot() { - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.0; + auto cmd_vel = std::make_unique(); + cmd_vel->linear.x = 0.0; + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; - vel_pub_->publish(cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); } }; diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index 026c3ecab4..f86cc38d8f 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp @@ -59,7 +59,9 @@ class RecoveryServer : public nav2_util::LifecycleNode // Utilities std::unique_ptr costmap_sub_; std::unique_ptr footprint_sub_; - std::shared_ptr collision_checker_; + std::shared_ptr collision_checker_; + + double transform_tolerance_; }; } // namespace recovery_server diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index fc785d6964..3f25840203 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include "back_up.hpp" #include "nav2_util/node_utils.hpp" @@ -25,7 +26,8 @@ namespace nav2_recoveries { BackUp::BackUp() -: Recovery() +: Recovery(), + feedback_(std::make_shared()) { } @@ -52,7 +54,10 @@ Status BackUp::onRun(const std::shared_ptr command) command_x_ = command->target.x; command_speed_ = command->speed; - if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom")) { + if (!nav2_util::getCurrentPose( + initial_pose_, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available."); return Status::FAILED; } @@ -63,7 +68,10 @@ Status BackUp::onRun(const std::shared_ptr command) Status BackUp::onCycleUpdate() { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } @@ -72,35 +80,39 @@ Status BackUp::onCycleUpdate() double diff_y = initial_pose_.pose.position.y - current_pose.pose.position.y; double distance = sqrt(diff_x * diff_x + diff_y * diff_y); + feedback_->distance_traveled = distance; + action_server_->publish_feedback(feedback_); + if (distance >= abs(command_x_)) { stopRobot(); return Status::SUCCEEDED; } + // TODO(mhpanah): cmd_vel value should be passed as a parameter - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.0; - command_x_ < 0 ? cmd_vel.linear.x = -command_speed_ : cmd_vel.linear.x = command_speed_; + auto cmd_vel = std::make_unique(); + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; + command_x_ < 0 ? cmd_vel->linear.x = -command_speed_ : cmd_vel->linear.x = command_speed_; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(distance, cmd_vel, pose2d)) { + if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting BackUp"); return Status::SUCCEEDED; } - vel_pub_->publish(cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); return Status::RUNNING; } bool BackUp::isCollisionFree( const double & distance, - const geometry_msgs::msg::Twist & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments @@ -110,7 +122,7 @@ bool BackUp::isCollisionFree( const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); while (cycle_count < max_cycle_count) { - sim_position_change = cmd_vel.linear.x * (cycle_count / cycle_frequency_); + sim_position_change = cmd_vel->linear.x * (cycle_count / cycle_frequency_); pose2d.x += sim_position_change; cycle_count++; diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_recoveries/plugins/back_up.hpp index 3ce3623e5a..ac219e65a7 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_recoveries/plugins/back_up.hpp @@ -38,7 +38,7 @@ class BackUp : public Recovery protected: bool isCollisionFree( const double & distance, - const geometry_msgs::msg::Twist & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d); void onConfigure() override; @@ -51,6 +51,8 @@ class BackUp : public Recovery double command_x_; double command_speed_; double simulate_ahead_time_; + + BackUpAction::Feedback::SharedPtr feedback_; }; } // namespace nav2_recoveries diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp index 547d56c169..e24be17d89 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_recoveries/plugins/spin.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "spin.hpp" #pragma GCC diagnostic push @@ -34,9 +35,10 @@ namespace nav2_recoveries { Spin::Spin() -: Recovery() +: Recovery(), + feedback_(std::make_shared()), + prev_yaw_(0.0) { - initial_yaw_ = 0.0; } Spin::~Spin() @@ -69,14 +71,18 @@ void Spin::onConfigure() Status Spin::onRun(const std::shared_ptr command) { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } - initial_yaw_ = tf2::getYaw(current_pose.pose.orientation); + prev_yaw_ = tf2::getYaw(current_pose.pose.orientation); + relative_yaw_ = 0.0; - cmd_yaw_ = -command->target_yaw; + cmd_yaw_ = command->target_yaw; RCLCPP_INFO( node_->get_logger(), "Turning %0.2f for spin recovery.", cmd_yaw_); @@ -86,48 +92,58 @@ Status Spin::onRun(const std::shared_ptr command) Status Spin::onCycleUpdate() { geometry_msgs::msg::PoseStamped current_pose; - if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) { + if (!nav2_util::getCurrentPose( + current_pose, *tf_, global_frame_, robot_base_frame_, + transform_tolerance_)) + { RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); return Status::FAILED; } const double current_yaw = tf2::getYaw(current_pose.pose.orientation); - double relative_yaw = abs(current_yaw - initial_yaw_); - if (relative_yaw > M_PI) { - relative_yaw -= 2.0 * M_PI; + + double delta_yaw = current_yaw - prev_yaw_; + if (abs(delta_yaw) > M_PI) { + delta_yaw = copysign(2 * M_PI - abs(delta_yaw), prev_yaw_); } - relative_yaw = abs(relative_yaw); - if (relative_yaw >= abs(cmd_yaw_)) { + relative_yaw_ += delta_yaw; + prev_yaw_ = current_yaw; + + feedback_->angular_distance_traveled = relative_yaw_; + action_server_->publish_feedback(feedback_); + + double remaining_yaw = abs(cmd_yaw_) - abs(relative_yaw_); + if (remaining_yaw <= 0) { stopRobot(); return Status::SUCCEEDED; } - double vel = sqrt(2 * rotational_acc_lim_ * relative_yaw); + double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw); vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_); - geometry_msgs::msg::Twist cmd_vel; - cmd_yaw_ < 0 ? cmd_vel.angular.z = -vel : cmd_vel.angular.z = vel; + auto cmd_vel = std::make_unique(); + cmd_vel->angular.z = copysign(vel, cmd_yaw_); geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; pose2d.y = current_pose.pose.position.y; pose2d.theta = tf2::getYaw(current_pose.pose.orientation); - if (!isCollisionFree(relative_yaw, cmd_vel, pose2d)) { + if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) { stopRobot(); RCLCPP_WARN(node_->get_logger(), "Collision Ahead - Exiting Spin"); return Status::SUCCEEDED; } - vel_pub_->publish(cmd_vel); + vel_pub_->publish(std::move(cmd_vel)); return Status::RUNNING; } bool Spin::isCollisionFree( const double & relative_yaw, - const geometry_msgs::msg::Twist & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d) { // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments @@ -136,7 +152,7 @@ bool Spin::isCollisionFree( const int max_cycle_count = static_cast(cycle_frequency_ * simulate_ahead_time_); while (cycle_count < max_cycle_count) { - sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_); + sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_); pose2d.theta += sim_position_change; cycle_count++; diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_recoveries/plugins/spin.hpp index 358fdec1c9..cf1d6b7d04 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_recoveries/plugins/spin.hpp @@ -40,14 +40,17 @@ class Spin : public Recovery protected: bool isCollisionFree( const double & distance, - const geometry_msgs::msg::Twist & cmd_vel, + geometry_msgs::msg::Twist * cmd_vel, geometry_msgs::msg::Pose2D & pose2d); + SpinAction::Feedback::SharedPtr feedback_; + double min_rotational_vel_; double max_rotational_vel_; double rotational_acc_lim_; double cmd_yaw_; - double initial_yaw_; + double prev_yaw_; + double relative_yaw_; double simulate_ahead_time_; }; diff --git a/nav2_recoveries/plugins/wait.cpp b/nav2_recoveries/plugins/wait.cpp index 7964103b3c..e9a27b4dda 100644 --- a/nav2_recoveries/plugins/wait.cpp +++ b/nav2_recoveries/plugins/wait.cpp @@ -22,9 +22,9 @@ namespace nav2_recoveries { Wait::Wait() -: Recovery() +: Recovery(), + feedback_(std::make_shared()) { - duration_.sec = 0.0; } Wait::~Wait() @@ -33,15 +33,25 @@ Wait::~Wait() Status Wait::onRun(const std::shared_ptr command) { - duration_ = command->time; + wait_end_ = std::chrono::steady_clock::now() + + rclcpp::Duration(command->time).to_chrono(); return Status::SUCCEEDED; } Status Wait::onCycleUpdate() { - rclcpp::sleep_for( - rclcpp::Duration(duration_).to_chrono()); - return Status::SUCCEEDED; + auto current_point = std::chrono::steady_clock::now(); + auto time_left = + std::chrono::duration_cast(wait_end_ - current_point).count(); + + feedback_->time_left = rclcpp::Duration(time_left); + action_server_->publish_feedback(feedback_); + + if (time_left > 0) { + return Status::RUNNING; + } else { + return Status::SUCCEEDED; + } } } // namespace nav2_recoveries diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_recoveries/plugins/wait.hpp index 4f883220d6..34364ce174 100644 --- a/nav2_recoveries/plugins/wait.hpp +++ b/nav2_recoveries/plugins/wait.hpp @@ -37,7 +37,8 @@ class Wait : public Recovery Status onCycleUpdate() override; protected: - builtin_interfaces::msg::Duration duration_; + std::chrono::time_point wait_end_; + WaitAction::Feedback::SharedPtr feedback_; }; } // namespace nav2_recoveries diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index a4caa363d8..127d00a994 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_recoveries/src/recovery_server.cpp @@ -22,7 +22,7 @@ namespace recovery_server { RecoveryServer::RecoveryServer() -: nav2_util::LifecycleNode("nav2_recoveries", "", true), +: LifecycleNode("recoveries_server", "", true), plugin_loader_("nav2_core", "nav2_core::Recovery") { declare_parameter( @@ -45,6 +45,16 @@ RecoveryServer::RecoveryServer() declare_parameter( "plugin_types", rclcpp::ParameterValue(plugin_types)); + + declare_parameter( + "global_frame", + rclcpp::ParameterValue(std::string("odom"))); + declare_parameter( + "robot_base_frame", + rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter( + "transform_tolerance", + rclcpp::ParameterValue(0.1)); } @@ -59,23 +69,29 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_ = std::make_shared(get_clock()); auto timer_interface = std::make_shared( - this->get_node_base_interface(), - this->get_node_timers_interface()); + get_node_base_interface(), + get_node_timers_interface()); tf_->setCreateTimerInterface(timer_interface); transform_listener_ = std::make_shared(*tf_); std::string costmap_topic, footprint_topic; this->get_parameter("costmap_topic", costmap_topic); this->get_parameter("footprint_topic", footprint_topic); + this->get_parameter("transform_tolerance", transform_tolerance_); costmap_sub_ = std::make_unique( shared_from_this(), costmap_topic); footprint_sub_ = std::make_unique( shared_from_this(), footprint_topic, 1.0); - collision_checker_ = std::make_shared( - *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom"); - this->get_parameter("plugin_names", plugin_names_); - this->get_parameter("plugin_types", plugin_types_); + std::string global_frame, robot_base_frame; + get_parameter("global_frame", global_frame); + get_parameter("robot_base_frame", robot_base_frame); + collision_checker_ = std::make_shared( + *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), + global_frame, robot_base_frame, transform_tolerance_); + + get_parameter("plugin_names", plugin_names_); + get_parameter("plugin_types", plugin_types_); loadRecoveryPlugins(); @@ -88,7 +104,7 @@ RecoveryServer::loadRecoveryPlugins() { auto node = shared_from_this(); - for (uint i = 0; i != plugin_names_.size(); i++) { + for (size_t i = 0; i != plugin_names_.size(); i++) { try { RCLCPP_INFO( get_logger(), "Creating recovery plugin %s of type %s", diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index f1600094a9..137e3500e4 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_recoveries/test/test_recoveries.cpp @@ -126,8 +126,8 @@ class RecoveryTest : public ::testing::Test std::shared_ptr footprint_sub_ = std::make_shared( node_lifecycle_, footprint_topic, 1.0); - std::shared_ptr collision_checker_ = - std::make_shared( + std::shared_ptr collision_checker_ = + std::make_shared( *costmap_sub_, *footprint_sub_, *tf_buffer_, node_lifecycle_->get_name(), "odom"); diff --git a/nav2_rviz_plugins/CMakeLists.txt b/nav2_rviz_plugins/CMakeLists.txt index 65c32d8e85..f344363296 100644 --- a/nav2_rviz_plugins/CMakeLists.txt +++ b/nav2_rviz_plugins/CMakeLists.txt @@ -112,7 +112,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_interfaces(${library_name} HAS_LIBRARY_TARGET) +ament_export_targets(${library_name} HAS_LIBRARY_TARGET) ament_export_dependencies( Qt5 rviz_common diff --git a/nav2_rviz_plugins/src/nav2_panel.cpp b/nav2_rviz_plugins/src/nav2_panel.cpp index 776f11ca70..9d1b315683 100644 --- a/nav2_rviz_plugins/src/nav2_panel.cpp +++ b/nav2_rviz_plugins/src/nav2_panel.cpp @@ -19,6 +19,7 @@ #include #include +#include #include "nav2_rviz_plugins/goal_common.hpp" #include "rviz_common/display_context.hpp" @@ -609,7 +610,7 @@ Nav2Panel::updateWpNavigationMarkers() { resetUniqueId(); - visualization_msgs::msg::MarkerArray marker_array; + auto marker_array = std::make_unique(); for (size_t i = 0; i < acummulated_poses_.size(); i++) { // Draw a green arrow at the waypoint pose @@ -628,7 +629,7 @@ Nav2Panel::updateWpNavigationMarkers() arrow_marker.color.a = 1.0f; arrow_marker.lifetime = rclcpp::Duration(0); arrow_marker.frame_locked = false; - marker_array.markers.push_back(arrow_marker); + marker_array->markers.push_back(arrow_marker); // Draw a red circle at the waypoint pose visualization_msgs::msg::Marker circle_marker; @@ -646,7 +647,7 @@ Nav2Panel::updateWpNavigationMarkers() circle_marker.color.a = 1.0f; circle_marker.lifetime = rclcpp::Duration(0); circle_marker.frame_locked = false; - marker_array.markers.push_back(circle_marker); + marker_array->markers.push_back(circle_marker); // Draw the waypoint number visualization_msgs::msg::Marker marker_text; @@ -666,16 +667,16 @@ Nav2Panel::updateWpNavigationMarkers() marker_text.lifetime = rclcpp::Duration(0); marker_text.frame_locked = false; marker_text.text = "wp_" + std::to_string(i + 1); - marker_array.markers.push_back(marker_text); + marker_array->markers.push_back(marker_text); } - if (marker_array.markers.empty()) { + if (marker_array->markers.empty()) { visualization_msgs::msg::Marker clear_all_marker; clear_all_marker.action = visualization_msgs::msg::Marker::DELETEALL; - marker_array.markers.push_back(clear_all_marker); + marker_array->markers.push_back(clear_all_marker); } - wp_navigation_markers_pub_->publish(marker_array); + wp_navigation_markers_pub_->publish(std::move(marker_array)); } } // namespace nav2_rviz_plugins diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 03891cebe4..9eae0646f2 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(nav2_common REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) +find_package(nav2_map_server REQUIRED) find_package(nav2_bringup REQUIRED) find_package(nav2_msgs REQUIRED) find_package(nav_msgs REQUIRED) @@ -20,12 +21,14 @@ find_package(rclpy REQUIRED) find_package(nav2_navfn_planner REQUIRED) find_package(nav2_planner REQUIRED) find_package(navigation2) +find_package(angles REQUIRED) nav2_package() set(dependencies rclcpp nav2_util + nav2_map_server nav2_bringup nav2_msgs nav_msgs @@ -39,6 +42,7 @@ set(dependencies rclpy nav2_planner nav2_navfn_planner + angles ) if(BUILD_TESTING) @@ -53,6 +57,9 @@ if(BUILD_TESTING) add_subdirectory(src/system) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) + add_subdirectory(src/recoveries/spin) + add_subdirectory(src/recoveries/wait) + add_subdirectory(src/recoveries/backup) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 8d74b8ad16..195973915f 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -13,6 +13,7 @@ rclcpp rclpy nav2_util + nav2_map_server nav2_msgs nav2_lifecycle_manager nav2_navfn_planner @@ -35,6 +36,7 @@ rclpy nav2_bringup nav2_util + nav2_map_server nav2_msgs nav2_lifecycle_manager nav2_navfn_planner diff --git a/nav2_system_tests/src/dummy_controller/dummy_controller.cpp b/nav2_system_tests/src/dummy_controller/dummy_controller.cpp index d87b87f857..16643ed4a6 100644 --- a/nav2_system_tests/src/dummy_controller/dummy_controller.cpp +++ b/nav2_system_tests/src/dummy_controller/dummy_controller.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "dummy_controller.hpp" @@ -76,9 +77,9 @@ DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedP } // Output control command - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = 0.1; - vel_pub_->publish(cmd_vel); + auto cmd_vel = std::make_unique(); + cmd_vel->linear.x = 0.1; + vel_pub_->publish(std::move(cmd_vel)); if (current_time - start_time >= 30s) { RCLCPP_INFO(get_logger(), "Reached end point"); @@ -93,11 +94,11 @@ DummyController::followPath(const nav2_behavior_tree::FollowPathCommand::SharedP void DummyController::setZeroVelocity() { - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = 0.0; - cmd_vel.linear.y = 0.0; - cmd_vel.angular.z = 0.0; - vel_pub_->publish(cmd_vel); + auto cmd_vel = std::make_unique(); + cmd_vel->linear.x = 0.0; + cmd_vel->linear.y = 0.0; + cmd_vel->angular.z = 0.0; + vel_pub_->publish(std::move(cmd_vel)); } } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/localization/CMakeLists.txt b/nav2_system_tests/src/localization/CMakeLists.txt index a4a76a327b..740d38f53e 100644 --- a/nav2_system_tests/src/localization/CMakeLists.txt +++ b/nav2_system_tests/src/localization/CMakeLists.txt @@ -9,7 +9,7 @@ ament_target_dependencies(test_localization_node ament_add_test(test_localization GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py" - TIMEOUT 60 + TIMEOUT 180 ENV TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_EXECUTABLE=$ diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py index 004b6ba68e..70c30e0842 100755 --- a/nav2_system_tests/src/localization/test_localization_launch.py +++ b/nav2_system_tests/src/localization/test_localization_launch.py @@ -35,28 +35,28 @@ def main(argv=sys.argv[1:]): output='screen') link_footprint = launch_ros.actions.Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']) footprint_scan = launch_ros.actions.Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']) run_map_server = launch_ros.actions.Node( package='nav2_map_server', - node_executable='map_server', - node_name='map_server', + executable='map_server', + name='map_server', output='screen', parameters=[{'yaml_filename': mapFile}]) run_amcl = launch_ros.actions.Node( package='nav2_amcl', - node_executable='amcl', + executable='amcl', output='screen') run_lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', - node_executable='lifecycle_manager', - node_name='lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', output='screen', parameters=[{'node_names': ['map_server', 'amcl']}, {'autostart': True}]) ld = LaunchDescription([launch_gazebo, link_footprint, footprint_scan, diff --git a/nav2_system_tests/src/planning/CMakeLists.txt b/nav2_system_tests/src/planning/CMakeLists.txt index 0938c08709..04745b348f 100644 --- a/nav2_system_tests/src/planning/CMakeLists.txt +++ b/nav2_system_tests/src/planning/CMakeLists.txt @@ -1,28 +1,38 @@ -ament_add_gtest_executable(test_planner_costmaps_node +set(test_planner_costmaps_exec test_planner_costmaps_node) + +ament_add_gtest_executable(${test_planner_costmaps_exec} test_planner_costmaps_node.cpp planner_tester.cpp ) -ament_target_dependencies(test_planner_costmaps_node +target_link_libraries(${test_planner_costmaps_exec} + ${nav2_map_server_LIBRARIES}) + +ament_target_dependencies(${test_planner_costmaps_exec} ${dependencies} ) -ament_add_gtest_executable(test_planner_random_node +set(test_planner_random_exec test_planner_random_node) + +ament_add_gtest_executable(${test_planner_random_exec} test_planner_random_node.cpp planner_tester.cpp ) -ament_target_dependencies(test_planner_random_node +ament_target_dependencies(${test_planner_random_exec} ${dependencies} ) +target_link_libraries(${test_planner_random_exec} + ${nav2_map_server_LIBRARIES}) + ament_add_test(test_planner_costmaps GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_planner_costmaps_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" ENV TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ + TEST_EXECUTABLE=$ TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm ) @@ -32,6 +42,6 @@ ament_add_test(test_planner_random WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" ENV TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ + TEST_EXECUTABLE=$ TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map.pgm ) diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index b0125f260b..534b823730 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -24,7 +25,8 @@ #include "planner_tester.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "nav2_util/map_loader/map_loader.hpp" +#include "nav2_map_server/map_mode.hpp" +#include "nav2_map_server/map_io.hpp" #include "nav2_msgs/msg/costmap_meta_data.hpp" using namespace std::chrono_literals; @@ -84,6 +86,7 @@ void PlannerTester::deactivate() spin_thread_.reset(); auto state = rclcpp_lifecycle::State(); + planner_tester_->onDeactivate(state); planner_tester_->onCleanup(state); map_timer_.reset(); @@ -148,15 +151,9 @@ void PlannerTester::loadDefaultMap() double free_threshold = 0.196; // Define origin offset - geometry_msgs::msg::Twist origin; - origin.linear.x = 0.0; - origin.linear.y = 0.0; - origin.linear.z = 0.0; - origin.angular.x = 0.0; - origin.angular.y = 0.0; - origin.angular.z = 0.0; + std::vector origin = {0.0, 0.0, 0.0}; - MapMode mode = TRINARY; + nav2_map_server::MapMode mode = nav2_map_server::MapMode::Trinary; std::string file_path = ""; char const * path = getenv("TEST_MAP"); @@ -171,11 +168,17 @@ void PlannerTester::loadDefaultMap() RCLCPP_INFO(this->get_logger(), "Loading map with file_path: %s", file_path.c_str()); try { - map_ = - std::make_shared( - map_loader::loadMapFromFile( - file_path, resolution, negate, - occupancy_threshold, free_threshold, origin, mode)); + map_ = std::make_shared(); + + nav2_map_server::LoadParameters load_parameters; + load_parameters.image_file_name = file_path; + load_parameters.resolution = resolution; + load_parameters.origin = origin; + load_parameters.free_thresh = free_threshold; + load_parameters.occupied_thresh = occupancy_threshold; + load_parameters.mode = mode; + load_parameters.negate = negate; + loadMapFromFile(load_parameters, *map_); } catch (...) { RCLCPP_ERROR( this->get_logger(), diff --git a/nav2_system_tests/src/planning/planner_tester.hpp b/nav2_system_tests/src/planning/planner_tester.hpp index a9dfaebd42..c667b9d7b4 100644 --- a/nav2_system_tests/src/planning/planner_tester.hpp +++ b/nav2_system_tests/src/planning/planner_tester.hpp @@ -50,7 +50,7 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer void printCostmap() { // print costmap for debug - for (uint i = 0; i != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY(); i++) { + for (size_t i = 0; i != costmap_->getSizeInCellsX() * costmap_->getSizeInCellsY(); i++) { if (i % costmap_->getSizeInCellsX() == 0) { std::cout << "" << std::endl; } @@ -67,7 +67,8 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer costmap_ros_->getCostmap()->resizeMap( prop.size_x, prop.size_y, prop.resolution, prop.origin.position.x, prop.origin.position.x); - unsigned char * costmap_ptr = costmap_ros_->getCostmap()->getCharMap(); + // Volatile prevents compiler from treating costmap_ptr as unused or changing its address + volatile unsigned char * costmap_ptr = costmap_ros_->getCostmap()->getCharMap(); delete[] costmap_ptr; costmap_ptr = new unsigned char[prop.size_x * prop.size_y]; std::copy(cm.data.begin(), cm.data.end(), costmap_ptr); @@ -83,6 +84,12 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer } try { path = planners_["GridBased"]->createPlan(start, goal); + // The situation when createPlan() did not throw any exception + // does not guarantee that plan was created correctly. + // So it should be checked additionally that path is correct. + if (!path.poses.size()) { + return false; + } } catch (...) { return false; } @@ -99,6 +106,11 @@ class NavFnPlannerTester : public nav2_planner::PlannerServer on_activate(state); } + void onDeactivate(const rclcpp_lifecycle::State & state) + { + on_deactivate(state); + } + void onConfigure(const rclcpp_lifecycle::State & state) { on_configure(state); diff --git a/nav2_system_tests/src/recoveries/README.md b/nav2_system_tests/src/recoveries/README.md new file mode 100644 index 0000000000..e4b764aaee --- /dev/null +++ b/nav2_system_tests/src/recoveries/README.md @@ -0,0 +1,4 @@ +# Recoveries Test + +Provides some simple tests for recovery plugins. +It creates an instance of the stack, with the recovery server loading different recovery plugins, and checks for successful recovery behaviors. diff --git a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt b/nav2_system_tests/src/recoveries/backup/CMakeLists.txt new file mode 100644 index 0000000000..d21387dec4 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_backup_recovery_exec test_backup_recovery_node) + +ament_add_gtest_executable(${test_backup_recovery_exec} + test_backup_recovery_node.cpp + backup_recovery_tester.cpp +) + +ament_target_dependencies(${test_backup_recovery_exec} + ${dependencies} +) + +ament_add_test(test_backup_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_recovery_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp new file mode 100644 index 0000000000..6d87fb1bd2 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp @@ -0,0 +1,223 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "backup_recovery_tester.hpp" +#include "nav2_util/geometry_utils.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +BackupRecoveryTester::BackupRecoveryTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("backup_recovery_test"); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + client_ptr_ = rclcpp_action::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "backup"); + + publisher_ = + node_->create_publisher("initialpose", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&BackupRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); +} + +BackupRecoveryTester::~BackupRecoveryTester() +{ + if (is_active_) { + deactivate(); + } +} + +void BackupRecoveryTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + + // Wait for lifecycle_manager_navigation to activate recoveries_server + std::this_thread::sleep_for(10s); + + if (!client_ptr_) { + RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); + is_active_ = false; + return; + } + + if (!client_ptr_->wait_for_action_server(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + is_active_ = false; + return; + } + + RCLCPP_INFO(this->node_->get_logger(), "Backup action server is ready"); + is_active_ = true; +} + +void BackupRecoveryTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool BackupRecoveryTester::defaultBackupRecoveryTest( + const float target_dist, + const double tolerance) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto goal_msg = BackUp::Goal(); + goal_msg.target.x = target_dist; + goal_msg.speed = 0.2; + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + geometry_msgs::msg::PoseStamped initial_pose; + if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Found current robot pose"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_get_result(goal_handle); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "result received"); + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + + double dist = nav2_util::geometry_utils::euclidean_distance(initial_pose, current_pose); + + if (fabs(dist) > fabs(target_dist) + tolerance) { + RCLCPP_ERROR( + node_->get_logger(), + "Distance from goal is %lf (tolerance %lf)", + fabs(dist - target_dist), tolerance); + return false; + } + + return true; +} + +void BackupRecoveryTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = rclcpp::Time(); + pose.pose.pose.position.x = -2.0; + pose.pose.pose.position.y = -0.5; + pose.pose.pose.position.z = 0.0; + pose.pose.pose.orientation.x = 0.0; + pose.pose.pose.orientation.y = 0.0; + pose.pose.pose.orientation.z = 0.0; + pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 35; i++) { + pose.pose.covariance[i] = 0.0; + } + pose.pose.covariance[0] = 0.08; + pose.pose.covariance[7] = 0.08; + pose.pose.covariance[35] = 0.05; + + publisher_->publish(pose); + RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); +} + +void BackupRecoveryTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp new file mode 100644 index 0000000000..4c658860a0 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ +#define RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "angles/angles.h" +#include "nav2_msgs/action/back_up.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class BackupRecoveryTester +{ +public: + using BackUp = nav2_msgs::action::BackUp; + using GoalHandleBackup = rclcpp_action::ClientGoalHandle; + + BackupRecoveryTester(); + ~BackupRecoveryTester(); + + // Runs a single test with given target yaw + bool defaultBackupRecoveryTest( + float target_dist, + double tolerance = 0.1); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +private: + void sendInitialPose(); + + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + bool is_active_; + bool initial_pose_received_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Node::SharedPtr node_; + + // Publisher to publish initial pose + rclcpp::Publisher::SharedPtr publisher_; + + // Subscriber for amcl pose + rclcpp::Subscription::SharedPtr subscription_; + + // Action client to call Backup action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#endif // RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py new file mode 100755 index 0000000000..7f23662f2e --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py @@ -0,0 +1,102 @@ +#! /usr/bin/env python3 +# Copyright (c) 2012 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + node_executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_backup_recovery_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp new file mode 100644 index 0000000000..82c6effbd9 --- /dev/null +++ b/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp @@ -0,0 +1,110 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "backup_recovery_tester.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::BackupRecoveryTester; + +std::string testNameGenerator(const testing::TestParamInfo> & param) +{ + std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( + std::get<1>(param.param)); + name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); + return name; +} + +class BackupRecoveryTestFixture + : public ::testing::TestWithParam> +{ +public: + static void SetUpTestCase() + { + backup_recovery_tester = new BackupRecoveryTester(); + if (!backup_recovery_tester->isActive()) { + backup_recovery_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete backup_recovery_tester; + backup_recovery_tester = nullptr; + } + +protected: + static BackupRecoveryTester * backup_recovery_tester; +}; + +BackupRecoveryTester * BackupRecoveryTestFixture::backup_recovery_tester = nullptr; + +TEST_P(BackupRecoveryTestFixture, testBackupRecovery) +{ + float target_dist = std::get<0>(GetParam()); + float tolerance = std::get<1>(GetParam()); + + if (!backup_recovery_tester->isActive()) { + backup_recovery_tester->activate(); + } + + bool success = false; + success = backup_recovery_tester->defaultBackupRecoveryTest(target_dist, tolerance); + + // if intentionally backing into an obstacle, should fail. + // TODO(stevemacenski): uncomment this once note below completed. + // if (target_dist < -1.0) { + // success = !success; + // } + + EXPECT_EQ(true, success); +} + +// TODO(stevemacenski): See issue #1779, while the 3rd test collides, +// it returns success due to technical debt in the BT. This test will +// remain as a reminder to update this to a `false` case once the +// recovery server returns true values. + +INSTANTIATE_TEST_CASE_P( + BackupRecoveryTests, + BackupRecoveryTestFixture, + ::testing::Values( + std::make_tuple(-0.1, 0.1), + std::make_tuple(-0.2, 0.1), + std::make_tuple(-2.0, 0.1)), + testNameGenerator); + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_system_tests/src/recoveries/spin/CMakeLists.txt b/nav2_system_tests/src/recoveries/spin/CMakeLists.txt new file mode 100644 index 0000000000..34abacca01 --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_spin_recovery_exec test_spin_recovery_node) + +ament_add_gtest_executable(${test_spin_recovery_exec} + test_spin_recovery_node.cpp + spin_recovery_tester.cpp +) + +ament_target_dependencies(${test_spin_recovery_exec} + ${dependencies} +) + +ament_add_test(test_spin_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp new file mode 100644 index 0000000000..521c675955 --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp @@ -0,0 +1,224 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "spin_recovery_tester.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +SpinRecoveryTester::SpinRecoveryTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("spin_recovery_test"); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + client_ptr_ = rclcpp_action::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "spin"); + + publisher_ = + node_->create_publisher("initialpose", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); +} + +SpinRecoveryTester::~SpinRecoveryTester() +{ + if (is_active_) { + deactivate(); + } +} + +void SpinRecoveryTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + + // Wait for lifecycle_manager_navigation to activate recoveries_server + std::this_thread::sleep_for(10s); + + if (!client_ptr_) { + RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); + is_active_ = false; + return; + } + + if (!client_ptr_->wait_for_action_server(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + is_active_ = false; + return; + } + + RCLCPP_INFO(this->node_->get_logger(), "Spin action server is ready"); + is_active_ = true; +} + +void SpinRecoveryTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool SpinRecoveryTester::defaultSpinRecoveryTest( + const float target_yaw, + const double tolerance) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto goal_msg = Spin::Goal(); + goal_msg.target_yaw = target_yaw; + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + geometry_msgs::msg::PoseStamped initial_pose; + if (!nav2_util::getCurrentPose(initial_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + RCLCPP_INFO(node_->get_logger(), "Found current robot pose"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_get_result(goal_handle); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "result received"); + + geometry_msgs::msg::PoseStamped current_pose; + if (!nav2_util::getCurrentPose(current_pose, *tf_buffer_, "odom")) { + RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available."); + return false; + } + + double goal_yaw = angles::normalize_angle( + tf2::getYaw(initial_pose.pose.orientation) + target_yaw); + double dyaw = angles::shortest_angular_distance( + goal_yaw, tf2::getYaw(current_pose.pose.orientation)); + + if (fabs(dyaw) > tolerance) { + RCLCPP_ERROR( + node_->get_logger(), + "Angular distance from goal is %lf (tolerance %lf)", + fabs(dyaw), tolerance); + return false; + } + + return true; +} + +void SpinRecoveryTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = rclcpp::Time(); + pose.pose.pose.position.x = -2.0; + pose.pose.pose.position.y = -0.5; + pose.pose.pose.position.z = 0.0; + pose.pose.pose.orientation.x = 0.0; + pose.pose.pose.orientation.y = 0.0; + pose.pose.pose.orientation.z = 0.0; + pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 35; i++) { + pose.pose.covariance[i] = 0.0; + } + pose.pose.covariance[0] = 0.08; + pose.pose.covariance[7] = 0.08; + pose.pose.covariance[35] = 0.05; + + publisher_->publish(pose); + RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); +} + +void SpinRecoveryTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp new file mode 100644 index 0000000000..29622efbe7 --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ +#define RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "angles/angles.h" +#include "nav2_msgs/action/spin.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class SpinRecoveryTester +{ +public: + using Spin = nav2_msgs::action::Spin; + using GoalHandleSpin = rclcpp_action::ClientGoalHandle; + + SpinRecoveryTester(); + ~SpinRecoveryTester(); + + // Runs a single test with given target yaw + bool defaultSpinRecoveryTest( + float target_yaw, + double tolerance = 0.1); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +private: + void sendInitialPose(); + + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + bool is_active_; + bool initial_pose_received_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Node::SharedPtr node_; + + // Publisher to publish initial pose + rclcpp::Publisher::SharedPtr publisher_; + + // Subscriber for amcl pose + rclcpp::Subscription::SharedPtr subscription_; + + // Action client to call spin action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#endif // RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py new file mode 100755 index 0000000000..f1b7c40a78 --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py @@ -0,0 +1,102 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_spin_recovery_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp new file mode 100644 index 0000000000..cb83ed981e --- /dev/null +++ b/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp @@ -0,0 +1,105 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "spin_recovery_tester.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::SpinRecoveryTester; + +std::string testNameGenerator(const testing::TestParamInfo> & param) +{ + std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( + std::get<1>(param.param)); + name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); + return name; +} + +class SpinRecoveryTestFixture + : public ::testing::TestWithParam> +{ +public: + static void SetUpTestCase() + { + spin_recovery_tester = new SpinRecoveryTester(); + if (!spin_recovery_tester->isActive()) { + spin_recovery_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete spin_recovery_tester; + spin_recovery_tester = nullptr; + } + +protected: + static SpinRecoveryTester * spin_recovery_tester; +}; + +SpinRecoveryTester * SpinRecoveryTestFixture::spin_recovery_tester = nullptr; + +TEST_P(SpinRecoveryTestFixture, testSpinRecovery) +{ + float target_yaw = std::get<0>(GetParam()); + float tolerance = std::get<1>(GetParam()); + + bool success = false; + int num_tries = 3; + for (int i = 0; i != num_tries; i++) { + success = success || spin_recovery_tester->defaultSpinRecoveryTest(target_yaw, tolerance); + if (success) { + break; + } + } + + EXPECT_EQ(true, success); +} + +INSTANTIATE_TEST_CASE_P( + SpinRecoveryTests, + SpinRecoveryTestFixture, + ::testing::Values( + std::make_tuple(-M_PIf32 / 6.0, 0.1), + std::make_tuple(M_PI_4f32, 0.1), + std::make_tuple(-M_PI_2f32, 0.1), + std::make_tuple(M_PIf32, 0.1), + std::make_tuple(3.0 * M_PIf32 / 2.0, 0.15), + std::make_tuple(-2.0 * M_PIf32, 0.1), + std::make_tuple(4.0 * M_PIf32, 0.15)), + testNameGenerator); + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt new file mode 100644 index 0000000000..06755020d5 --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_wait_recovery_exec test_wait_recovery_node) + +ament_add_gtest_executable(${test_wait_recovery_exec} + test_wait_recovery_node.cpp + wait_recovery_tester.cpp +) + +ament_target_dependencies(${test_wait_recovery_exec} + ${dependencies} +) + +ament_add_test(test_wait_recovery + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_recovery_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py new file mode 100755 index 0000000000..484c16fda1 --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py @@ -0,0 +1,102 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params/nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites='', + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={ + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + testExecutable = os.getenv('TEST_EXECUTABLE') + + test1_action = ExecuteProcess( + cmd=[testExecutable], + name='test_wait_recovery_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp new file mode 100644 index 0000000000..e14ca5c7dc --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "wait_recovery_tester.hpp" + +using namespace std::chrono_literals; + +using nav2_system_tests::WaitRecoveryTester; + +std::string testNameGenerator(const testing::TestParamInfo> & param) +{ + std::string name = std::to_string(std::abs(std::get<0>(param.param))) + "_" + std::to_string( + std::get<1>(param.param)); + name.erase(std::remove(name.begin(), name.end(), '.'), name.end()); + return name; +} + +class WaitRecoveryTestFixture + : public ::testing::TestWithParam> +{ +public: + static void SetUpTestCase() + { + wait_recovery_tester = new WaitRecoveryTester(); + if (!wait_recovery_tester->isActive()) { + wait_recovery_tester->activate(); + } + } + + static void TearDownTestCase() + { + delete wait_recovery_tester; + wait_recovery_tester = nullptr; + } + +protected: + static WaitRecoveryTester * wait_recovery_tester; +}; + +WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; + +TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) +{ + float wait_time = std::get<0>(GetParam()); + + bool success = false; + int num_tries = 3; + for (int i = 0; i != num_tries; i++) { + success = success || wait_recovery_tester->recoveryTest(wait_time); + if (success) { + break; + } + } + + EXPECT_EQ(true, success); +} + +INSTANTIATE_TEST_CASE_P( + WaitRecoveryTests, + WaitRecoveryTestFixture, + ::testing::Values( + std::make_tuple(1.0, 0.0), + std::make_tuple(2.0, 0.0), + std::make_tuple(5.0, 0.0)), + testNameGenerator); + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + bool all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp new file mode 100644 index 0000000000..b6bdd62148 --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp @@ -0,0 +1,204 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "wait_recovery_tester.hpp" + +using namespace std::chrono_literals; +using namespace std::chrono; // NOLINT + +namespace nav2_system_tests +{ + +WaitRecoveryTester::WaitRecoveryTester() +: is_active_(false), + initial_pose_received_(false) +{ + node_ = rclcpp::Node::make_shared("wait_recovery_test"); + + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + client_ptr_ = rclcpp_action::create_client( + node_->get_node_base_interface(), + node_->get_node_graph_interface(), + node_->get_node_logging_interface(), + node_->get_node_waitables_interface(), + "wait"); + + publisher_ = + node_->create_publisher("initialpose", 10); + + subscription_ = node_->create_subscription( + "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&WaitRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); +} + +WaitRecoveryTester::~WaitRecoveryTester() +{ + if (is_active_) { + deactivate(); + } +} + +void WaitRecoveryTester::activate() +{ + if (is_active_) { + throw std::runtime_error("Trying to activate while already active"); + return; + } + + while (!initial_pose_received_) { + RCLCPP_WARN(node_->get_logger(), "Initial pose not received"); + sendInitialPose(); + std::this_thread::sleep_for(100ms); + rclcpp::spin_some(node_); + } + + // Wait for lifecycle_manager_navigation to activate recoveries_server + std::this_thread::sleep_for(10s); + + if (!client_ptr_) { + RCLCPP_ERROR(node_->get_logger(), "Action client not initialized"); + is_active_ = false; + return; + } + + if (!client_ptr_->wait_for_action_server(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + is_active_ = false; + return; + } + + RCLCPP_INFO(this->node_->get_logger(), "Wait action server is ready"); + is_active_ = true; +} + +void WaitRecoveryTester::deactivate() +{ + if (!is_active_) { + throw std::runtime_error("Trying to deactivate while already inactive"); + } + is_active_ = false; +} + +bool WaitRecoveryTester::recoveryTest( + const float wait_time) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto start_time = node_->now(); + auto goal_msg = Wait::Goal(); + goal_msg.time = rclcpp::Duration(wait_time, 0.0); + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_get_result(goal_handle); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get result call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was canceled"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + RCLCPP_INFO(node_->get_logger(), "result received"); + + + if ((node_->now() - start_time).seconds() < static_cast(wait_time)) { + return false; + } + + return true; +} + +void WaitRecoveryTester::sendInitialPose() +{ + geometry_msgs::msg::PoseWithCovarianceStamped pose; + pose.header.frame_id = "map"; + pose.header.stamp = rclcpp::Time(); + pose.pose.pose.position.x = -2.0; + pose.pose.pose.position.y = -0.5; + pose.pose.pose.position.z = 0.0; + pose.pose.pose.orientation.x = 0.0; + pose.pose.pose.orientation.y = 0.0; + pose.pose.pose.orientation.z = 0.0; + pose.pose.pose.orientation.w = 1.0; + for (int i = 0; i < 35; i++) { + pose.pose.covariance[i] = 0.0; + } + pose.pose.covariance[0] = 0.08; + pose.pose.covariance[7] = 0.08; + pose.pose.covariance[35] = 0.05; + + publisher_->publish(pose); + RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); +} + +void WaitRecoveryTester::amclPoseCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) +{ + initial_pose_received_ = true; +} + +} // namespace nav2_system_tests diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp new file mode 100644 index 0000000000..5daef5025b --- /dev/null +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2020 Samsung Research +// Copyright (c) 2020 Sarthak Mittal +// Copyright (c) 2018 Intel Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ +#define RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "angles/angles.h" +#include "nav2_msgs/action/wait.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/node_thread.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + +#include "tf2/utils.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace nav2_system_tests +{ + +class WaitRecoveryTester +{ +public: + using Wait = nav2_msgs::action::Wait; + using GoalHandleWait = rclcpp_action::ClientGoalHandle; + + WaitRecoveryTester(); + ~WaitRecoveryTester(); + + // Runs a single test with given target yaw + bool recoveryTest( + float time); + + void activate(); + + void deactivate(); + + bool isActive() const + { + return is_active_; + } + +private: + void sendInitialPose(); + + void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr); + + bool is_active_; + bool initial_pose_received_; + + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + rclcpp::Node::SharedPtr node_; + + // Publisher to publish initial pose + rclcpp::Publisher::SharedPtr publisher_; + + // Subscriber for amcl pose + rclcpp::Subscription::SharedPtr subscription_; + + // Action client to call wait action + rclcpp_action::Client::SharedPtr client_ptr_; +}; + +} // namespace nav2_system_tests + +#endif // RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 9784be1ac0..09017bfc6c 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -2,7 +2,7 @@ ament_add_test(test_bt_navigator GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 100 + TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml @@ -16,7 +16,7 @@ ament_add_test(test_bt_navigator_with_dijkstra GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 100 + TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml @@ -30,7 +30,7 @@ ament_add_test(test_dynamic_obstacle GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 100 + TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml @@ -44,7 +44,7 @@ ament_add_test(test_dynamic_obstacle # GENERATE_RESULT_FOR_RETURN_CODE_ZERO # COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_multi_robot_launch.py" # WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# TIMEOUT 120 +# TIMEOUT 180 # ENV # TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} # TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 5adfa97f17..87be85d613 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -41,9 +41,9 @@ def generate_launch_description(): bringup_dir = get_package_share_directory('nav2_bringup') robot1_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_1.yaml') + 'params/nav2_multirobot_params_1.yaml') robot2_params_file = os.path.join(bringup_dir, # noqa: F841 - 'params/nav2_multirobot_params_2.yaml') + 'params/nav2_multirobot_params_2.yaml') # Names and poses of the robots robots = [ @@ -62,7 +62,7 @@ def generate_launch_description(): spawn_robots_cmds.append( Node( package='nav2_gazebo_spawner', - node_executable='nav2_gazebo_spawner', + executable='nav2_gazebo_spawner', output='screen', arguments=[ '--robot_name', TextSubstitution(text=robot['name']), @@ -79,8 +79,8 @@ def generate_launch_description(): robot_state_pubs_cmds.append( Node( package='robot_state_publisher', - node_executable='robot_state_publisher', - node_namespace=TextSubstitution(text=robot['name']), + executable='robot_state_publisher', + namespace=TextSubstitution(text=robot['name']), output='screen', parameters=[{'use_sim_time': 'True'}], remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 53a57028ec..dde6618cdc 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -61,13 +61,13 @@ def generate_launch_description(): # using a local copy of TB3 urdf file Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index ca51089052..a66e2f7b89 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -203,22 +203,13 @@ def shutdown(self): except Exception as e: self.error_msg('Service call failed %r' % (e,)) - -def test_InitialPose(robot_tester, timeout, retries): - robot_tester.initial_pose_received = False - retry_count = 1 - while not robot_tester.initial_pose_received and retry_count <= retries: - retry_count += 1 - robot_tester.info_msg('Setting initial pose') - robot_tester.setInitialPose() - robot_tester.info_msg('Waiting for amcl_pose to be received') - rclpy.spin_once(robot_tester, timeout_sec=timeout) # wait for poseCallback - - if (robot_tester.initial_pose_received): - robot_tester.info_msg('test_InitialPose PASSED') - else: - robot_tester.info_msg('test_InitialPose FAILED') - return robot_tester.initial_pose_received + def wait_for_initial_pose(self): + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) def test_RobotMovesToGoal(robot_tester): @@ -233,10 +224,8 @@ def run_all_tests(robot_tester): result = True if (result): robot_tester.wait_for_node_active('amcl') - result = test_InitialPose(robot_tester, timeout=1, retries=10) - if (result): + robot_tester.wait_for_initial_pose() robot_tester.wait_for_node_active('bt_navigator') - if (result): result = robot_tester.runNavigateAction() # TODO(orduno) Test sending the navigation request through the topic interface. diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index 98434123da..aac4c6e2de 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -34,25 +34,25 @@ def generate_launch_description(): # Specify the actions start_tf_cmd_1 = Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']) start_tf_cmd_2 = Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_footprint']) start_tf_cmd_3 = Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']) start_tf_cmd_4 = Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']) diff --git a/nav2_system_tests/src/waypoint_follower/CMakeLists.txt b/nav2_system_tests/src/waypoint_follower/CMakeLists.txt index a8d005b175..2cd3c53a92 100644 --- a/nav2_system_tests/src/waypoint_follower/CMakeLists.txt +++ b/nav2_system_tests/src/waypoint_follower/CMakeLists.txt @@ -2,7 +2,7 @@ ament_add_test(test_waypoint_follower GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_py.launch" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 100 + TIMEOUT 180 ENV TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml diff --git a/nav2_system_tests/src/waypoint_follower/test_case_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index 23b5200110..06d1298f15 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/nav2_system_tests/src/waypoint_follower/test_case_py.launch @@ -59,13 +59,13 @@ def generate_launch_description(): # using a local copy of TB3 urdf file Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), Node( package='tf2_ros', - node_executable='static_transform_publisher', + executable='static_transform_publisher', output='screen', arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt index a1823a14a2..4ca5da258a 100644 --- a/nav2_util/CMakeLists.txt +++ b/nav2_util/CMakeLists.txt @@ -8,8 +8,6 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(SDL REQUIRED) -find_package(SDL_image REQUIRED) find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) find_package(lifecycle_msgs REQUIRED) @@ -24,8 +22,6 @@ set(dependencies tf2 tf2_geometry_msgs geometry_msgs - SDL - SDL_image nav_msgs rclcpp lifecycle_msgs @@ -54,7 +50,7 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} map_loader) +ament_export_libraries(${library_name}) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_util/include/nav2_util/geometry_utils.hpp b/nav2_util/include/nav2_util/geometry_utils.hpp index 2015487496..52bd5ebb41 100644 --- a/nav2_util/include/nav2_util/geometry_utils.hpp +++ b/nav2_util/include/nav2_util/geometry_utils.hpp @@ -15,6 +15,11 @@ #ifndef NAV2_UTIL__GEOMETRY_UTILS_HPP_ #define NAV2_UTIL__GEOMETRY_UTILS_HPP_ +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/quaternion.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -30,6 +35,33 @@ inline geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle) return tf2::toMsg(q); } +inline double euclidean_distance( + const geometry_msgs::msg::Point & pos1, + const geometry_msgs::msg::Point & pos2) +{ + double dx = pos1.x - pos2.x; + double dy = pos1.y - pos2.y; + double dz = pos1.z - pos2.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +inline double euclidean_distance( + const geometry_msgs::msg::Pose & pos1, + const geometry_msgs::msg::Pose & pos2) +{ + double dx = pos1.position.x - pos2.position.x; + double dy = pos1.position.y - pos2.position.y; + double dz = pos1.position.z - pos2.position.z; + return std::sqrt(dx * dx + dy * dy + dz * dz); +} + +inline double euclidean_distance( + const geometry_msgs::msg::PoseStamped & pos1, + const geometry_msgs::msg::PoseStamped & pos2) +{ + return euclidean_distance(pos1.pose, pos2.pose); +} + } // namespace geometry_utils } // namespace nav2_util diff --git a/nav2_util/include/nav2_util/map_loader/map_loader.hpp b/nav2_util/include/nav2_util/map_loader/map_loader.hpp deleted file mode 100644 index 9de0ecc206..0000000000 --- a/nav2_util/include/nav2_util/map_loader/map_loader.hpp +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright (c) 2008, Willow Garage, Inc. -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// Author: Brian Gerkey - -#ifndef NAV2_UTIL__MAP_LOADER__MAP_LOADER_HPP_ -#define NAV2_UTIL__MAP_LOADER__MAP_LOADER_HPP_ - -#include -#include "nav_msgs/msg/occupancy_grid.hpp" -#include "geometry_msgs/msg/twist.hpp" - -/** Map mode - * Default: TRINARY - - * value >= occ_th - Occupied (100) - * value <= free_th - Free (0) - * otherwise - Unknown - * SCALE - - * alpha < 1.0 - Unknown - * value >= occ_th - Occupied (100) - * value <= free_th - Free (0) - * otherwise - f( (free_th, occ_th) ) = (0, 100) - * (linearly map in between values to (0,100) - * RAW - - * value = value - */ -enum MapMode {TRINARY, SCALE, RAW}; - -namespace map_loader -{ - -/** Read the image from file and fill out the resp object, for later - * use when our services are requested. - * - * @param image_file_name The image file to read from - * @param resolution The resolution of the map (gets stored in resp) - * @param negate If true, then whiter pixels are occupied, and blacker - * pixels are free - * @param occupancy_threshold Threshold above which pixels are occupied - * @param free_threshold Threshold below which pixels are free - * @param origin Triple specifying 2-D pose of lower-left corner of image - * @param mode Map mode - * @throws std::runtime_error If the image file can't be loaded - * */ - -nav_msgs::msg::OccupancyGrid loadMapFromFile( - const std::string image_file_name, const double resolution, const bool negate, - const double occupancy_threshold, const double free_threshold, - const geometry_msgs::msg::Twist origin, const MapMode mode = TRINARY); -} // namespace map_loader - -#endif // NAV2_UTIL__MAP_LOADER__MAP_LOADER_HPP_ diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 690e654178..e4767abf5f 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -322,6 +322,7 @@ class SimpleActionServer { if (!is_active(current_handle_)) { error_msg("Trying to publish feedback when the current goal handle is not active"); + return; } current_handle_->publish_feedback(feedback); diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 34d337dabd..48f11c8a5f 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -20,8 +20,6 @@ tf2 tf2_ros tf2_geometry_msgs - sdl - sdl-image lifecycle_msgs rclcpp_action test_msgs diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 70f1c7b624..bd903ef6d1 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -21,8 +21,6 @@ ament_target_dependencies(${library_name} tf2_geometry_msgs ) -add_subdirectory(map_loader) - add_executable(lifecycle_bringup lifecycle_bringup_commandline.cpp ) @@ -39,9 +37,13 @@ target_link_libraries(dump_params ${Boost_PROGRAM_OPTIONS_LIBRARY}) install(TARGETS ${library_name} - lifecycle_bringup - dump_params ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS + lifecycle_bringup + dump_params RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp index b47c6815b7..34672f2b21 100644 --- a/nav2_util/src/dump_params.cpp +++ b/nav2_util/src/dump_params.cpp @@ -388,6 +388,14 @@ validate( boost::any_cast &>(v).values.swap(result); } +#ifdef _WIN32 +static const char * basename(const char * filepath) +{ + const char * base = std::strrchr(filepath, '/'); + return base ? (base + 1) : filepath; +} +#endif + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); diff --git a/nav2_util/src/map_loader/CMakeLists.txt b/nav2_util/src/map_loader/CMakeLists.txt deleted file mode 100644 index 42f1434667..0000000000 --- a/nav2_util/src/map_loader/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -add_library(map_loader SHARED - map_loader.cpp -) - -ament_target_dependencies(map_loader - geometry_msgs - tf2 - tf2_geometry_msgs - tf2_ros - nav_msgs - SDL - SDL_image -) - -target_include_directories(map_loader PRIVATE - ${SDL_INCLUDE_DIR} - ${SDL_IMAGE_INCLUDE_DIRS} -) - -target_link_libraries(map_loader - ${SDL_LIBRARY} - ${SDL_IMAGE_LIBRARIES} -) - -install(TARGETS - map_loader - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) diff --git a/nav2_util/src/map_loader/map_loader.cpp b/nav2_util/src/map_loader/map_loader.cpp deleted file mode 100644 index 734a23d3e2..0000000000 --- a/nav2_util/src/map_loader/map_loader.cpp +++ /dev/null @@ -1,158 +0,0 @@ -// Copyright (c) 2008, Willow Garage, Inc. -// All rights reserved. -// -// Software License Agreement (BSD License 2.0) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. -// * Neither the name of the Willow Garage, Inc. nor the names of its -// contributors may be used to endorse or promote products derived -// from this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -// Author: Brian Gerkey - -#include "nav2_util/map_loader/map_loader.hpp" -#include -#include -// We use SDL_image to load the image from disk -#include -#include -#include -#include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" - - -// compute linear index for given map coords -#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) - -namespace map_loader -{ - -nav_msgs::msg::OccupancyGrid loadMapFromFile( - const std::string image_file_name, const double resolution, const bool negate, - const double occupancy_threshold, const double free_threshold, - const geometry_msgs::msg::Twist origin, const MapMode mode) -{ - SDL_Surface * img; - - unsigned char * pixels; - unsigned char * p; - unsigned char value; - int rowstride, n_channels, avg_channels; - unsigned int i, j; - int k; - double occ; - int alpha; - int color_sum; - double color_avg; - - // Load the image using SDL. If we get NULL back, the image load failed. - if (!(img = IMG_Load(image_file_name.c_str()))) { - std::string errmsg = std::string("failed to open image file \"") + - image_file_name + std::string("\": ") + IMG_GetError(); - throw std::runtime_error(errmsg); - } - - // Copy the image data into the map structure - nav_msgs::msg::OccupancyGrid map; - map.info.width = img->w; - map.info.height = img->h; - map.info.resolution = resolution; - map.info.origin.position.x = origin.linear.x; - map.info.origin.position.y = origin.linear.y; - map.info.origin.position.z = origin.linear.z; - - tf2::Quaternion quaternion; - // yaw, pitch and roll are rotations in z, y, x respectively - quaternion.setRPY(origin.angular.x, origin.angular.y, origin.angular.z); - map.info.origin.orientation = tf2::toMsg(quaternion); - - // Allocate space to hold the data - map.data.resize(map.info.width * map.info.height); - - // Get values that we'll need to iterate through the pixels - rowstride = img->pitch; - n_channels = img->format->BytesPerPixel; - - // NOTE: Trinary mode still overrides here to preserve existing behavior. - // Alpha will be averaged in with color channels when using trinary mode. - if (mode == TRINARY || !img->format->Amask) { - avg_channels = n_channels; - } else { - avg_channels = n_channels - 1; - } - - // Copy pixel data into the map structure - pixels = (unsigned char *)(img->pixels); - for (j = 0; j < map.info.height; j++) { - for (i = 0; i < map.info.width; i++) { - // Compute mean of RGB for this pixel - p = pixels + j * rowstride + i * n_channels; - color_sum = 0; - for (k = 0; k < avg_channels; k++) { - color_sum += *(p + (k)); - } - color_avg = color_sum / static_cast(avg_channels); - - if (n_channels == 1) { - alpha = 1; - } else { - alpha = *(p + n_channels - 1); - } - if (negate) { - color_avg = 255 - color_avg; - } - if (mode == RAW) { - value = color_avg; - map.data[MAP_IDX(map.info.width, i, map.info.height - j - 1)] = value; - continue; - } - - // If negate is true, we consider blacker pixels free, and whiter - // pixels occupied. Otherwise, it's vice versa. - occ = (255 - color_avg) / 255.0; - - // Apply thresholds to RGB means to determine occupancy values for - // map. Note that we invert the graphics-ordering of the pixels to - // produce a map with cell (0,0) in the lower-left corner. - if (occ > occupancy_threshold) { - value = +100; - } else if (occ < free_threshold) { - value = 0; - } else if (mode == TRINARY || alpha < 1.0) { - value = -1; - } else { - double ratio = (occ - free_threshold) / (occupancy_threshold - free_threshold); - value = 99 * ratio; - } - - map.data[MAP_IDX(map.info.width, i, map.info.height - j - 1)] = value; - } - } - SDL_FreeSurface(img); - - return map; -} - -} // namespace map_loader diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 4ebfaf205e..6f735e6d25 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -57,7 +57,7 @@ bool getCurrentPose( RCLCPP_ERROR( logger, "Transform timeout with tolerance: %.4f", transform_timeout); - } catch (...) { + } catch (tf2::TransformException & ex) { RCLCPP_ERROR( logger, "Failed to transform from %s to %s", global_frame.c_str(), robot_frame.c_str()); diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index e12e36e16c..8840edf675 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -24,3 +24,15 @@ target_link_libraries(test_actions ${library_name}) ament_add_gtest(test_lifecycle_node test_lifecycle_node.cpp) ament_target_dependencies(test_lifecycle_node rclcpp_lifecycle) target_link_libraries(test_lifecycle_node ${library_name}) + +ament_add_gtest(test_lifecycle_cli_node test_lifecycle_cli_node.cpp) +ament_target_dependencies(test_lifecycle_cli_node rclcpp_lifecycle) +target_link_libraries(test_lifecycle_cli_node ${library_name}) + +ament_add_gtest(test_geometry_utils test_geometry_utils.cpp) +ament_target_dependencies(test_geometry_utils geometry_msgs) +target_link_libraries(test_geometry_utils ${library_name}) + +ament_add_gtest(test_robot_utils test_robot_utils.cpp) +ament_target_dependencies(test_robot_utils geometry_msgs) +target_link_libraries(test_robot_utils ${library_name}) diff --git a/nav2_util/test/test_actions.cpp b/nav2_util/test/test_actions.cpp index 292fb7f37f..11d5c54f11 100644 --- a/nav2_util/test/test_actions.cpp +++ b/nav2_util/test/test_actions.cpp @@ -74,6 +74,13 @@ class FibonacciServerNode : public rclcpp::Node void on_term() { + // when nothing's running make sure everything's dead. + const std::shared_ptr a = action_server_->accept_pending_goal(); + const std::shared_ptr b = action_server_->get_current_goal(); + assert(a == b); + assert(action_server_->is_cancel_requested() == false); + auto feedback = std::make_shared(); + action_server_->publish_feedback(feedback); action_server_.reset(); } @@ -489,6 +496,24 @@ TEST_F(ActionTest, test_simple_action_preemption_after_succeeded) SUCCEED(); } +TEST_F(ActionTest, test_handle_goal_deactivated) +{ + node_->deactivate_server(); + auto goal = Fibonacci::Goal(); + goal.order = 12; + + // Send the goal + auto future_goal_handle = node_->action_client_->async_send_goal(goal); + EXPECT_EQ( + rclcpp::spin_until_future_complete( + node_, + future_goal_handle), rclcpp::executor::FutureReturnCode::SUCCESS); + + node_->activate_server(); + + SUCCEED(); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/nav2_util/test/test_geometry_utils.cpp b/nav2_util/test/test_geometry_utils.cpp new file mode 100644 index 0000000000..9c379575bb --- /dev/null +++ b/nav2_util/test/test_geometry_utils.cpp @@ -0,0 +1,51 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Sarthak Mittal +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nav2_util/geometry_utils.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "gtest/gtest.h" + +using nav2_util::geometry_utils::euclidean_distance; + +TEST(GeometryUtils, euclidean_distance_point) +{ + geometry_msgs::msg::Point point1; + point1.x = 3.0; + point1.y = 2.0; + point1.z = 1.0; + + geometry_msgs::msg::Point point2; + point2.x = 1.0; + point2.y = 2.0; + point2.z = 3.0; + + ASSERT_NEAR(euclidean_distance(point1, point2), 2.82843, 1e-5); +} + +TEST(GeometryUtils, euclidean_distance_pose) +{ + geometry_msgs::msg::Pose pose1; + pose1.position.x = 7.0; + pose1.position.y = 4.0; + pose1.position.z = 3.0; + + geometry_msgs::msg::Pose pose2; + pose2.position.x = 17.0; + pose2.position.y = 6.0; + pose2.position.z = 2.0; + + ASSERT_NEAR(euclidean_distance(pose1, pose2), 10.24695, 1e-5); +} diff --git a/nav2_util/test/test_lifecycle_cli_node.cpp b/nav2_util/test/test_lifecycle_cli_node.cpp new file mode 100644 index 0000000000..2177412e7d --- /dev/null +++ b/nav2_util/test/test_lifecycle_cli_node.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2020 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_ +#define NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_ + +#include +#include +#include "gtest/gtest.h" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/lifecycle_utils.hpp" +#include "nav2_util/node_thread.hpp" +#include "rclcpp/rclcpp.hpp" + +class DummyNode : public nav2_util::LifecycleNode +{ +public: + DummyNode() + : nav2_util::LifecycleNode("nav2_test_cli", "") + { + activated = false; + } + + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*state*/) + { + activated = true; + return nav2_util::CallbackReturn::SUCCESS; + } + + bool activated; +}; + +class Handle +{ +public: + Handle() + { + node = std::make_shared(); + thread = std::make_shared(node->get_node_base_interface()); + } + ~Handle() + { + thread.reset(); + node.reset(); + } + + std::shared_ptr thread; + std::shared_ptr node; +}; + +class RclCppFixture +{ +public: + RclCppFixture() + { + rclcpp::init(0, nullptr); + } + + ~RclCppFixture() + { + rclcpp::shutdown(); + } +}; + +RclCppFixture g_rclcppfixture; + +TEST(LifeycleCLI, fails_no_node_name) +{ + Handle handle; + auto rc = system("ros2 run nav2_util lifecycle_bringup"); + (void)rc; + sleep(1); + // check node didn't mode + EXPECT_EQ(handle.node->activated, false); + SUCCEED(); +} + +TEST(LifeycleCLI, succeeds_node_name) +{ + Handle handle; + auto rc = system("ros2 run nav2_util lifecycle_bringup nav2_test_cli"); + sleep(3); + // check node moved + (void)rc; + EXPECT_EQ(handle.node->activated, true); + SUCCEED(); +} + +#endif // NAV2_UTIL__TEST__TEST_LIFECYCLE_CLI_NODE_HPP_ diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index 0f5cb0ebd5..69d74f7958 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -18,6 +18,7 @@ using nav2_util::sanitize_node_name; using nav2_util::generate_internal_node_name; using nav2_util::generate_internal_node; +using nav2_util::add_namespaces; using nav2_util::time_to_string; TEST(SanitizeNodeName, SanitizeNodeName) @@ -47,3 +48,9 @@ TEST(GenerateInternalNodeName, GenerateNodeName) ASSERT_EQ(defaultName[0], '_'); ASSERT_EQ(defaultName.length(), 9u); } + +TEST(AddNamespaces, AddNamespaceSlash) +{ + ASSERT_EQ(add_namespaces("hi", "bye"), "hi/bye"); + ASSERT_EQ(add_namespaces("hi/", "bye"), "/hi/bye"); +} diff --git a/nav2_util/test/test_robot_utils.cpp b/nav2_util/test/test_robot_utils.cpp new file mode 100644 index 0000000000..a3530048a4 --- /dev/null +++ b/nav2_util/test/test_robot_utils.cpp @@ -0,0 +1,32 @@ +// Copyright (c) 2020 Samsung Research +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/robot_utils.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "gtest/gtest.h" +#include "nav2_util/node_thread.hpp" +#include "tf2_ros/create_timer_ros.h" + +TEST(RobotUtils, LookupExceptionError) +{ + rclcpp::init(0, nullptr); + auto node = std::make_shared("name", rclcpp::NodeOptions()); + geometry_msgs::msg::PoseStamped global_pose; + tf2_ros::Buffer tf(node->get_clock()); + ASSERT_FALSE(nav2_util::getCurrentPose(global_pose, tf, "map", "base_link", 0.1)); +} diff --git a/nav2_util/test/test_string_utils.cpp b/nav2_util/test/test_string_utils.cpp index 20fc3d35bd..488bd1ea00 100644 --- a/nav2_util/test/test_string_utils.cpp +++ b/nav2_util/test/test_string_utils.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "nav2_util/string_utils.hpp" #include "gtest/gtest.h" @@ -26,4 +28,5 @@ TEST(Split, SplitFunction) ASSERT_EQ(split("foo:bar:", ':'), Tokens({"foo", "bar", ""})); ASSERT_EQ(split(":", ':'), Tokens({"", ""})); ASSERT_EQ(split("foo::bar", ':'), Tokens({"foo", "", "bar"})); + ASSERT_TRUE(nav2_util::strip_leading_slash(std::string("/hi")) == std::string("hi")); } diff --git a/nav2_voxel_grid/CMakeLists.txt b/nav2_voxel_grid/CMakeLists.txt index fe5d7fe753..4b88a87d86 100644 --- a/nav2_voxel_grid/CMakeLists.txt +++ b/nav2_voxel_grid/CMakeLists.txt @@ -25,7 +25,7 @@ ament_target_dependencies(voxel_grid install(TARGETS voxel_grid ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION bin ) install(DIRECTORY include/ diff --git a/nav2_voxel_grid/src/voxel_grid.cpp b/nav2_voxel_grid/src/voxel_grid.cpp index 4eca35f317..c41bc40533 100644 --- a/nav2_voxel_grid/src/voxel_grid.cpp +++ b/nav2_voxel_grid/src/voxel_grid.cpp @@ -35,7 +35,6 @@ * Author: Eitan Marder-Eppstein *********************************************************************/ #include -#include namespace nav2_voxel_grid { diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index d7b5460261..71925e74b5 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -49,9 +49,13 @@ ament_target_dependencies(${library_name} ${dependencies} ) -install(TARGETS ${executable_name} ${library_name} +install(TARGETS ${library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 978ecb834c..3a0edabce6 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -26,7 +26,7 @@ namespace nav2_waypoint_follower { WaypointFollower::WaypointFollower() -: nav2_util::LifecycleNode("WaypointFollower", "", true) +: nav2_util::LifecycleNode("WaypointFollower", "", false) { RCLCPP_INFO(get_logger(), "Creating"); @@ -47,8 +47,9 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); + // use suffix '_rclcpp_node' to keep parameter file consistency #1773 client_node_ = std::make_shared( - std::string(get_name()) + std::string("_client_node")); + std::string(get_name()) + std::string("_rclcpp_node")); nav_to_pose_client_ = rclcpp_action::create_client( client_node_, "navigate_to_pose"); @@ -126,7 +127,7 @@ WaypointFollower::followWaypoints() static_cast(goal->poses.size())); rclcpp::Rate r(loop_rate_); - uint goal_index = 0; + uint32_t goal_index = 0; bool new_goal = true; while (rclcpp::ok()) { diff --git a/release_branch.Dockerfile b/release_branch.Dockerfile deleted file mode 100644 index 3a03113420..0000000000 --- a/release_branch.Dockerfile +++ /dev/null @@ -1,43 +0,0 @@ -# This dockerfile expects proxies to be set via --build-arg if needed -# It also expects to be contained in the /navigation2 root folder for file copy -# -# Example build command: -# export CMAKE_BUILD_TYPE=Debug -# docker build -t nav2:release_branch \ -# --build-arg FROM_IMAGE=dashing \ -# --build-arg CMAKE_BUILD_TYPE \ -# -f Dockerfile.release_branch ./ - -ARG FROM_IMAGE=dashing -FROM ros:$FROM_IMAGE - -RUN rosdep update - -# copy ros package repo -ENV NAV2_WS /opt/nav2_ws -RUN mkdir -p $NAV2_WS/src -WORKDIR $NAV2_WS/src -COPY ./ navigation2/ - -# install navigation2 package dependencies -WORKDIR $NAV2_WS -RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - apt-get update && \ - rosdep install -q -y \ - --from-paths \ - src \ - --ignore-src \ - && rm -rf /var/lib/apt/lists/* - -# build navigation2 package source -ARG CMAKE_BUILD_TYPE=Release -RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ - colcon build \ - --symlink-install \ - --cmake-args \ - -DCMAKE_BUILD_TYPE=$CMAKE_BUILD_TYPE - -# source navigation2 workspace from entrypoint -RUN sed --in-place --expression \ - '$isource "$NAV2_WS/install/setup.bash"' \ - /ros_entrypoint.sh diff --git a/sphinx_doc/LICENSE b/sphinx_doc/LICENSE deleted file mode 100644 index 62da4936b3..0000000000 --- a/sphinx_doc/LICENSE +++ /dev/null @@ -1 +0,0 @@ -Apache 2.0 diff --git a/sphinx_doc/Makefile b/sphinx_doc/Makefile deleted file mode 100644 index 3ffb456a7c..0000000000 --- a/sphinx_doc/Makefile +++ /dev/null @@ -1,64 +0,0 @@ -# Minimal makefile for Sphinx documentation -# - -ifeq ($(VERBOSE),1) - Q = -else - Q = @ -endif - -# You can set these variables from the command line. -SPHINXOPTS ?= -q -SPHINXBUILD = sphinx-build -SPHINXPROJ = "Navigation 2 Documentation" -SOURCEDIR = . -BUILDDIR = _build - -DOC_TAG ?= development -RELEASE ?= latest -PUBLISHDIR = /tmp/navigation2 - -# Put it first so that "make" without argument is like "make help". -help: - @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) - @echo "" - @echo "make publish" - @echo " publish generated html to thesofproject.github.io site:" - @echo " specify RELEASE=name to publish as a tagged release version" - @echo " and placed in a version subfolder. Requires repo merge permission." - -.PHONY: help Makefile - -# Generate the doxygen xml (for Sphinx) and copy the doxygen html to the -# api folder for publishing along with the Sphinx-generated API docs. - -html: - $(Q)$(SPHINXBUILD) -t $(DOC_TAG) -b html -d $(BUILDDIR)/doctrees $(SOURCEDIR) $(BUILDDIR)/html $(SPHINXOPTS) $(O) - -# Remove generated content (Sphinx and doxygen) - -clean: - rm -fr $(BUILDDIR) - -# Copy material over to the GitHub pages staging repo -# along with a README - -publish: - git clone --reference .. git@github.com:ros-planning/navigation2.git $(PUBLISHDIR) - cd $(PUBLISHDIR) && \ - git checkout gh-pages && \ - git config user.email "navigation2-ci@circleci.com" && \ - git config user.name "navigation2-ci" - rm -fr $(PUBLISHDIR)/* - cp -r $(BUILDDIR)/html/* $(PUBLISHDIR) - cp scripts/.nojekyll $(PUBLISHDIR)/.nojekyll - cd $(PUBLISHDIR) && \ - git add -A && \ - git diff-index --quiet HEAD || \ - (git commit -s -m "[skip ci] publish $(RELEASE)" && git push origin) - - -# Catch-all target: route all unknown targets to Sphinx using the new -# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). -%: Makefile doxy - @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/sphinx_doc/README.md b/sphinx_doc/README.md deleted file mode 100644 index ff7c1c8099..0000000000 --- a/sphinx_doc/README.md +++ /dev/null @@ -1,6 +0,0 @@ -# Navigation2 Documentation - -This folder holds the source and configuration files used to generate the -[Navigation2 documentation](https://ros-planning.github.io/navigation2/) web site. - -Learn how to setup and generate documentation by reading the [howtos](https://ros-planning.github.io/navigation2/howtos/index.html). diff --git a/sphinx_doc/_themes/otc_tcs_sphinx_theme/.gitignore b/sphinx_doc/_themes/otc_tcs_sphinx_theme/.gitignore deleted file mode 100644 index bee8a64b79..0000000000 --- a/sphinx_doc/_themes/otc_tcs_sphinx_theme/.gitignore +++ /dev/null @@ -1 +0,0 @@ -__pycache__ diff --git a/sphinx_doc/_themes/otc_tcs_sphinx_theme/layout.html b/sphinx_doc/_themes/otc_tcs_sphinx_theme/layout.html deleted file mode 100644 index 1bdd7ce460..0000000000 --- a/sphinx_doc/_themes/otc_tcs_sphinx_theme/layout.html +++ /dev/null @@ -1,14 +0,0 @@ -{% extends "sphinx_rtd_theme/layout.html" %} - -{% block extrahead %} - - - -{% endblock %} - - -{% block footer %} - - - -{% endblock %} diff --git a/sphinx_doc/_themes/otc_tcs_sphinx_theme/readme.rst b/sphinx_doc/_themes/otc_tcs_sphinx_theme/readme.rst deleted file mode 100644 index 7f5f284f28..0000000000 --- a/sphinx_doc/_themes/otc_tcs_sphinx_theme/readme.rst +++ /dev/null @@ -1,23 +0,0 @@ -OTC-TCS Sphinx Theme -#################### - -Built on top of the popular Read the Docs Sphinx theme, this theme -has a few small formatting/color improvements and implements collapsible -sections. It is in active development in order to support OTC and other -Intel organizations to publish high quality, consistent documentation -for open source projects. - -How to use it -************* - -#. Download or clone the repository -#. Create a ``_themes`` directory in main directory of your sphinx - documentation -#. Install sphinx_rtd_theme using pip: ``pip3 install sphinx_rtd_theme`` -#. copy ``otc_tcs_sphinx_theme`` directory into the new ``_themes`` directory -#. Add the following to your ``conf.py``: - - .. code-block:: python - - html_theme = 'otc_tcs_sphinx_theme' - html_theme_path = ['_themes'] diff --git a/sphinx_doc/_themes/otc_tcs_sphinx_theme/static/tcs_theme.css b/sphinx_doc/_themes/otc_tcs_sphinx_theme/static/tcs_theme.css deleted file mode 100644 index 02d5051b2d..0000000000 --- a/sphinx_doc/_themes/otc_tcs_sphinx_theme/static/tcs_theme.css +++ /dev/null @@ -1,172 +0,0 @@ -/* -- Extra CSS styles for content (RTD theme) ----------------------- */ - -/* make the page width fill the window */ -.wy-nav-content { - max-width: 1080px; -} - -/* increase the size of the side bar from 300px default to 320px */ -.wy-nav-side { - width: 320px; -} - -.wy-side-scroll { - width: 340px; -} - -.wy-side-nav-search { - width: 320px; -} - -.wy-menu-vertical { - width: 320px; -} - -/* (temporarily) add an under development tagline to the bread crumb -.wy-breadcrumbs::after { - content: " (Content under development)"; - background-color: #FFFACD; - color: red; - font-weight: bold; -} -*/ - -/* code block highlight color in rtd changed to lime green, no no no */ - -.rst-content tt.literal, .rst-content code.literal, .highlight { - background: #f0f0f0; -} -.rst-content tt.literal, .rst-content code.literal { - color: #000000; -} - -/* Make the version number more visible */ -.wy-side-nav-search>div.version { - color: rgba(255,255,255,1); -} - -/* squish the space between a paragraph before a list */ -div > p + ul, div > p + ol { - margin-top: -20px; -} - -/* add some space before the figure caption */ -p.caption { - border-top: 1px solid; - margin-top: 1em; -} - -/* add a colon after the figure/table number (before the caption) */ -span.caption-number::after { - content: ": "; -} - -p.extrafooter { - text-align: right; - margin-top: -36px; -} - -table.align-center { - display: table !important; -} - - -.code-block-caption { - color: #000; - font: italic 85%/1 arial,sans-serif; - padding: 1em 0; - text-align: center; -} - -/* make .. hlist:: tables fill the page */ -table.hlist { - width: 95% !important; -} - -/* override rtd theme white-space no-wrap in table heading and content */ -th,td { - white-space: normal !important; -} - -/* tweak for doxygen-generated API headings (for RTD theme) */ -.rst-content dl.group>dt, .rst-content dl.group>dd>p { - display:none !important; -} -.rst-content dl.group { - margin: 0 0 12px 0px; -} -.rst-content dl.group>dd { - margin-left: 0 !important; -} -.rst-content p.breathe-sectiondef-title { - text-decoration: underline; /* for API sub-headings */ - font-size: 1.25rem; - font-weight: bold; - margin-bottom: 12px; -} - -.rst-content div.breathe-sectiondef { - padding-left: 0 !important; -} - -.clps1 { - font-size: 175%; -} - -.clps2 { - font-size: 150%; -} - -.clps3 { - font-size: 125%; -} - -.clps4 { - font-size: 115%; -} - -.clps5 { - font-size: 110%; -} - -.clps6 { - font-size: 100%; -} - -.collapsible { - margin-left: -10px; - background-color: #f1f1f1; - cursor: pointer; - padding: 18px 18px 18px 10px; - width: 100%; - border: none; - text-align: left; - outline: none; - font-weight: 700; - font-family: "Roboto Slab","ff-tisa-web-pro","Georgia",Arial,sans-serif; -} - -.collapsible:hover { - background-color: #d8d8d8; -} - -.collapsible:after { - content: '\002B'; - font-weight: bold; - float: right; - margin-left: 5px; -} - -.active:after { - content: "\2212"; -} - -.content-collapse { - overflow: hidden; - transition: max-height 0.2s ease-out; -} - -.header__menu_list li { - display: inline; - margin-left: 20px; -} \ No newline at end of file diff --git a/sphinx_doc/_themes/otc_tcs_sphinx_theme/static/tcs_theme.js b/sphinx_doc/_themes/otc_tcs_sphinx_theme/static/tcs_theme.js deleted file mode 100644 index 9064b9f774..0000000000 --- a/sphinx_doc/_themes/otc_tcs_sphinx_theme/static/tcs_theme.js +++ /dev/null @@ -1,50 +0,0 @@ -var i; -var contents = document.getElementsByClassName("content-collapse section"); - -for (i = 0; i < contents.length; i++) { - - //Make sure the "content-collapse section" class is occurring in
- if (contents[i].tagName.toLowerCase() == 'div') { - var element = contents[i].children[0]; - var element_type = element.tagName.toLowerCase(); - var span_id; - var spanElement; - - //if the next element is a span grab the id and skip to the header - if (element_type == 'span') { - span_id = element.id; - element.id = ""; - element = contents[i].children[1]; - element_type = element.tagName.toLowerCase(); - } - - var btn = document.createElement("BUTTON"); - //If it is a header capture which level and pass on to button - if (element_type.length == 2 && element_type[0] == 'h') { - var newClass = 'clps' + element_type[1]; - //collapses the section by default only if javascript is working - contents[i].style.maxHeight = 0; - //Build the button and define behavior - btn.className += " " + newClass; - btn.innerHTML = element.innerHTML; - btn.className += " collapsible"; - btn.id = span_id; - btn.addEventListener("click", function() { - this.classList.toggle("active"); - var content = this.nextElementSibling; - if (content.style.maxHeight != "0px"){ - content.style.maxHeight = 0; - } else { - content.style.maxHeight = content.scrollHeight + "px"; - } - }); - - //Add the button to the page and remove the header - contents[i].parentNode.insertBefore(btn, contents[i]); - contents[i].removeChild(element); - }else{ - //reset span id if it isn't followed by Hx element - spanElement.id = span_id; - } - } -} \ No newline at end of file diff --git a/sphinx_doc/_themes/otc_tcs_sphinx_theme/theme.conf b/sphinx_doc/_themes/otc_tcs_sphinx_theme/theme.conf deleted file mode 100644 index fc97c5522d..0000000000 --- a/sphinx_doc/_themes/otc_tcs_sphinx_theme/theme.conf +++ /dev/null @@ -1,2 +0,0 @@ -[theme] -inherit = sphinx_rtd_theme diff --git a/sphinx_doc/about/images/coming_soon.jpeg b/sphinx_doc/about/images/coming_soon.jpeg deleted file mode 100644 index 3fe5980c23..0000000000 Binary files a/sphinx_doc/about/images/coming_soon.jpeg and /dev/null differ diff --git a/sphinx_doc/about/images/move_base_compare_2.png b/sphinx_doc/about/images/move_base_compare_2.png deleted file mode 100644 index 5b1a31cec3..0000000000 Binary files a/sphinx_doc/about/images/move_base_compare_2.png and /dev/null differ diff --git a/sphinx_doc/about/images/rb1.png b/sphinx_doc/about/images/rb1.png deleted file mode 100644 index f04040137b..0000000000 Binary files a/sphinx_doc/about/images/rb1.png and /dev/null differ diff --git a/sphinx_doc/about/images/rover.png b/sphinx_doc/about/images/rover.png deleted file mode 100644 index 416fa6889e..0000000000 Binary files a/sphinx_doc/about/images/rover.png and /dev/null differ diff --git a/sphinx_doc/about/images/tb2.png b/sphinx_doc/about/images/tb2.png deleted file mode 100644 index c0d4138c1c..0000000000 Binary files a/sphinx_doc/about/images/tb2.png and /dev/null differ diff --git a/sphinx_doc/about/images/tb3.png b/sphinx_doc/about/images/tb3.png deleted file mode 100644 index 05811eea59..0000000000 Binary files a/sphinx_doc/about/images/tb3.png and /dev/null differ diff --git a/sphinx_doc/about/images/tiago.png b/sphinx_doc/about/images/tiago.png deleted file mode 100644 index 28570cee27..0000000000 Binary files a/sphinx_doc/about/images/tiago.png and /dev/null differ diff --git a/sphinx_doc/about/images/yunji.png b/sphinx_doc/about/images/yunji.png deleted file mode 100644 index 695c5da056..0000000000 Binary files a/sphinx_doc/about/images/yunji.png and /dev/null differ diff --git a/sphinx_doc/about/index.rst b/sphinx_doc/about/index.rst deleted file mode 100644 index 0e1420d501..0000000000 --- a/sphinx_doc/about/index.rst +++ /dev/null @@ -1,73 +0,0 @@ -.. _about: - -About and Contact -################# - -.. toctree:: - :hidden: - - robots.rst - ros1_comparison.rst - -About -***** - -Navigation is a community effort to lower the barrier of entry of mobile robotics technology for all. -This project is one of the flagship projects along with MoveIt that acts as an applications entry point and frameworks for ROS. -Navigation in ROS2 builds on the wild success of the original Navigation Stack (Nav Stack) in ROS (1). -This project has been used by researchers, educators, and industry for over 10 years. -There are very few projects that have lasted as long or were as successful as ROS (1) Navigation. -We would like to thank David Lu!! and Mike Ferguson for their tireless over the span of a decade to keep the Nav Stack alive and well. -Without their hard work, this project would not have been able to happen. -If you're interested in a comparison between ROS1 Navigation and ROS2 Navigation, see :ref:`ros1_comparison`. -For a list of robots using Navigation2, see :ref:`robots`. - -Aslas, as time moves on, so must we. -ROS (1) had a great run and those of us that build projects using it will remember it fondly(ish). -With change comes possibilities. -Navigation2 builds on the success but redesigns the framework to be more flexible and the feedback gathered over 10 years. - -We strive to create an open community and encourage new ROS users and experts alike to collaborate. -However, that can't happen without your issues, pull requests, and support. -Navigation, like all open-source projects, is kept going by a dedicated group of developers, maintainers, users, and collaborators. -We would like to thank here our current and past contributors and maintainers. - -If you or your organization are interested in sponsoring Navigation or any work around it, please reach out to the current project lead. - -Our current leadership team includes: - -+----------------+-------------------+-----------------+--------------+ -| Name | Organization | GitHub ID | Current Role | -+================+===================+=================+==============+ -| Steve Macenski | Samsung Research | SteveMacenski_ | Project Lead | -+----------------+-------------------+-----------------+--------------+ -| Carl Delsey | Intel Corporation | crdelsey_ | Maintainer | -+----------------+-------------------+-----------------+--------------+ -| Ruffin White | UC San Diego | ruffsl_ | CI Wizard | -+----------------+-------------------+-----------------+--------------+ - -.. _SteveMacenski: https://github.com/SteveMacenski -.. _crdelsey: https://github.com/crdelsey -.. _ruffsl: https://github.com/ruffsl - -Our former leadership team includes: - -+----------------+-------------------+-----------------+---------------------+ -| Name | Organization | GitHub ID | Role | -+================+===================+=================+=====================+ -| Matt Hansen | Intel Corporation | mkhansen_ | Former Project Lead | -+----------------+-------------------+-----------------+---------------------+ -| Brian Wilcox | Intel Corporation | bpwilcox_ | Former Maintainer | -+----------------+-------------------+-----------------+---------------------+ - -.. _mkhansen: https://github.com/mkhansen-intel -.. _bpwilcox: https://github.com/bpwilcox - -Contact -******* - -If you are interested in contacting someone about Navigation, ROS2, or related projects, please email the project leader. -We intentionally make our emails easy to find. -If your inquiry relates to bugs or open-source feature requests, consider posting a ticket on our GitHub project. -If your inquiry relates to configuration support or private feature development, reach out and we may be able to connect you with -independent consultants or contractors that know this project well. diff --git a/sphinx_doc/about/robots.rst b/sphinx_doc/about/robots.rst deleted file mode 100644 index 69367e6f84..0000000000 --- a/sphinx_doc/about/robots.rst +++ /dev/null @@ -1,60 +0,0 @@ -.. _robots: - -Robots Using -############ - -It's always helpful (and fun!) to have a list of robots using or ship with our work. -Below is a very early list of robots we have encountered using our software as examples. - -Click on the images below for a link to the drivers or navigation configurations. - -+------------------------+------------------------+------------------------+ -| |tb2| + |tb3| + |rover| | -+------------------------+------------------------+------------------------+ -| |yunji| + |RB1| + |Tiago| | -+------------------------+------------------------+------------------------+ -| |soon| + |soon| + |soon| | -+------------------------+------------------------+------------------------+ - -.. |tb2| image:: images/tb2.png - :width: 100% - :align: middle - :alt: Turtlebot2 - :target: https://github.com/kobuki-base/kobuki_ros - -.. |tb3| image:: images/tb3.png - :width: 100% - :align: middle - :alt: Turtlebot3 - :target: https://github.com/ROBOTIS-GIT/turtlebot3 - -.. |rover| image:: images/rover.png - :width: 100% - :align: middle - :alt: Rover Robotics - :target: https://github.com/RoverRobotics/openrover-ros2 - -.. |yunji| image:: images/yunji.png - :width: 100% - :align: middle - :alt: Yunji Robot - :target: https://en.yunjichina.com.cn/a/53.html - -.. |RB1| image:: images/rb1.png - :width: 100% - :align: middle - :alt: Robotnik Rb1 base - :target: https://github.com/IntelligentRoboticsLabs/marathon_ros2 - -.. |Tiago| image:: images/tiago.png - :align: middle - :height: 350px - :alt: Tiago Robot - :target: https://github.com/IntelligentRoboticsLabs/marathon_ros2 - - -.. |soon| image:: images/coming_soon.jpeg - :width: 100% - :align: middle - :alt: Coming Soon - :target: https://www.youtube.com/watch?v=oHg5SJYRHA0 diff --git a/sphinx_doc/about/ros1_comparison.rst b/sphinx_doc/about/ros1_comparison.rst deleted file mode 100644 index 363caec89c..0000000000 --- a/sphinx_doc/about/ros1_comparison.rst +++ /dev/null @@ -1,54 +0,0 @@ -.. _ros1_comparison: - -ROS to ROS2 Navigation -###################### - -``move_base`` has been split into multiple components. -Rather than a single monolithic state machine, navigation 2 makes use of action servers and ROS2's -low-latency, reliable communication to separate ideas. A behavior tree is used to orchestrate these tasks. -This allows Navigation2 to have highly configurable navigation behavior without programming by rearranging tasks -in a behavior tree xml file. - -The *nav2_bt_navigator* replaces ``move_base`` at the top level, with an *Action* interface to complete a navigation task with a tree-based action model. -It uses *Behavior Trees* to make it possible to have more complex state machines and to add in recovery behaviors as additional *Action Servers*. -These behavior trees are configurable XMLs and we provide several starting examples. - -The planning, recovery, and controller servers are also action servers that the BT navigator can call to compute. -All 3 servers can host many plugins of many algorithms each and individually called from the navigation behavior tree for specific behaviors. -The default plugins provided are ported from ROS1, namely: DWB, NavFn, and similar recoveries such as spinning and clearing costmaps. -A new recovery for waiting a fixed duration was also added. -These servers are called from the BT navigator through their action servers to compute a result or complete a task. -The state is maintained by the BT navigator behavior tree. - -All these changes make it possible to replace any of these nodes at launch/run time with any other algorithm that implements that same interface. -See each package README.md for more details. - -.. image:: images/move_base_compare_2.png - :align: center - :width: 700px - :alt: Move Base -> Navigation2 Overview - -Note: ``nav2_simple_navigator`` no longer exists, it has been replaced by ``nav2_bt_navigator``. - -**In Summary:** - -Ported packages: - - * amcl: Ported to nav2_amcl - * map_server: Ported to nav2_map_server - * nav2_planner: Replaces global_planner, hosts ``N`` planner plugins - * nav2_controller: Replaces local_planner, hosts ``N`` controller plugins - * Navfn: Ported to nav2_navfn_planner - * DWB: Replaces DWA and ported to ROS2 under nav2_dwb_controller metapackage - * nav_core: Ported as nav2_core with updates to interfaces - * costmap_2d: Ported as nav2_costmap_2d - -New packages: - - * nav2_bt_navigator: replaces ``move_base`` state machine - * nav2_lifecycle_manager: Handles the server program lifecycles - * nav2_waypoint_follower: Can take in many waypoints to execute a complex task through - * nav2_system_tests: A set of integration tests for CI and basic tutorials in simulation - * nav2_rviz_plugins: An rviz plugin to control the Navigation2 servers, command, cancel, and navigation with - * nav2_experimental: Experimental (and incomplete) work for deep reinforement learning controllers - * navigation2_behavior_trees: wrappers for the behavior tree library to call ROS action servers diff --git a/sphinx_doc/build_instructions/build_docs/build_troubleshooting_guide.rst b/sphinx_doc/build_instructions/build_docs/build_troubleshooting_guide.rst deleted file mode 100644 index 055b76d5a5..0000000000 --- a/sphinx_doc/build_instructions/build_docs/build_troubleshooting_guide.rst +++ /dev/null @@ -1,31 +0,0 @@ -.. _build-troubleshooting-guide: - -Build Troubleshooting Guide -********************************************** - -Common Navigation2 Dependencies Build Failures -============================================== - -* Make sure that .bashrc file has no ROS environment variables in it. Open new terminals and try to build the packages again. - -* Make sure to run rosdep for the correct ROS 2 distribution. - ``rosdep install -y -r -q --from-paths src --ignore-src --rosdistro `` - -* Make sure that the ``setup.bash`` is sourced in the ROS 2 installation or ROS 2 master build workspace, if applicable. Check if you can run talker and listener nodes. - -* Make sure that the ``setup.bash`` in ``nav2_depend_ws/install`` is sourced. - -* Check if you have the correct ROS version and distribution. ``printenv | grep -i ROS`` - -* If you see a bunch of errors on startup about ``map`` or ``odom`` frame not existing, remember to activate drivers (or gazebo for simulation) and set an initial pose in ``map`` frame. Costmap2D will block activation until a full TF tree is available. - -* Make sure you've activated the lifecycle nodes if you're not seeing transforms or servers running. - -* Search `GitHub Issues `_ - -Still can't solve it? Let us know about your issue. `Open a ticket `_. - -Reporting Issue -=============== - -- If you run into any issues when building Navigation2, you can use the search tool in the issues tab on `GitHub `_ and always feel free to `open a ticket `_. diff --git a/sphinx_doc/build_instructions/index.rst b/sphinx_doc/build_instructions/index.rst deleted file mode 100644 index be273232a3..0000000000 --- a/sphinx_doc/build_instructions/index.rst +++ /dev/null @@ -1,198 +0,0 @@ -.. _build-instructions: - -Build and Install -################# - -Install -******* - -Navigation2 and its dependencies are released as binaries. -You may install it via the following to get the latest stable released version: - - ``sudo apt install ros--navigation2 ros--nav2-bringup ros--turtlebot3*`` - -Build -***** - -There are 3 ways to build Navigation2. -Building for a specific released distribution (e.g. ``eloquent``, ``foxy``), build Navigation2 on master branch using a quickstart setup script, or building master branch manually. - -.. rst-class:: content-collapse - -Build Navigation2 For Released Distribution -=========================================== - -Install ROS ------------ - -Please install ROS2 via the usual `build instructions `_ for your desired distribution. - -Build Navigation2 ------------------ - -We're going to create a new workspace, ``navigation2_ws``, clone our Navigation2 branch into it, and build. -``rosdep`` will be used to get the dependency binaries for navigation2 in your specific distribution. - -.. code:: bash - - mkdir -p ~/navigation2_ws/src - cd ~/navigation2_ws/src - git clone https://github.com/ros-planning/navigation2.git --branch eloquent-devel - cd ~/navigation2_ws - rosdep install -y -r -q --from-paths src --ignore-src --rosdistro eloquent - colcon build --symlink-install - -Note: You need to change ``--rosdistro`` to the selected ROS 2 distribution name (e.g ``eloquent``). - - - -.. rst-class:: content-collapse - -Quickstart Build Master -======================= - -Steps ------ - -Install all ROS 2 dependencies from the `ROS2 Installation page `_. -Ensure there are no ROS environment variables set in your terminal or `.bashrc` file before taking the steps below.* - - -.. code:: bash - - mkdir - cd - wget https://raw.githubusercontent.com/ros-planning/navigation2/master/tools/initial_ros_setup.sh - chmod a+x initial_ros_setup.sh - ./initial_ros_setup.sh - - -**Summary of what's being done** - -The ``initial_ros_setup.sh`` script downloads three ROS workspaces and then builds them in the correct order. The three workspaces are: - -- **ROS 2 release**: This is the latest ROS 2 release as defined by the repos file found `here `_ -- **ROS 2 dependencies**: This is a set of ROS 2 packages that aren't included in the ROS 2 release yet. However, you need them to be able to build Navigation2. This also includes packages that are part of the ROS 2 release where Navigation2 uses a different version. -- **Navigation2**: This repository. - -After all the workspaces are downloaded, run the `navigation2/tools/build_all.sh` script. `build_all.sh` builds each repo in the order listed above using the `colcon build --symlink-install` command. - -Options -------- - -The `initial_ros_setup.sh` accepts the following options: - -- `--no-ros2` This skips downloading and building the ROS 2 release. Instead it uses the binary packages and ``setup.sh`` installed in ``/opt/ros/`` -- ``--download-only`` This skips the build steps - - -.. rst-class:: content-collapse - -Manually Build Master -===================== - -Build ROS 2 Master ------------------- - -.. warning:: - - When building ROS 2 from source, make sure that the `ros2.repos` file is from the `master` branch. - -Build ROS2 master using the `build instructions `_ provided in the ROS2 documentation. - - -Build Navigation2 Dependencies ------------------------------- - -Since we're not building for a released distribution, we must build the dependencies ourselves rather than using binaries. -First, source the setup.bash file in the ROS 2 build workspace. - - ``source ~/ros2_ws/install/setup.bash`` - -Next, we're going to get the ``ros2_dependencies.repos`` file from Navigation2. -Then, use ``vcs`` to clone the repos and versions in it into a workspace. - -.. code:: bash - - source ros2_ws/install/setup.bash - mkdir -p ~/nav2_depend_ws/src - cd ~/nav2_depend_ws - wget https://raw.githubusercontent.com/ros-planning/navigation2/master/tools/ros2_dependencies.repos - vcs import src < ros2_dependencies.repos - colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release - -Build Navigation2 Master ------------------------- - -Finally, now that we have ROS2 master and the necessary dependencies, we can now build Navigation2 master itself. -We'll source the ``nav2_depend_ws``, which will also source the ROS2 master build workspace packages, to build with dependencies. -The rest of this should look familiar. - -.. code:: bash - - source ~/nav2_depend_ws/install/setup.bash - mkdir -p ~/navigation2_ws/src - cd ~/navigation2_ws/src - git clone https://github.com/ros-planning/navigation2.git --branch master - cd ~/navigation2_ws - colcon build --symlink-install - -Docker -****** - - -There are 2 options for docker with Navigation2: -building a container and using the DockerHub container. - -.. rst-class:: content-collapse - -Building Docker Container -========================= - -To build an image from the Dockerfile in the navigation2 folder: -First, clone the repo to your local system (or see Building the source above) - -.. code:: bash - - sudo docker build -t nav2/latest . - -If proxies are needed: - -.. code:: bash - - sudo docker build -t nav2/latest --build-arg http_proxy=http://proxy.my.com:### --build-arg https_proxy=http://proxy.my.com:### . - -Note: You may also need to configure your docker for DNS to work. See article here for details: https://development.robinwinslow.uk/2016/06/23/fix-docker-networking-dns/ - -.. rst-class:: content-collapse - -Using DockerHub Container -========================= - -We allow for you to pull the latest docker image from the master branch at any time. As new releases and tags are made, docker containers on docker hub will be versioned as well to chose from. - -.. code:: bash - - sudo docker pull rosplanning/navigation2:latest - -!!!! - -Generate Doxygen -**************** - -Run ``doxygen`` in the root of the Navigation2 repository. -It will generate a ``/doc/*`` directory containing the documentation. -The documentation entrypoint in a browser is index.html. - -!!!! - -Help -**** - -:ref:`build-troubleshooting-guide` - - -.. toctree:: - :hidden: - - build_docs/build_troubleshooting_guide.rst diff --git a/sphinx_doc/concepts/index.rst b/sphinx_doc/concepts/index.rst deleted file mode 100644 index d132c0d4be..0000000000 --- a/sphinx_doc/concepts/index.rst +++ /dev/null @@ -1,258 +0,0 @@ -.. _concepts: - -Navigation Concepts -################### - -This page is to help familiarize new robotists to the concepts of mobile robot navigation, in particular, with the concepts required to appreciating and working with this project. - -ROS2 -**** - -ROS2 is the core middleware used for Navigation2. If you are unfamilar with this, please visit `the ROS2 documentation `_ before continuing. - -Action Server -============= - -Just as in ROS, action servers are a common way to control long running tasks like navigation. -This stack makes more extensive use of actions, and in some cases, without an easy topic interface. -It is more important to understand action servers as a developer in ROS2. -Some simple CLI examples can be found in the `ROS2 documementation `_. - -Action servers are similar to a canonical service server. -A client will request some task to be completed, except, this task may take a long time. -An example would be moving the shovel up from a bulldozer or ask a robot to travel 10 meters to the right. - -In this situation, action servers and clients allow us to call a long-running task in another process or thread and return a future to its result. -It is permissible at this point to block until the action is complete, however, you may want to occasionally check if the action is complete and continue to process work in the client thread. -Since it is long-running, action servers will also provide feedback to their clients. -This feedback can be anything and is defined in the ROS ``.action`` along with the request and result types. -In the bulldozer example, a request may be an angle, a feedback may be the angle remaining to be moved, and the result is a success or fail boolean with the end angle. -In the navigation example, a request may be a position, a feedback may be the time its been navigating for and the distance to the goal, and the result a boolean for success. - -Feedback and results can be gathered synchronously by registering callbacks with the action client. -They may also be gathered by asychronously requesting information from the shared future objects. -Both require spinning the client node to process callback groups. - -Action servers are used in this stack to communicate with the highest level BT navigator through a ``NavigateToPose`` action message. -They are also used for the BT navigator to communicate with the subsequent smaller action servers to compute plans, control efforts, and recoveries. -Each will have their own unique ``.action`` type in ``nav2_msgs`` for interacting with the servers. - -Lifecycle Nodes -=============== - -Lifecycle (or Managed, more correctly) nodes are unique to ROS2. -More information can be `found here `_. -They are nodes that contain state machine transitions for bringup and teardown of ROS2 servers. -This helps in determinstic behavior of ROS systems in startup and shutdown. -It also helps users structure their programs in reasonable ways for commercial uses and debugging. - -When a node is started, it is in the unconfigured state, only processing the node's constructor which should **not** contain any ROS networking setup or parameter reading. -By the launch system, or the supplied lifecycle manager, the nodes need to be transitioned to inactive by configuring. -After, it is possible to activate the node by transitioning through the activing stage. - -This state will allow the node to process information and be fully setup to run. -The configuration stage, triggering the ``on_configure()`` method, will setup all parameters, ROS networking interfaces, and for safety systems, all dynamically allocated memory. -The activation stage, triggering the ``on_activate()`` method, will active the ROS networking interfaces and set any states in the program to start processing information. - -To shutdown, we transition into deactivating, cleaning up, shutting down and end in the finalized state. -The networking interfaces are deactivated and stop processing, deallocate memory, exit cleanly, in those stages, respectively. - -The lifecycle node framework is used extensively through out this project and all servers utilize it. -It is best convention for all ROS systems to use lifecycle nodes if it is possible. - -Behavior Trees -************** - -Behavior trees (BT) are becoming increasingly common in complex robotics tasks. -They are a tree structure of tasks to be completed. -It creates a more scalable and human-understandable framework for defining multi-step or many state applications. -This is opposed to a finite state machine (FSM) which may then have dozens or states and hundreds of transitions. -An example would be a soccer playing robot. -Embedding the logic of soccer game play into a FSM would be challenging and error prone with many possible states and rules. -Additionally, modeling choices like to shoot at the goal from the left, right, or center, is particularly unclear. -With a BT, basic primitives like "kick" "walk" "go to ball" can be created and reused for many behaviors. -More information can be found `in this book `_. -I **strongly** recommend reading chapters 1-3 to get a good understanding of the nomenclature and workflow. -It should only take about 30 minutes. - -For this project, we use `BehaviorTree CPP V3 `_ as the behavior tree library. -We create node plugins which can be constructed into a tree, inside the ``BT Navigato``. -The node plugins are loaded into the BT and when the XML file of the tree is parsed, the registered names are associated. -At this point, we can march through the behavior tree to navigate. - -One reason this library is used is its ability to load subtrees. This means that the Navigation2 behavior tree can be loaded into another higher-level BT to use this project as node plugin. -An example would be in soccer play, using the Navigation2 behavior tree as the "go to ball" node with a ball detection as part of a larger task. -Additionally, we supply a ``NavigateToPoseAction`` plugin for BT so the Navigation2 stack can be called from a client application through the usual action interface. - - -Navigation Servers -****************** - -Planners and controllers are at the heart of a navigation task. -Recoveries are used to get the robot out of a bad situation or attempt to deal with various forms of issues to make the system fault-tolerant. -In this section, the general concepts around them and their uses in this project are analyzed. - -Planner, Controller, and Recovery Servers -========================================= - -Three of the action servers in this project are the planner, recovery, and controller servers. -These action servers are used to host a map of algorithm plugins to complete various tasks. -They also host the environmental representation used by the algorithm plugins to compute their outputs. - -The planner and controller servers will be configured at runtime with the names (aliases) and types of algorithms to use. -These types are the pluginlib names that have been registered and the names are the aliases for the task. -An example would be the DWB controller used with name ``FollowPath``, as it follows a reference path. -In this case, then all parameters for DWB would be placed in that namespace, e.g. ``FollowPath.``. - -These two servers then expose an action interface corresponding to its task. -When the behavior tree ticks the corresponding BT node, it will call the action server to process its task. -The action server callback inside the server will call the chosen algorithm by its name (e.g. ``FollowPath``) that maps to a specific algorithm. -This allows a user to abstract the algorithm used in the behavior tree to classes of algorithms. -For instance, you can have ``N`` plugin controllers to follow paths, dock with charger, avoid dynamic obstacles, or interface with a tool. -Having all of these plugins in the same server allows the user to make use of a single environmental representation object, which is costly to duplicate. - -For the recovery server, each of the recoveries also contains their own name, however, each plugin will also expose its own special action server. -This is done because of the wide variety of recovery actions that may be created cannot have a single simple interface to share. -The recovery server also contains a costmap subscriber to the local costmap, receiving real-time updates from the controller server, to compute its tasks. -We do this to avoid having multiple instances of the local costmap which are computationally expensive to duplicate. - -Alternatively, since the BT nodes are trivial plugins calling an action, new BT nodes can be created to call other action servers with other action types. -It is advisable to use the provided servers if possible at all times. -If, due to the plugin or action interfaces, a new server is needed, that can be sustained with the framework. -The new server should use the new type and plugin interface, similar to the provided servers. -A new BT node plugin will need to be created to call the new action server -- however no forking or modification is required in the navigation2 repo itself by making extensive use of servers and plugins. - -If you find that you require a new interface to the pluginlib definition or action type, please file a ticket and see if we can rectify that in the same interfaces. - -Planners -======== - -The task of a planner is to compute a path to complete some objective function. -The path can also be known as a route, depending on the nomenclature and algorithm selected. -Two canonical examples are computing a plan to a goal (e.g. from current position to a goal) or complete coverage (e.g. plan to cover all free space). -The planner will have access to a global environmental representation and sensor data buffered into it. -Planners can be written to: - -- Compute shortest path -- Compute complete coverage path -- Compute paths along sparse or predefined routes - -The general task in Navigation2 for the planner is to compute a valid, and potentially optimal, path from the current pose to a goal pose. -However, many classes of plans and routes exist which are supported. - -Controllers -=========== - -Controllers, also known as local planners in ROS1, are the way we follow the globally computed path or complete a local task. -The controller will have access to a local environment representation to attempt to compute feasible control efforts for the base to follow. -Many controller will project the robot forward in space and compute a locally feasible path at each update iteration. -Controllers can be written to: - -- Follow a path -- Dock with a charging station using detectors in the odometric frame -- Board an elevator -- Interface with a tool - -The general task in Navigation2 for a controller is to compute a valid control effort to follow the global plan. -However, many classes of controllers and local planners exist. -It is the goal of this project that all controller algorithms can be plugins in this server for common research and industrial tasks. - -Recoveries -========== - -Recoveries are a mainstay of fault-tolerant systems. -The goal of recoveries are to deal with unknown or failure conditions of the system and autonomously handle them. -Examples may include faults in the perception system resulting in the environmental representation being full of fake obstacles. -The clear costmap recovery would then be triggered to allow the robot to move. - -Another example would be if the robot was stuck due to dynamic obstacles or poor control. -Backing up or spinning in place, if permissible, allow the robot to move from a poor location into free space it may navigate successfully. - -Finally, in the case of a total failure, a recovery may be implemented to call an operators attention for help. -This can be done with email, SMS, Slack, Matrix, etc. - - -State Estimation -**************** - -Within the navigation project, there are 2 major transformations that need to be provided, according to community standards. -The ``map`` to ``odom`` transform is provided by a positioning system (localization, mapping, SLAM) and ``odom`` to ``base_link`` by an odometry system. - -.. note:: - - There is **no** requirement on using a LIDAR on your robot to use the navigation system. There is no requirement to use lidar-based collision avoidance, - localization, or slam. However, we do provide instructions and support tried and true implementations of these things using lidars. - You can be equally as successful using a vision or depth based positioning system and using other sensors for collision avoidance. - The only requirement is that you follow the standards below with your choice of implementation. - -Standards -========= - -`REP 105 `_ defines the frames and conventions required for navigation and the larger ROS ecosystem. -These conventions should be followed at all times to make use of the rich positioning, odometry, and slam projects available in the community. - -In a nutshell, REP-105 says that you must, at minimum, build a TF tree that contains a full ``map`` -> ``odom`` -> ``base_link`` -> ``[sensor frames]`` for your robot. -TF2 are the time-variant transformation library in ROS2 we use to represent and obtain time synchronized transformations. -It is the job of the global positioning system (GPS, SLAM, Motion Capture) to, at minimum, provide the ``map`` -> ``odom`` transformation. -It is then the role of the odometry system to provide the ``odom`` -> ``base_link`` transformation. -The remainder of the transformations relative to ``base_link`` should be static and defined in your `URDF `_. - -Global Positioning: Localization and SLAM -========================================= - -It is the job of the global positioning system (GPS, SLAM, Motion Capture) to, at minimum, provide the ``map`` -> ``odom`` transformation. -We provide ``amcl`` which is an Adaptive Monte-Carlo Localization technique based on a particle filter for localization of a static map. -We also provide SLAM Toolbox as the default SLAM algorithm for use to position and generate a static map. - -These methods may also produce other output including position topics, maps, or other metadata, but they must provide that transformation to be valid. -Multiple positioning methods can be fused together using robot localization, discussed more below. - - -Odometry -======== - -It is the role of the odometry system to provide the ``odom`` -> ``base_link`` transformation. -Odometry can come from many sources including LIDAR, RADAR, wheel encoders, VIO, and IMUs. -The goal of the odometry is to provide a smooth and continuous local frame based on robot motion. -The global positioning system will update the transformation relative to the global frame to account for the odometric drift. - -`Robot Localization `_ is typically used for this fusion. -It will take in ``N`` sensors of various types and provide a continuous and smooth odometry to TF and to a topic. -A typical mobile robotics setup may have odometry from wheel encoders, IMUs, and vision fused in this manor. - -The smooth output can be used then for dead-reckoning for precise motion and updating the position of the robot accurately between global position updates. - - - -Environmental Representation -**************************** - -The environmental representation is the way the robot perceives its environment. -It also acts as the central localization for various algorithms and data sources to combine their information into a single space. -This space is then used by the controllers, planners, and recoveries to compute their tasks safely and efficiently. - -Costmaps and Layers -=================== - -The current environmental representation is a costmap. -A costmap is a regular 2D grid of cells containing a cost from unknown, free, occupied, or inflated cost. -This costmap is then searched to compute a global plan or sampled to compute local control efforts. - -Various costmap layers are implemented as pluginlib plugins to buffer information into the costmap. -This includes information from LIDAR, RADAR, sonar, depth, images, etc. -It may be wise to process sensor data before inputting it into the costmap layer, but that is up to the developer. - -Costmap layers can be created to detect and track obstacles in the scene for collision avoidance using camera or depth sensors. -Additionally, layers can be created to algorithmically change the underlying costmap based on some rule or heuristic. -Finally, they may be used to buffer live data into the 2D or 3D world for binary obstacle marking. - -Other Forms -=========== - -Various other forms of environmental representations exist. -These include: - -- gradient maps, which are similar to costmaps but represent surface gradients to check traversibility over -- 3D costmaps, which represent the space in 3D, but then also requires 3D planning and collision checking -- Mesh maps, which are similar to gradient maps but with surface meshes at many angles -- "Vector space", taking in sensor information and using machine learning to detect individual items and locations to track rather than buffering discrete points. diff --git a/sphinx_doc/conf.py b/sphinx_doc/conf.py deleted file mode 100644 index d907dc8615..0000000000 --- a/sphinx_doc/conf.py +++ /dev/null @@ -1,193 +0,0 @@ -# -*- coding: utf-8 -*- -# -# Project SOF documentation build configuration file, created by -# sphinx-quickstart on Wed Jan 10 20:51:29 2018. -# -# This file is execfile()d with the current directory set to its -# containing dir. -# -# Note that not all possible configuration values are present in this -# autogenerated file. -# -# All configuration values have a default; values that are commented out -# serve to show the default. - -# If extensions (or modules to document with autodoc) are in another directory, -# add these directories to sys.path here. If the directory is relative to the -# documentation root, use os.path.abspath to make it absolute, like shown here. -# -import os -import sys -sys.path.insert(0, os.path.abspath('.')) - - -# -- General configuration ------------------------------------------------ - -# If your documentation needs a minimal Sphinx version, state it here. -# -# needs_sphinx = '1.0' - -# Add any Sphinx extension module names here, as strings. They can be -# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom -# ones. -extensions = ['breathe', 'sphinx.ext.graphviz', 'sphinxcontrib.plantuml', 'sphinx.ext.extlinks'] - -graphviz_output_format='png' -graphviz_dot_args=[ - '-Nfontname="verdana"', - '-Gfontname="verdana"', - '-Efontname="verdana"'] - -plantuml = 'java -jar ' + os.path.join(os.path.abspath('.'), 'scripts/plantuml.jar') -plantuml_output_format = 'png' - -# Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] - -# The suffix(es) of source filenames. -# You can specify multiple suffix as a list of string: -# -# source_suffix = ['.rst', '.md'] -source_suffix = '.rst' - -# The master toctree document. -master_doc = 'index' - -# General information about the project. -project = u'Navigation 2' -copyright = u'2020' -author = u'Various' - -# The version info for the project you're documenting, acts as replacement for -# |version| and |release|, also used in various other places throughout the -# built documents. - -version = release = "1.0.0" - -# -# The short X.Y version. -# version = u'0.1' -# The full version, including alpha/beta/rc tags. -# release = u'0.1' - -# The language for content autogenerated by Sphinx. Refer to documentation -# for a list of supported languages. -# -# This is also used if you do content translation via gettext catalogs. -# Usually you set "language" from the command line for these cases. -language = None - -# List of patterns, relative to source directory, that match files and -# directories to ignore when looking for source files. -# This patterns also effect to html_static_path and html_extra_path -exclude_patterns = ['_build','_themes','scripts' ] - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = 'sphinx' - -# If true, `todo` and `todoList` produce output, else they produce nothing. -todo_include_todos = False - -# -- Options for HTML output ---------------------------------------------- - -# The theme to use for HTML and HTML Help pages. See the documentation for -# a list of builtin themes. -# -try: - import sphinx_rtd_theme -except ImportError: - html_theme = 'alabaster' - # This is required for the alabaster theme - # refs: http://alabaster.readthedocs.io/en/latest/installation.html#sidebars - html_sidebars = { - '**': [ - 'relations.html', # needs 'show_related': True theme option to display - 'searchbox.html', - ] - } - sys.stderr.write('Warning: sphinx_rtd_theme missing. Use pip to install it.\n') -else: - html_theme = "sphinx_rtd_theme" - html_theme_path = [sphinx_rtd_theme.get_html_theme_path()] - html_theme_options = { - 'canonical_url': '', - 'analytics_id': '', - 'logo_only': False, - 'display_version': True, - 'prev_next_buttons_location': 'None', - # Toc options - 'collapse_navigation': False, - 'sticky_navigation': True, - 'navigation_depth': 4, - } - -html_theme_path = ['_themes'] -html_theme = 'otc_tcs_sphinx_theme' - -# Here's where we (manually) list the document versions maintained on -# the published doc website. On a daily basis we publish to the -# /latest folder but when releases are made, we publish to a / -# folder (specified via RELEASE=name on the make command). - -if tags.has('release'): - current_version = version -else: - version = current_version = "latest" - -html_context = { - 'current_version': current_version, - 'versions': ( ("latest", "/latest/"), -# ("0.1-rc4", "/0.1-rc4/"), - ) - } - - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -# -# html_theme_options = {} - -html_logo = 'images/logo_white_200w.png' -html_favicon = 'images/favicon-48x48.png' - -numfig = True -#numfig_secnum_depth = (2) -numfig_format = {'figure': 'Figure %s', 'table': 'Table %s', 'code-block': 'Code Block %s'} - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -#html_static_path = ['static'] - -# Custom sidebar templates, must be a dictionary that maps document names -# to template names. -# - -# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. -html_show_sphinx = False - -# If true, links to the reST sources are added to the pages. -html_show_sourcelink = False - -# If not '', a 'Last updated on:' timestamp is inserted at every page -# bottom, -# using the given strftime format (ei %b %d, %Y). -html_last_updated_fmt = None - -# -- Options for HTMLHelp output ------------------------------------------ - - -rst_epilog = """ -.. include:: /substitutions.txt -""" - - -breathe_projects = { - "SOF Project" : "doxygen/xml", -} -breathe_default_project = "SOF Project" -breathe_default_members = ('members', 'undoc-members', 'content-only') - -extlinks = {'projectfile': - ('https://github.com/ros-planning/navigation2/blob/master/%s', 'filepath ')} diff --git a/sphinx_doc/configuration/configuring-the-controller.rst b/sphinx_doc/configuration/configuring-the-controller.rst deleted file mode 100644 index 3f90309c1b..0000000000 --- a/sphinx_doc/configuration/configuring-the-controller.rst +++ /dev/null @@ -1,388 +0,0 @@ -.. _configuring_the_controller: - -Configuring The Controller -########################## - -Controller Server -================= - -The controller server responds to FollowPath action requests. It runs a loop -that continuously computes new command velocities until the goal is reached, -cancelled or an error occurs. It computes command velocities by using the -services of a controller plugin. The parameters in this section are set on the -controller_server node - -Parameters -********** - -:controller_frequency: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | 20.0 | - +----------------+----------------------------------------------------------------------------+ - |**units** | Hz | - +----------------+----------------------------------------------------------------------------+ - |**description** | This parameter controls how often a new command velocity is | - | | published. It also controls how often the controller checks if the goal is| - | | reached. Faster robots may need to increase this value. Slower robots can | - | | use a lower value. | - +----------------+----------------------------------------------------------------------------+ - -:controller_plugin_ids: - +----------------+----------------------------------------------------------------------------+ - |**type** | vector | - +----------------+----------------------------------------------------------------------------+ - |**default** | "FollowPath" | - +----------------+----------------------------------------------------------------------------+ - |**description** | The name of each controller plugin to be loaded into the controller server.| - | | Multiple controller plugins can be loaded simultaneously so they can share | - | | a costmap instance. The name assigned to the plugin using this parameter | - | | determines how it should be addressed in the *controller_id* field of the | - | | FollowPath action request. | - +----------------+----------------------------------------------------------------------------+ - - -:controller_plugin_types: - +----------------+----------------------------------------------------------------------------+ - |**type** | vector | - +----------------+----------------------------------------------------------------------------+ - |**default** | "dwb_core::DWBLocalPlanner" | - +----------------+----------------------------------------------------------------------------+ - |**description** | The data type of each controller plugin to be loaded into the controller | - | | server. This is used by pluginlib to dyanmically load the object. This | - | | needs to be a class derived from nav2_core::LocalPlanner. The default | - | | controller is an instance of DWB whose parameters are here | - | | `DWB Controller`_ | - +----------------+----------------------------------------------------------------------------+ - -Progress Checker -================= - -This is a component of the controller server that verifies that the robot has moved sufficiently. -If the robot fails to make progress, the controller fails and triggers recoveries in the -behavior tree. The parameters in this section are set on the controller_server node. - -Parameters -********** - -:required_movement_radius: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | 0.5 | - +----------------+----------------------------------------------------------------------------+ - |**units** | m | - +----------------+----------------------------------------------------------------------------+ - |**description** | The absolute distance the robot must move from the previous checkpoint. If | - | | the robot doesn't move this far in *movement_time_allowance* seconds, an | - | | error is thrown. Once the robot moves this distance the time is reset | - +----------------+----------------------------------------------------------------------------+ - -:movement_time_allowance: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | 10.0 | - +----------------+----------------------------------------------------------------------------+ - |**units** | s | - +----------------+----------------------------------------------------------------------------+ - |**description** | Controls how much time the robot has time to show progress. If the robot | - | | fails to move more than *required_movement_radius* meters in this time, an | - | | error is thrown. For slow robots, this time should be extended. Reducing | - | | this time will cause the robot to recover more quickly if it is stuck, | - | | but too little time doesn't give the robot enough time to manoeuver in | - | | dyanamic enviroments | - +----------------+----------------------------------------------------------------------------+ - -DWB Controller -============== - -The DWB controller is the default controller in |PN|. It is a fork of `David Lu's -controller `_ -modified for ROS 2. - -.. warning:: - - The parameters below are not properly scoped. The exact parameter names are - likely to change in the near future - -Parameters -********** - -:trajectory_generator_name: - +----------------+----------------------------------------------------------------------------+ - |**type** | string | - +----------------+----------------------------------------------------------------------------+ - |**default** | "dwb_plugins::StandardTrajectoryGenerator" | - +----------------+----------------------------------------------------------------------------+ - |**description** | This is the name of the pluginlib class to use as the trajectory generator.| - | | The trajectory generator projects a sample of possible trajectories the | - | | robot can follow from its current pose and velocity. These trajectories | - | | are then provided to the critics to score. | - | | | - | | You can create a custom trajectory generator by inheriting from the | - | | dwb_core::TrajectoryGenerator base class | - | | | - | | The trajectory generators provided with |PN| are: | - | | | - | | * dwb_plugins::StandardTrajectoryGenerator | - | | * dwb_plugins::LimitedAccelGenerator | - +----------------+----------------------------------------------------------------------------+ - -:critics: - +----------------+----------------------------------------------------------------------------+ - |**type** | vector | - +----------------+----------------------------------------------------------------------------+ - |**default** | ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", | - | | "PathAlign", "PathDist", "GoalDist"] | - +----------------+----------------------------------------------------------------------------+ - |**description** | This is a list of critic plugins that will evaluate the trajectories | - | | generated by the trajectory generator. Each critic evaluates the | - | | trajectories according to its own criteria. The scores from each critic | - | | will be combined, and the trajectory with the best score will be chosen. By| - | | weighting the different critics appropriately, the quality of the chosen | - | | trajectory can be modified. | - | | | - | | You can create a custom critic by inheriting from the | - | | dwb_core::TrajectoryCritic base class | - | | | - | | The critics provided with |PN| are: | - | | | - | | * dwb_critics::PreferForwardCritic | - | | * dwb_critics::GoalDistCritic | - | | * dwb_critics::PathAlignCritic | - | | * dwb_critics::GoalAlignCritic | - | | * dwb_critics::PathDistCritic | - | | * dwb_critics::OscillationCritic | - | | * dwb_critics::RotateToGoalCritic | - | | * dwb_critics::BaseObstacleCritic | - | | * dwb_critics::ObstacleFootprintCritic | - | | * dwb_critics::TwirlingCritic | - +----------------+----------------------------------------------------------------------------+ - -:goal_checker_name: - +----------------+----------------------------------------------------------------------------+ - |**type** | string | - +----------------+----------------------------------------------------------------------------+ - |**default** | "dwb_plugins::SimpleGoalChecker" | - +----------------+----------------------------------------------------------------------------+ - |**description** | This is the plugin that evaluates the current pose and velocity to | - | | determine if the goal has been reached | - | | | - | | You can create a custom goal checker by inheriting from the | - | | nav2_core::GoalChecker base class | - | | | - | | The critics provided with |PN| are: | - | | | - | | * dwb_plugins::SimpleGoalChecker | - | | * dwb_plugins::StoppedGoalChecker | - +----------------+----------------------------------------------------------------------------+ - -.. rst-class:: content-collapse - -Additional Parameters -********************* - -:prune_plan: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true, the path evaluated by the controller is trimmed to only include | - | | points within *prune_distance* of the current robot pose. This improves CPU| - | | performance, and also has the effect of making the Goal* critics aim for a | - | | more nearby goal | - +----------------+----------------------------------------------------------------------------+ - -:prune_distance: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | 1.0 | - +----------------+----------------------------------------------------------------------------+ - |**units** | m | - +----------------+----------------------------------------------------------------------------+ - |**description** | If *prune_plan* is true, all points in the path that are further away from | - | | the robot than this distance are discarded. | - | | This is re-evaluated each time the controller is run, so the points are not| - | | permanently discared. | - +----------------+----------------------------------------------------------------------------+ - -:transform_tolerance: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | 0.1 | - +----------------+----------------------------------------------------------------------------+ - |**units** | s | - +----------------+----------------------------------------------------------------------------+ - |**description** | When determining the current robot pose, if the transform data is older | - | | than this, an error is returned | - +----------------+----------------------------------------------------------------------------+ - -Debug Parameters -================ - -:debug_trajectory_details: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | false | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true and if all trajectories are rejected by the critics, DWB will print| - | | statistics on the console indicating which critics rejected the | - | | trajectories and for what reason. | - +----------------+----------------------------------------------------------------------------+ - -:publish_cost_grid_pc: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | false | - +----------------+----------------------------------------------------------------------------+ - |**description** | Certain trajectory critics compute scores based on location in the costmap.| - | | For those critics, if this parameter is true, DWB will publish a PointCloud| - | | on the **cost_cloud** topic showing that critic's score for each cell. This| - | | can be visualized in RViz. | - | | | - | | The standard critics provided with |PN| that provide this capability are: | - | | | - | | * dwb_critics::GoalDistCritic | - | | * dwb_critics::PathAlignCritic | - | | * dwb_critics::GoalAlignCritic | - | | * dwb_critics::PathDistCritic | - | | * dwb_critics::BaseObstacleCritic | - | | * dwb_critics::ObstacleFootprintCritic | - +----------------+----------------------------------------------------------------------------+ - -:publish_evaluation: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true, publishes all evaluated trajectories and the scores each critic | - | | assigned them. This is the most detailed information on what is going on in| - | | DWB, but can be hard to visualize. The data is published on the | - | | **evaluation** topic | - +----------------+----------------------------------------------------------------------------+ - -:publish_global_plan: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true, publishes the path recieved by the controller. This is published | - | | on the **received_global_plan** topic and can be visualized in RViz | - +----------------+----------------------------------------------------------------------------+ - - -:publish_local_plan: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true, published the chosen trajectory on the **local_plan** topic. This | - | | can be visualized in RViz. | - +----------------+----------------------------------------------------------------------------+ - - -:publish_trajectories: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true, all evaluated trajectories are published on the **marker** topic. | - | | The trajectories can be visualized in RViz and are color coded. Black means| - | | a critic rejected that particular trajectory as invalid. The remaining | - | | trajectories are scaled from red to green, with the best trajectories being| - | | the most green and the worst being the most red. | - +----------------+----------------------------------------------------------------------------+ - - | | | - -:publish_transformed_plan: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true, publishes the received path after it has been transformed into the| - | | local frame of reference. This is published on the | - | | **transformed_global_plan** topic and can be visualized in RViz | - +----------------+----------------------------------------------------------------------------+ - -:short_circuit_trajectory_evaluation: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | This parameter allows process to move forward if trajectories score by | - | | critics is just equal to the best score. As after this point it would be | - | | pointless to increse the score as it will always be more than best score. | - +----------------+----------------------------------------------------------------------------+ - -:trans_stopped_velocity: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | 0.25 | - +----------------+----------------------------------------------------------------------------+ - |**description** | Minimal speed to check if the robot traj is if rotating near the goal. | - +----------------+----------------------------------------------------------------------------+ - -:slowing_factor: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | 5.0 | - +----------------+----------------------------------------------------------------------------+ - |**description** | If true, publishes the received path after it has been transformed into the| - | | local frame of reference. This is published on the | - | | **transformed_global_plan** topic and can be visualized in RViz | - +----------------+----------------------------------------------------------------------------+ - -:lookahead_time: - +----------------+----------------------------------------------------------------------------+ - |**type** | double | - +----------------+----------------------------------------------------------------------------+ - |**default** | -1.0 | - +----------------+----------------------------------------------------------------------------+ - |**description** | If the `lookahead_time` parameter is negative, the pose evaluated will be | - | | the last pose in the trajectory, which is the same as DWA's behavior. This | - | | is the default. Otherwise, a new pose will be projected using the | - | | dwb_local_planner::projectPose. By using a lookahead time shorter than | - | | sim_time, the critic will be less concerned about overshooting the goal yaw| - | | and thus will continue to turn faster for longer. | - +----------------+----------------------------------------------------------------------------+ - -:stateful: - +----------------+----------------------------------------------------------------------------+ - |**type** | bool | - +----------------+----------------------------------------------------------------------------+ - |**default** | true | - +----------------+----------------------------------------------------------------------------+ - |**description** | If the this parameter is set to true (which it is by default), once | - | | the desired cartesian difference is obtained, it will not check the | - | | cartesian difference again (until reset) and only check the yaw tolerance. | - | | This handles cases where the robot may accidentally leave the desired | - | | `xy_goal_tolerance` while rotating to the desired yaw. If `stateful` | - | | is true, this won't force the robot to try to move closer to the goal | - | | again, and instead just rotate to the goal. | - +----------------+----------------------------------------------------------------------------+ - -DWB Plugins -=========== - -.. toctree:: - :maxdepth: 1 - - dwb-plugins/trajectory-generators.rst - dwb-plugins/trajectory-critics.rst - dwb-plugins/goal-checkers.rst diff --git a/sphinx_doc/configuration/dwb-plugins/goal-checkers.rst b/sphinx_doc/configuration/dwb-plugins/goal-checkers.rst deleted file mode 100644 index 89e4ad88ab..0000000000 --- a/sphinx_doc/configuration/dwb-plugins/goal-checkers.rst +++ /dev/null @@ -1,22 +0,0 @@ -.. _dwb_goal_checkers: - -Goal Checkers -############# - -.. warning:: - - The parameters below are not properly scoped. The exact parameter names are - likely to change in the near future - -Common Parameters -================= - -:xy_goal_tolerance: - -:yaw_goal_tolerance: - -Simple Goal Checker -=================== - -Stopped Goal Checker -==================== diff --git a/sphinx_doc/configuration/dwb-plugins/trajectory-critics.rst b/sphinx_doc/configuration/dwb-plugins/trajectory-critics.rst deleted file mode 100644 index 8320027dee..0000000000 --- a/sphinx_doc/configuration/dwb-plugins/trajectory-critics.rst +++ /dev/null @@ -1,125 +0,0 @@ -.. _dwb_trajectory_critics: - -Trajectory Critics -################## - -.. warning:: - - The parameters below are not properly scoped. The exact parameter names are - likely to change in the near future - -Base Obstacle -============= -:BaseObstacle.class: - -:BaseObstacle.scale: - -:BaseObstacle.sum_scores: - - -Goal Align -========== -:GoalAlign.aggregation_type: - -:GoalAlign.class: - -:GoalAlign.forward_point_distance: - -:GoalAlign.scale: - - -Goal Distance -============= -:GoalDist.aggregation_type: - -:GoalDist.class: - -:GoalDist.scale: - - -Oscillation -=========== -:Oscillation.class: - -:Oscillation.oscillation_reset_angle: - -:Oscillation.oscillation_reset_dist: - -:Oscillation.oscillation_reset_time: - -:Oscillation.scale: - -:Oscillation.x_only_threshold: - - -Obstacle Footprint -================== - -Path Align -========== - -Parameters -********** - -:PathAlign.forward_point_distance: - -:PathAlign.scale: - -.. rst-class:: content-collapse - -Additional Parameters -********************* - -:PathAlign.aggregation_type: - -:PathAlign.class: - - -Path Distance -============= - -Parameters -********** - -:PathDist.scale: - -.. rst-class:: content-collapse - -Additional Parameters -********************* - -:PathDist.aggregation_type: - -:PathDist.class: - -Prefer Forward -============== - -Rotate To Goal -============== - -Parameters -********** - -:RotateToGoal.scale: - -:RotateToGoal.xy_goal_tolerance: - -:RotateToGoal.stateful: - -:RotateToGoal.trans_stopped_velocity: - -:RotateToGoal.slowing_factor: - -:RotateToGoal.lookahead_time: - -.. rst-class:: content-collapse - -Additional Parameters -********************* - -:RotateToGoal.class: - - -Twirling -======== diff --git a/sphinx_doc/configuration/dwb-plugins/trajectory-generators.rst b/sphinx_doc/configuration/dwb-plugins/trajectory-generators.rst deleted file mode 100644 index 247624c4c0..0000000000 --- a/sphinx_doc/configuration/dwb-plugins/trajectory-generators.rst +++ /dev/null @@ -1,61 +0,0 @@ -.. _dwb_trajectory_generators: - -Trajectory Generators -##################### - -.. warning:: - - The parameters below are not properly scoped. The exact parameter names are - likely to change in the near future - -Common Parameters -================= - -:acc_lim_theta: - -:acc_lim_x: - -:acc_lim_y: - -:decel_lim_theta: - -:decel_lim_x: - -:decel_lim_y: - -:max_speed_xy: - -:max_vel_theta: - -:max_vel_x: - -:max_vel_y: - -:min_speed_theta: - -:min_speed_xy: - -:min_theta_velocity_threshold: - -:min_vel_x: - -:min_vel_y: - -:min_x_velocity_threshold: - -:min_y_velocity_threshold: - -:vx_samples: - -:vy_samples: - -:discretize_by_time: - -:sim_time: - - -Standard Trajectory Generator -============================= - -Limited Acceleration Trajectory Generator -========================================= diff --git a/sphinx_doc/configuration/index.rst b/sphinx_doc/configuration/index.rst deleted file mode 100644 index 0cbd9ae643..0000000000 --- a/sphinx_doc/configuration/index.rst +++ /dev/null @@ -1,13 +0,0 @@ -.. _configuration: - -Configuration Guidelines -######################## - -This guide provides a process through which the user can adjust the tunable parameters to obtain -the best navigation performance. - -.. toctree:: - :maxdepth: 1 - - configuring-the-controller.rst - params/tunable-params.rst diff --git a/sphinx_doc/configuration/params/tunable-params.rst b/sphinx_doc/configuration/params/tunable-params.rst deleted file mode 100644 index 48f151a877..0000000000 --- a/sphinx_doc/configuration/params/tunable-params.rst +++ /dev/null @@ -1,419 +0,0 @@ -.. _docs_tunable-params: - -Tunable Parameters -################## - -Below is a list of all tunable parameters per node with some example values. -Only relevant nodes are shown. - -Also, the `nav2_bringup pkg`_ contains a `nav2_params file`_ with the default values to use for TB3. - -.. rst-class:: content-collapse - -AMCL -==== - -.. code-block:: yaml - - amcl: - ros__parameters: - alpha1: 0.2 - alpha2: 0.2 - alpha3: 0.2 - alpha4: 0.2 - alpha5: 0.2 - always_reset_initial_pose: false - base_frame_id: base_footprint - beam_skip_distance: 0.5 - beam_skip_error_threshold: 0.9 - beam_skip_threshold: 0.3 - do_beamskip: false - global_frame_id: map - lambda_short: 0.1 - laser_likelihood_max_dist: 2.0 - laser_max_range: 100.0 - laser_min_range: -1.0 - laser_model_type: likelihood_field - max_beams: 60 - max_particles: 2000 - min_particles: 500 - odom_frame_id: odom - pf_err: 0.05 - pf_z: 0.99 - recovery_alpha_fast: 0.0 - recovery_alpha_slow: 0.0 - resample_interval: 1 - robot_model_type: differential - save_pose_rate: 0.5 - set_initial_pose: false - sigma_hit: 0.2 - tf_broadcast: true - transform_tolerance: 1.0 - update_min_a: 0.2 - update_min_d: 0.25 - use_sim_time: true - z_hit: 0.5 - z_max: 0.05 - z_rand: 0.5 - z_short: 0.05 - - -.. rst-class:: content-collapse - -Controller Server -================= - -Hosts the `DWB` controller. - -.. code-block:: yaml - - controller_server: - ros__parameters: - BaseObstacle: - scale: 0.02 - sum_scores: false - BaseObstacle/class: BaseObstacle - GoalAlign: - aggregation_type: last - scale: 0.0 - GoalAlign/class: GoalAlign - GoalDist: - aggregation_type: last - scale: 24.0 - GoalDist/class: GoalDist - Oscillation: - scale: 1.0 - x_only_threshold: 0.05 - Oscillation/class: Oscillation - PathAlign: - aggregation_type: last - scale: 0.0 - PathAlign/class: PathAlign - PathDist: - aggregation_type: last - scale: 32.0 - PathDist/class: PathDist - RotateToGoal: - scale: 32.0 - RotateToGoal/class: RotateToGoal - acc_lim_theta: 3.2 - acc_lim_x: 2.5 - acc_lim_y: 0.0 - controller_frequency: 20.0 - critics: - - RotateToGoal - - Oscillation - - BaseObstacle - - GoalAlign - - PathAlign - - PathDist - - GoalDist - debug_trajectory_details: true - decel_lim_theta: -3.2 - decel_lim_x: -2.5 - decel_lim_y: 0.0 - discretize_by_time: false - goal_checker_name: dwb_plugins::SimpleGoalChecker - local_controller_plugin: dwb_core::DWBLocalPlanner - max_speed_xy: 0.26 - max_vel_theta: 1.0 - max_vel_x: 0.26 - max_vel_y: 0.0 - min_speed_theta: 0.0 - min_speed_xy: 0.0 - min_theta_velocity_threshold: 0.001 - min_vel_x: 0.0 - min_vel_y: 0.0 - min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 - prune_distance: 1.0 - prune_plan: true - publish_cost_grid_pc: false - publish_evaluation: true - publish_global_plan: true - publish_local_plan: true - publish_trajectories: true - publish_transformed_plan: true - sim_time: 1.7 - trajectory_generator_name: dwb_plugins::StandardTrajectoryGenerator - transform_tolerance: 0.2 - use_sim_time: true - vx_samples: 20 - vy_samples: 5 - xy_goal_tolerance: 0.25 - yaw_goal_tolerance: 0.25 - short_circuit_trajectory_evaluation: True - trans_stopped_velocity: 0.25 - slowing_factor: 5.0 - lookahead_time: -1.0 - stateful: True - - -.. rst-class:: content-collapse - -Local Costmap -================= - -Hosted on the `PlannerServer` node. - -.. code-block:: yaml - - local_costmap: - ros__parameters: - always_send_full_costmap: true - clearable_layers: - - obstacle_layer - footprint: '[]' - footprint_padding: 0.01 - global_frame: odom - height: 3 - inflation_layer: - cost_scaling_factor: 3.0 - enabled: true - inflate_unknown: false - inflation_radius: 0.55 - lethal_cost_threshold: 100 - map_topic: /map - observation_sources: '' - obstacle_layer: - combination_method: 1 - enabled: true - footprint_clearing_enabled: true - max_obstacle_height: 2.0 - observation_sources: scan - scan: - clearing: true - data_type: LaserScan - expected_update_rate: 0.0 - inf_is_valid: false - marking: true - max_obstacle_height: 2.0 - min_obstacle_height: 0.0 - observation_persistence: 0.0 - obstacle_range: 2.5 - raytrace_range: 3.0 - sensor_frame: '' - topic: /scan - origin_x: 0.0 - origin_y: 0.0 - plugin_names: - - obstacle_layer - - voxel_layer - - inflation_layer - plugin_types: - - nav2_costmap_2d::ObstacleLayer - - nav2_costmap_2d::VoxelLayer - - nav2_costmap_2d::InflationLayer - publish_frequency: 1.0 - resolution: 0.05 - robot_base_frame: base_link - robot_radius: 0.22 - rolling_window: true - track_unknown_space: false - transform_tolerance: 0.3 - trinary_costmap: true - unknown_cost_value: 255 - update_frequency: 5.0 - use_maximum: false - use_sim_time: true - voxel_layer: - combination_method: 1 - enabled: true - footprint_clearing_enabled: true - mark_threshold: 0 - max_obstacle_height: 2.0 - observation_sources: pointcloud - origin_z: 0.0 - pointcloud: - clearing: true - data_type: PointCloud2 - expected_update_rate: 0.0 - inf_is_valid: false - marking: true - max_obstacle_height: 2.0 - min_obstacle_height: 0.0 - observation_persistence: 0.0 - obstacle_range: 2.5 - raytrace_range: 3.0 - sensor_frame: '' - topic: /intel_realsense_r200_depth/points - publish_voxel_map: true - unknown_threshold: 15 - z_resolution: 0.2 - z_voxels: 10 - width: 3 - - -.. rst-class:: content-collapse - -Planner Server -================= - -Hosts the `NAVFN` controller. - -.. code-block:: yaml - - planner_server: - ros__parameters: - allow_unknown: true - planner_plugin: nav2_navfn_planner/NavfnPlanner - tolerance: 0.0 - use_astar: false - use_sim_time: true - - -.. rst-class:: content-collapse - -Global Costmap -================= - -Hosted on the `ControllerServer` node. - -.. code-block:: yaml - - global_costmap: - ros__parameters: - always_send_full_costmap: true - clearable_layers: - - obstacle_layer - footprint: '[]' - footprint_padding: 0.01 - global_frame: map - height: 10 - inflation_layer: - cost_scaling_factor: 10.0 - enabled: true - inflate_unknown: false - inflation_radius: 0.55 - lethal_cost_threshold: 100 - map_topic: /map - observation_sources: '' - obstacle_layer: - combination_method: 1 - enabled: true - footprint_clearing_enabled: true - max_obstacle_height: 2.0 - observation_sources: scan - scan: - clearing: true - data_type: LaserScan - expected_update_rate: 0.0 - inf_is_valid: false - marking: true - max_obstacle_height: 2.0 - min_obstacle_height: 0.0 - observation_persistence: 0.0 - obstacle_range: 2.5 - raytrace_range: 3.0 - sensor_frame: '' - topic: /scan - origin_x: 0.0 - origin_y: 0.0 - plugin_names: - - static_layer - - obstacle_layer - - voxel_layer - - inflation_layer - plugin_types: - - nav2_costmap_2d::StaticLayer - - nav2_costmap_2d::ObstacleLayer - - nav2_costmap_2d::VoxelLayer - - nav2_costmap_2d::InflationLayer - publish_frequency: 1.0 - resolution: 0.1 - robot_base_frame: base_link - robot_radius: 0.22 - rolling_window: false - static_layer: - enabled: true - map_subscribe_transient_local: true - subscribe_to_updates: false - track_unknown_space: false - transform_tolerance: 0.3 - trinary_costmap: true - unknown_cost_value: 255 - update_frequency: 5.0 - use_maximum: false - use_sim_time: true - voxel_layer: - combination_method: 1 - enabled: true - footprint_clearing_enabled: true - mark_threshold: 0 - max_obstacle_height: 2.0 - observation_sources: pointcloud - origin_z: 0.0 - pointcloud: - clearing: true - data_type: PointCloud2 - expected_update_rate: 0.0 - inf_is_valid: false - marking: true - max_obstacle_height: 2.0 - min_obstacle_height: 0.0 - observation_persistence: 0.0 - obstacle_range: 2.5 - raytrace_range: 3.0 - sensor_frame: '' - topic: /intel_realsense_r200_depth/points - publish_voxel_map: true - unknown_threshold: 15 - z_resolution: 0.2 - z_voxels: 10 - width: 10 - - -.. rst-class:: content-collapse - -Map Server -================= - -.. code-block:: yaml - - map_server: - ros__parameters: - use_sim_time: true - yaml_filename: turtlebot3_world.yaml - - -.. rst-class:: content-collapse - -Recovery Server -================= - -Hosts multiple recovery actions - -.. code-block:: yaml - - recoveries_server: - ros__parameters: - costmap_topic: local_costmap/costmap_raw - cycle_frequency: 10.0 - footprint_topic: local_costmap/published_footprint - max_rotational_vel: 1.0 - min_rotational_vel: 0.4 - plugin_names: - - Spin - - BackUp - - Wait - plugin_types: - - nav2_recoveries/Spin - - nav2_recoveries/BackUp - - nav2_recoveries/Wait - rotational_acc_lim: 3.2 - simulate_ahead_time: 2.0 - use_sim_time: true - - -.. rst-class:: content-collapse - -BT Navigator -================= -Behavior-Tree-based Navigator - -.. code-block:: yaml - - bt_navigator: - ros__parameters: - bt_xml_filename: navigate_w_replanning_and_recovery.xml - use_sim_time: true diff --git a/sphinx_doc/contribute/index.rst b/sphinx_doc/contribute/index.rst deleted file mode 100644 index a83c105f7e..0000000000 --- a/sphinx_doc/contribute/index.rst +++ /dev/null @@ -1,129 +0,0 @@ -.. _contribute: - -Getting Involved -################ - -As an open-source project, we welcome and encourage the community to -submit patches directly to the |LPN|. In our collaborative open -source environment, standards and methods for submitting changes help -reduce the chaos that can result from an active development community. - -This document explains how to participate in project conversations, log -and track bugs and enhancement requests, and submit patches to the -project so your patch will be accepted quickly in the codebase. - -Getting Involved -**************** - -If you're interested in getting involved in Navigation 2, first of all, welcome! -We encourage everyone to get involved from students, to junior developers, to senior developers, and executives. -There's something to do for everyone from bug fixes, to feature development, new algorithms, and refactoring. - -All ROS2 TSC Working Groups have their meetings on the `working group calendar `_. -Here, you can find the date and time of the Navigation2 working group meeting. Make sure you're checking in your local timezone. -From this calendar, you can add yourself to the event so it will appear on your google calendar and get the event link to the call through Google Hangouts. -We encourage everyone interested to come to the meeting to introduce yourself, your project, and see what everyone is working on. - -Further, `ROS Discourse `_ is a good place to follow larger discussions happening in the community and announcements. This is **not** the correct place to post questions or ask for assistance. Please visit `ROS Answers `_ for Q&A. - -If you're looking to contribute code or bugs, please see the Process section below. - -Over time, for developers that have an interest and have shown technical competence in an area of the stack, we elevate developers to a maintainers status. -That allows push rights to our protected branches, first-reviewers rights, and getting your name on :ref:`about`. -There currently is not a clear process for getting to be a maintainer, but if you've been involved and contributing over a duration of several months, you may be a good candidate and should email the project lead listed on :ref:`about`. - -Process -******* - -After you've introduced yourself in a working group meeting (recommended, not required), you're ready to get started! -We recommend a typical open-source project flow and value detail and transparency. -If you commit to something and need to pull back, say so. -We all know priorities change and appreciate the heads up so that task can go into the open queue of tasks. - -The process is simple and is as follow: - -1. Create a ticket for any issues or features you'd like to see. You are not required to fix / implement patches required, but it would be helpful. Reporting bugs is also a valuable contribution. - -2. If this ticket, or another existing ticket, is something you would like to work on, comment in the ticket claiming ownership over it. It would be helpful at this time if you declared a strategy and a timeline for planning purposes of other folks working around you. Over time, update the ticket with progress of key markers and engage in any constructive feedback maintainers or other users may have. - -3. Once you've completed the task you set out to complete, submit a PR! Please fill out the PR template in complete to ensure that we have a full understanding of your work. At that point, 1-2 reviewers will take a look at your work and give it some feedback to be merged into the codebase. For trivial changes, a single maintainer may merge it after review if they're happy with it, up to their discretion. Any substantial changes should be approved by at least 1 maintainer and 1 other community member. - -Note: We take code quality seriously and strive for high-quality and consistent code. -We make use of the linting and static analysis tools provided in ROS2 (``ament_cpplint``, ``ament_uncrustify``, ``ament_cppcheck``, etc). -All PRs are built in CI with the appropriate ROS distributions and run through a set of unit and system level tests including static analysis. -You can see the results of these tests in the pull request. -It is expected for feature development for tests to cover this work to be added. -If any documentation must be updated due to your changes, that should be included in your pull request. - -Licensing -********* - -Licensing is very important to open source projects. It helps ensure the -software continues to be available under the terms that the author -desired. - -Because much of the source code is ported from other ROS 1 projects, each -package has it's own license. Contributions should be made under the predominant -license of that package. Entirely new packages should be made available under -the `Apache 2.0 license `_. - -A license tells you what rights you have as a developer, as provided by -the copyright holder. It is important that the contributor fully -understands the licensing rights and agrees to them. Sometimes the -copyright holder isn't the contributor, such as when the contributor is -doing work on behalf of a company. - -If for some reason Apache 2.0 or BSD licenses are not appropriate for your work, please get in contact with a project maintainer and discuss your concerns or requirements. -We may consider special exceptions for exceptional work, within reason (we will not accept any licenses that makes it unsuitable for commercial use). - -.. _DCO: - -Developer Certification of Origin (DCO) -*************************************** - -To make a good faith effort to ensure licensing criteria are met, -|LPN| encourages the Developer Certificate of Origin (DCO) process -to be followed. - -The DCO is an attestation attached to every contribution made by a -developer. In the commit message of the contribution, (described more -fully later in this document), the developer simply adds a -``Signed-off-by`` statement and thereby agrees to the DCO. - -In practice, its easier to just ``git commit -s -m "commit messsage."``. -Where ``-s`` adds this automatically. -If you forgot to add this to a commit, it is easy to append via: ``git commit --amend -s``. - -When a developer submits a patch, it is a commitment that the -contributor has the right to submit the patch per the license. The DCO -agreement is shown below and at http://developercertificate.org/. - -.. code-block:: none - - Developer's Certificate of Origin 1.1 - - By making a contribution to this project, I certify that: - - (a) The contribution was created in whole or in part by me and I - have the right to submit it under the open source license - indicated in the file; or - - (b) The contribution is based upon previous work that, to the - best of my knowledge, is covered under an appropriate open - source license and I have the right under that license to - submit that work with modifications, whether created in whole - or in part by me, under the same open source license (unless - I am permitted to submit under a different license), as - Indicated in the file; or - - (c) The contribution was provided directly to me by some other - person who certified (a), (b) or (c) and I have not modified - it. - - (d) I understand and agree that this project and the contribution - are public and that a record of the contribution (including - all personal information I submit with it, including my - sign-off) is maintained indefinitely and may be redistributed - consistent with this project or the open source license(s) - involved. - diff --git a/sphinx_doc/getting_started/images/navigation_with_recovery_behaviours.gif b/sphinx_doc/getting_started/images/navigation_with_recovery_behaviours.gif deleted file mode 100644 index 58fd51cb78..0000000000 Binary files a/sphinx_doc/getting_started/images/navigation_with_recovery_behaviours.gif and /dev/null differ diff --git a/sphinx_doc/getting_started/index.rst b/sphinx_doc/getting_started/index.rst deleted file mode 100644 index fe11320c19..0000000000 --- a/sphinx_doc/getting_started/index.rst +++ /dev/null @@ -1,122 +0,0 @@ -.. _getting_started: - -Getting Started -############### - -This document will take you through the process of installing the |PN| binaries -and navigating a simulated Turtlebot 3 in the Gazebo simulator. We'll use the -|Distro| version of ROS 2 and Ubuntu 18 for the standard installation options. - -.. note:: - - See the :ref:`build-instructions` for other situations such as building from source or - working with other types of robots. - -.. warning:: - - This is a simplified version of the Turtlebot 3 instructions. We highly - recommend you follow the `official Turtlebot 3 manual`_ if you intend to - continue working with this robot beyond the minimal example provided here. - -Installation -************ - -1. Install the `ROS 2 binary packages`_ as described in the official docs -2. Install the |PN| packages using your operating system's package manager: - - .. code-block:: bash - - sudo apt install ros-dashing-navigation2 - sudo apt install ros-dashing-nav2-bringup - -3. Install the Turtlebot 3 packages: - - .. code-block:: bash - - sudo apt install ros-dashing-turtlebot3* - -Running the Example -******************* - -1. Start a terminal in your GUI -2. Set key environment variables: - - .. code-block:: bash - - source /opt/ros/dashing/setup.bash - export TURTLEBOT3_MODEL=waffle - export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/dashing/share/turtlebot3_gazebo/models - -3. In the same terminal, run - - .. code-block:: bash - - ros2 launch nav2_bringup tb3_simulation_launch.py - - This launch file will launch Navigation2 with the AMCL localizer in the - ``turtlebot3_world`` world. - It will also launch a the robot state publisher to provide transforms, - a Gazebo instance with the Turtlebot3 URDF, and RVIZ. - - If everything has started correctly, you will see the RViz and Gazebo GUIs like - this: - - .. image:: /images/rviz/rviz-not-started.png - :width: 45% - .. image:: /images/gazebo/gazebo_turtlebot1.png - :width: 46% - -4. Click the "Startup" button in the bottom left corner of RViz. This will - cause |PN| to change to the Active state. It should - change appearance to show the map. - - .. image:: /images/rviz/rviz_initial.png - :width: 700px - :align: center - :alt: Initial appearance of RViz transitioning to the Active state - -Navigating -********** - -After starting, the robot initially has no idea where it is. By default, -|PN| waits for you to give it an approximate starting position. Take a look -at where the robot is in the Gazebo world, and find that spot on the map. Set -the initial pose by clicking the "2D Pose Estimate" button in RViz, and then -down clicking on the map in that location. You set the orientation by dragging -forward from the down click. - -If you are using the defaults so far, the robot should look roughly like this. - - .. image:: /images/rviz/rviz-set-initial-pose.png - :width: 700px - :align: center - :alt: Approximate starting location of Turtlebot - -If you don't get the location exactly right, that's fine. |PN| will refine -the position as it navigates. You can also, click the "2D Pose -Estimate" button and try again, if you prefer. - -Once you've set the initial pose, the transform tree will be complete and -|PN| is fully active and ready to go. You should see the robot and particle -cloud now. - - .. image:: /images/rviz/navstack-ready.png - :width: 700px - :align: center - :alt: |PN| is ready. Transforms and Costmap show in RViz. - -Next, click the "Navigaton2 Goal" button and choose a destination. -This will call the BT navigator to go to that goal through an action server. -You can pause (cancel) or reset the action through the navigation2 rviz plugin shown. - - .. image:: /images/rviz/navigate-to-pose.png - :width: 700px - :align: center - :alt: Setting the goal pose in RViz. - -Now watch the robot go! - -.. image:: images/navigation_with_recovery_behaviours.gif - :width: 700px - :alt: Navigation2 with Turtlebot 3 Demo - :align: center diff --git a/sphinx_doc/images/architectural_diagram.png b/sphinx_doc/images/architectural_diagram.png deleted file mode 100644 index 9801ef9d69..0000000000 Binary files a/sphinx_doc/images/architectural_diagram.png and /dev/null differ diff --git a/sphinx_doc/images/favicon-16x16.png b/sphinx_doc/images/favicon-16x16.png deleted file mode 100644 index e9a16774c9..0000000000 Binary files a/sphinx_doc/images/favicon-16x16.png and /dev/null differ diff --git a/sphinx_doc/images/favicon-48x48.png b/sphinx_doc/images/favicon-48x48.png deleted file mode 100644 index ce2315d535..0000000000 Binary files a/sphinx_doc/images/favicon-48x48.png and /dev/null differ diff --git a/sphinx_doc/images/gazebo/gazebo_turtlebot1.png b/sphinx_doc/images/gazebo/gazebo_turtlebot1.png deleted file mode 100644 index b5776b16f4..0000000000 Binary files a/sphinx_doc/images/gazebo/gazebo_turtlebot1.png and /dev/null differ diff --git a/sphinx_doc/images/logo_dark.png b/sphinx_doc/images/logo_dark.png deleted file mode 100644 index 0af0b3cc92..0000000000 Binary files a/sphinx_doc/images/logo_dark.png and /dev/null differ diff --git a/sphinx_doc/images/logo_dark_120w.png b/sphinx_doc/images/logo_dark_120w.png deleted file mode 100644 index 888f5f1632..0000000000 Binary files a/sphinx_doc/images/logo_dark_120w.png and /dev/null differ diff --git a/sphinx_doc/images/logo_white.png b/sphinx_doc/images/logo_white.png deleted file mode 100644 index 71aa3c69a7..0000000000 Binary files a/sphinx_doc/images/logo_white.png and /dev/null differ diff --git a/sphinx_doc/images/logo_white_200w.png b/sphinx_doc/images/logo_white_200w.png deleted file mode 100644 index b446b057cd..0000000000 Binary files a/sphinx_doc/images/logo_white_200w.png and /dev/null differ diff --git a/sphinx_doc/images/rviz/navigate-to-pose.png b/sphinx_doc/images/rviz/navigate-to-pose.png deleted file mode 100644 index 0634a46c8c..0000000000 Binary files a/sphinx_doc/images/rviz/navigate-to-pose.png and /dev/null differ diff --git a/sphinx_doc/images/rviz/navigating.png b/sphinx_doc/images/rviz/navigating.png deleted file mode 100644 index 3e84ce8098..0000000000 Binary files a/sphinx_doc/images/rviz/navigating.png and /dev/null differ diff --git a/sphinx_doc/images/rviz/navstack-ready.png b/sphinx_doc/images/rviz/navstack-ready.png deleted file mode 100644 index 43e89ea6ca..0000000000 Binary files a/sphinx_doc/images/rviz/navstack-ready.png and /dev/null differ diff --git a/sphinx_doc/images/rviz/rviz-not-started.png b/sphinx_doc/images/rviz/rviz-not-started.png deleted file mode 100644 index 733471141b..0000000000 Binary files a/sphinx_doc/images/rviz/rviz-not-started.png and /dev/null differ diff --git a/sphinx_doc/images/rviz/rviz-set-initial-pose.png b/sphinx_doc/images/rviz/rviz-set-initial-pose.png deleted file mode 100644 index c732e2ecf5..0000000000 Binary files a/sphinx_doc/images/rviz/rviz-set-initial-pose.png and /dev/null differ diff --git a/sphinx_doc/images/rviz/rviz_initial.png b/sphinx_doc/images/rviz/rviz_initial.png deleted file mode 100644 index 5a1a4e505b..0000000000 Binary files a/sphinx_doc/images/rviz/rviz_initial.png and /dev/null differ diff --git a/sphinx_doc/index.rst b/sphinx_doc/index.rst deleted file mode 100644 index 0db0a7bfc6..0000000000 --- a/sphinx_doc/index.rst +++ /dev/null @@ -1,92 +0,0 @@ -.. _documentation_home: - -***** -|LPN| -***** - -.. raw:: html - -

-
-
- - -
-
-

- -Overview -######## - -The Navigation 2 project is the spiritual successor of the ROS Navigation Stack. -This project seeks to find a safe way to have a mobile robot move from point A to -point B. This will complete dynamic path planning, compute velocities for motors, -avoid obstacles, and structure recovery behaviors. To learn more about this project see :ref:`about`. - -Navigation 2 uses behavior trees to call modular servers to complete an action. -An action can be to compute a path, control effort, recovery, or any other navigation -related action. These are each separate nodes that communicate with the behavior tree (BT) -over a ROS action server. The diagram below will give you a good first-look at the structure -of Navigation 2. Note: It is possible to have multiple plugins for controllers, planners, -and recoveries in each of their servers with matching BT plugins. This can be used to -create contextual navigation behaviors. -If you would like to see a comparison between this project and ROS (1) Navigation, see :ref:`ros1_comparison`. - -The expected inputs to Navigation2 (Nav2) are TF transformations conforming to REP-105, a -map source if utilizing the Static Costmap Layer, a BT XML file, and any relevant sensor data -sources. It will then provide valid velocity commands for the motors of a holonomic or -non-holonomic robot to follow. We currently support holonomic and differential-drive base types -but plan to support Ackermann (car-like) robots as well in the near future. - - -It has tools to: - -- load, serve, and store maps (Map Server) -- localize the robot on the map (AMCL) -- plan a path from A to B around obstacles (Nav2 Planner) -- control the robot as it follows the path (Nav2 Controller) -- convert sensor data into a costmap representation of the world (Nav2 Costmap 2D) -- build complicated robot behaviors using behavior trees (Nav2 Behavior Trees and BT Navigator) -- Compute recovery behaviors in case of failure (Nav2 Recoveries) -- Follow sequential waypoints (Nav2 Waypoint Follower) -- Manage the lifecycle of the servers (Nav2 Lifecycle Manager) -- Plugins to enable your own custom algorithms and behaviors (Nav2 Core) - -.. image:: images/architectural_diagram.png - :width: 700px - :align: center - :alt: Navigation2 Block Diagram - -We also provide a set of starting plugins to get you going. NavFn computes the shortest path -from a pose to a goal pose using A* or Dijkstra's algorithm. DWB will use the DWA algorithm -to compute a control effort to follow a path, with several plugins of its own for trajectory -critics. There are recovery behaviors included: waiting, spinning, clearing costmaps, and -backing up. There are a set of BT plugins for calling these servers and computing conditions. -Finally, there are a set of Rviz plugins for interacting with the stack and controlling the -lifecycle. A list of all user-reported plugins can be found on :ref:`plugins`. - -Here is the documentation on how to install and use |PN| with an example robot, Turtlebot -3 (TB3), as well as how to customize it for other robots, tune the behavior for better -performance, as well as customize the internals for advanced results. Below is an example -of the TB3 navigating in a small lounge. - -.. raw:: html - -

-
- -
-

- -.. toctree:: - :hidden: - - getting_started/index.rst - build_instructions/index.rst - concepts/index.rst - tutorials/index.rst - configuration/index.rst - plugins/index.rst - migration/index.rst - contribute/index.rst - about/index.rst diff --git a/sphinx_doc/make.bat b/sphinx_doc/make.bat deleted file mode 100644 index 0825fc7298..0000000000 --- a/sphinx_doc/make.bat +++ /dev/null @@ -1,263 +0,0 @@ -@ECHO OFF - -REM Command file for Sphinx documentation - -if "%SPHINXBUILD%" == "" ( - set SPHINXBUILD=sphinx-build -) -set BUILDDIR=_build -set ALLSPHINXOPTS=-d %BUILDDIR%/doctrees %SPHINXOPTS% . -set I18NSPHINXOPTS=%SPHINXOPTS% . -if NOT "%PAPER%" == "" ( - set ALLSPHINXOPTS=-D latex_paper_size=%PAPER% %ALLSPHINXOPTS% - set I18NSPHINXOPTS=-D latex_paper_size=%PAPER% %I18NSPHINXOPTS% -) - -if "%1" == "" goto help - -if "%1" == "help" ( - :help - echo.Please use `make ^` where ^ is one of - echo. html to make standalone HTML files - echo. dirhtml to make HTML files named index.html in directories - echo. singlehtml to make a single large HTML file - echo. pickle to make pickle files - echo. json to make JSON files - echo. htmlhelp to make HTML files and a HTML help project - echo. qthelp to make HTML files and a qthelp project - echo. devhelp to make HTML files and a Devhelp project - echo. epub to make an epub - echo. latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter - echo. text to make text files - echo. man to make manual pages - echo. texinfo to make Texinfo files - echo. gettext to make PO message catalogs - echo. changes to make an overview over all changed/added/deprecated items - echo. xml to make Docutils-native XML files - echo. pseudoxml to make pseudoxml-XML files for display purposes - echo. linkcheck to check all external links for integrity - echo. doctest to run all doctests embedded in the documentation if enabled - echo. coverage to run coverage check of the documentation if enabled - goto end -) - -if "%1" == "clean" ( - for /d %%i in (%BUILDDIR%\*) do rmdir /q /s %%i - del /q /s %BUILDDIR%\* - goto end -) - - -REM Check if sphinx-build is available and fallback to Python version if any -%SPHINXBUILD% 2> nul -if errorlevel 9009 goto sphinx_python -goto sphinx_ok - -:sphinx_python - -set SPHINXBUILD=python -m sphinx.__init__ -%SPHINXBUILD% 2> nul -if errorlevel 9009 ( - echo. - echo.The 'sphinx-build' command was not found. Make sure you have Sphinx - echo.installed, then set the SPHINXBUILD environment variable to point - echo.to the full path of the 'sphinx-build' executable. Alternatively you - echo.may add the Sphinx directory to PATH. - echo. - echo.If you don't have Sphinx installed, grab it from - echo.http://sphinx-doc.org/ - exit /b 1 -) - -:sphinx_ok - - -if "%1" == "html" ( - %SPHINXBUILD% -b html %ALLSPHINXOPTS% %BUILDDIR%/html - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The HTML pages are in %BUILDDIR%/html. - goto end -) - -if "%1" == "dirhtml" ( - %SPHINXBUILD% -b dirhtml %ALLSPHINXOPTS% %BUILDDIR%/dirhtml - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The HTML pages are in %BUILDDIR%/dirhtml. - goto end -) - -if "%1" == "singlehtml" ( - %SPHINXBUILD% -b singlehtml %ALLSPHINXOPTS% %BUILDDIR%/singlehtml - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The HTML pages are in %BUILDDIR%/singlehtml. - goto end -) - -if "%1" == "pickle" ( - %SPHINXBUILD% -b pickle %ALLSPHINXOPTS% %BUILDDIR%/pickle - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can process the pickle files. - goto end -) - -if "%1" == "json" ( - %SPHINXBUILD% -b json %ALLSPHINXOPTS% %BUILDDIR%/json - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can process the JSON files. - goto end -) - -if "%1" == "htmlhelp" ( - %SPHINXBUILD% -b htmlhelp %ALLSPHINXOPTS% %BUILDDIR%/htmlhelp - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can run HTML Help Workshop with the ^ -.hhp project file in %BUILDDIR%/htmlhelp. - goto end -) - -if "%1" == "qthelp" ( - %SPHINXBUILD% -b qthelp %ALLSPHINXOPTS% %BUILDDIR%/qthelp - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; now you can run "qcollectiongenerator" with the ^ -.qhcp project file in %BUILDDIR%/qthelp, like this: - echo.^> qcollectiongenerator %BUILDDIR%\qthelp\ClearLinux.qhcp - echo.To view the help file: - echo.^> assistant -collectionFile %BUILDDIR%\qthelp\ClearLinux.ghc - goto end -) - -if "%1" == "devhelp" ( - %SPHINXBUILD% -b devhelp %ALLSPHINXOPTS% %BUILDDIR%/devhelp - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. - goto end -) - -if "%1" == "epub" ( - %SPHINXBUILD% -b epub %ALLSPHINXOPTS% %BUILDDIR%/epub - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The epub file is in %BUILDDIR%/epub. - goto end -) - -if "%1" == "latex" ( - %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex - if errorlevel 1 exit /b 1 - echo. - echo.Build finished; the LaTeX files are in %BUILDDIR%/latex. - goto end -) - -if "%1" == "latexpdf" ( - %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex - cd %BUILDDIR%/latex - make all-pdf - cd %~dp0 - echo. - echo.Build finished; the PDF files are in %BUILDDIR%/latex. - goto end -) - -if "%1" == "latexpdfja" ( - %SPHINXBUILD% -b latex %ALLSPHINXOPTS% %BUILDDIR%/latex - cd %BUILDDIR%/latex - make all-pdf-ja - cd %~dp0 - echo. - echo.Build finished; the PDF files are in %BUILDDIR%/latex. - goto end -) - -if "%1" == "text" ( - %SPHINXBUILD% -b text %ALLSPHINXOPTS% %BUILDDIR%/text - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The text files are in %BUILDDIR%/text. - goto end -) - -if "%1" == "man" ( - %SPHINXBUILD% -b man %ALLSPHINXOPTS% %BUILDDIR%/man - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The manual pages are in %BUILDDIR%/man. - goto end -) - -if "%1" == "texinfo" ( - %SPHINXBUILD% -b texinfo %ALLSPHINXOPTS% %BUILDDIR%/texinfo - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The Texinfo files are in %BUILDDIR%/texinfo. - goto end -) - -if "%1" == "gettext" ( - %SPHINXBUILD% -b gettext %I18NSPHINXOPTS% %BUILDDIR%/locale - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The message catalogs are in %BUILDDIR%/locale. - goto end -) - -if "%1" == "changes" ( - %SPHINXBUILD% -b changes %ALLSPHINXOPTS% %BUILDDIR%/changes - if errorlevel 1 exit /b 1 - echo. - echo.The overview file is in %BUILDDIR%/changes. - goto end -) - -if "%1" == "linkcheck" ( - %SPHINXBUILD% -b linkcheck %ALLSPHINXOPTS% %BUILDDIR%/linkcheck - if errorlevel 1 exit /b 1 - echo. - echo.Link check complete; look for any errors in the above output ^ -or in %BUILDDIR%/linkcheck/output.txt. - goto end -) - -if "%1" == "doctest" ( - %SPHINXBUILD% -b doctest %ALLSPHINXOPTS% %BUILDDIR%/doctest - if errorlevel 1 exit /b 1 - echo. - echo.Testing of doctests in the sources finished, look at the ^ -results in %BUILDDIR%/doctest/output.txt. - goto end -) - -if "%1" == "coverage" ( - %SPHINXBUILD% -b coverage %ALLSPHINXOPTS% %BUILDDIR%/coverage - if errorlevel 1 exit /b 1 - echo. - echo.Testing of coverage in the sources finished, look at the ^ -results in %BUILDDIR%/coverage/python.txt. - goto end -) - -if "%1" == "xml" ( - %SPHINXBUILD% -b xml %ALLSPHINXOPTS% %BUILDDIR%/xml - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The XML files are in %BUILDDIR%/xml. - goto end -) - -if "%1" == "pseudoxml" ( - %SPHINXBUILD% -b pseudoxml %ALLSPHINXOPTS% %BUILDDIR%/pseudoxml - if errorlevel 1 exit /b 1 - echo. - echo.Build finished. The pseudo-XML files are in %BUILDDIR%/pseudoxml. - goto end -) - -:end diff --git a/sphinx_doc/migration/Dashing.rst b/sphinx_doc/migration/Dashing.rst deleted file mode 100644 index e22a8a5d3e..0000000000 --- a/sphinx_doc/migration/Dashing.rst +++ /dev/null @@ -1,58 +0,0 @@ -.. _dashing_migration: - -Dashing to Eloquent -################### - -Moving from ROS2 Dashing to Eloquent, a number of stability improvements were added that we will not specifically address here. - -New Packages -************ - -Navigation2 now includes a new package ``nav2_waypoint_follower``. -The waypoint follower is an action server that will take in a list of waypoints to follow and follow them in order. -There is a parameter ``stop_on_failure`` whether the robot should continue to the next waypoint on a single waypoint failure, -or to return fail to the action client. -The waypoint follower is also a reference application for how to use the Navigation2 action server to complete a basic autonomy task. - - -Navigation2 now supports new algorithms for control and SLAM. -The Timed-Elastic Band (TEB) controller was implemented `and can be found here `_. -It is its own controller plugin that can be used instead of the DWB controller. -Nav2 also supports SLAM Toolbox as the default SLAM implementation for ROS2. -This replaces the use of Cartographer. - -New Plugins -*********** - -Eloquent introduces back in pluginlib plugins to the navigation stack. -``nav2_core`` defines the plugin header interfaces to be used to implement controller, planner, recovery, and goal checker plugins. -All algorithms (NavFn, DWB, recoveries) were added as plugin interfaces and the general packages for servers were created. -``nav2_planner`` is the action server for planning that hosts a plugin for the planner. -``nav2_controller`` is the action server for controller that hosts a plugin for the controller. -``nav2_recovery`` is the action server for recovery that hosts a plugin for recovery. - -New recovery plugins were added including backup, which will take in a distance to back up, if collision-free. -Additionally, the wait recovery was added that will wait a configurable period of time before trying to navigate again. -This plugin is especially helpful for time-dependent obstacles or pausing navigation for a scene to become less dynamic. - -Many new behavior tree nodes were added. These behavior tree nodes are hard-coded in the behavior tree engine. -Behavior tree cpp v3 supports plugins and will be converted in the next release. - -Navigation2 Architectural Changes -********************************* - -The ``nav2_world_model`` package was removed. The individual ``nav2_planner`` and ``nav2_controller`` servers now host their relevent costmaps. -This was done to reduce network traffic and ensure up-to-date information for the safety-critical elements of the system. -As above mentions, plugins were introduced into the stack and these servers each host plugins for navigation, control, and costmap layers. - -Map server was substantially refactored but the external API remains the same. It now uses the SDL library for image loading. - -TF-based positioning is now used for pose-estimation everywhere in the stack. -Prior, some elements of the navigation stack only updated its pose from the ``/amcl_pose`` topic publishing at an irregular rate. -This is obviously low-accuracy and high-latency. -All positioning is now based on the TF tree from the global frame to the robot frame. - -Prior to Eloquent, there were no ROS2 action servers and clients available. -Navigation2, rather, used an interface we called Tasks. -Eloquent now contains actions and a simple action server interface was created and is used now throughout the stack. -Tasks were removed. diff --git a/sphinx_doc/migration/Eloquent.rst b/sphinx_doc/migration/Eloquent.rst deleted file mode 100644 index 356c032f78..0000000000 --- a/sphinx_doc/migration/Eloquent.rst +++ /dev/null @@ -1,44 +0,0 @@ -.. _eloquent_migration: - -Eloquent to Foxy -################ - -Moving from ROS2 Eloquent to Foxy, a number of stability improvements were added that we will not specifically address here. -We will specifically mention, however, the reduction in terminal noise. -TF2 transformation timeout errors and warnings on startup have been largely removed or throttled to be more tractable. -Additionally, message filters filling up resulting in messages being dropped were resolved in costmap 2d. - -General -******* - -The lifecycle manager was split into 2 unique lifecycle managers. -They are the ``navigation_lifecycle_manager`` and ``localization_lifecycle_manager``. -This gives each process their own manager to allow users to switch between SLAM and localization without effecting Navigation. -It also reduces the redundant code in ``nav2_bringup``. - -A fix to the BT navigator was added to remove a rare issue where it may crash due to asynchronous issues. -As a result, a behavior tree is created for each navigation request rather than resetting an existing tree. -The creation of this tree will add a small amount of latency. -Proposals to reduce this latency will be considered before the next release. - -Server Updates -************** -All plugin servers (controller, planner, recovery) now supports the use of multiple plugins. -This can be done by loading a map of plugins, mapping the name of the plugin to its intended use-case. -An example: ``FollowPath`` controller of type ``dwb_local_planner/DWBLocalPlanner`` and a ``DynamicFollowPath`` of type ``teb_local_planner/TEBLocalPlanner``. -Each plugin will load the parameters in their namespace, e.g. ``FollowPath.max_vel_x``, rather than globally in the server namespace. -This will allow multiple plugins of the same type with different parameters and reduce conflicting parameter names. - -DWB Contains new parameters as an update relative to the ROS1 updates, `see here for more information `_. -Additionally, the controller and planner interfaces were updated to include a ``std::string name`` parameter on initialization. -This was added to the interfaces to allow the plugins to know the namespace it should load its parameters in. -E.g. for a controller to find the parameter ``FollowPath.max_vel_x``, it must be given its name, ``FollowPath`` to get this parameter. -All plugins will be expected to look up parameters in the namespace of its given name. - -New Plugins -*********** - -Many new behavior tree nodes were added. -These behavior tree nodes are now BT plugins and dynamically loadable at run-time using behavior tree cpp v3. -See ``nav2_behavior_tree`` for a full listing, or :ref:`plugins` for the current list of behavior tree plugins and their descriptions. -These plugins are set as default in the ``nav2_bt_navigator`` but may be overrided by the ``bt_plugins`` parameter to include your specific plugins. diff --git a/sphinx_doc/migration/index.rst b/sphinx_doc/migration/index.rst deleted file mode 100644 index 9c9ba06535..0000000000 --- a/sphinx_doc/migration/index.rst +++ /dev/null @@ -1,21 +0,0 @@ -.. _migration: - -Migration Guides -################ - -Navigation2 guides for migration between distributions. - -.. raw:: html - -

-
- -
-

- -.. toctree:: - :hidden: - :maxdepth: 1 - - Dashing.rst - Eloquent.rst diff --git a/sphinx_doc/plugins/index.rst b/sphinx_doc/plugins/index.rst deleted file mode 100644 index e20a83a178..0000000000 --- a/sphinx_doc/plugins/index.rst +++ /dev/null @@ -1,206 +0,0 @@ -.. _plugins: - -Navigation Plugins -################## - -There are a number of plugin interfaces for users to create their own custom applications or algorithms with. -Namely, the costmap layer, planner, controller, behavior tree, and recovery plugins. -A list of all known plugins are listed here below for ROS2 Navigation. -If you know of a plugin, or you have created a new plugin, please consider submitting a pull request with that information. - -This file can be found and editted under ``sphinx_docs/plugins/index.rst``. -For tutorials on creating your own plugins, please see :ref:`writing_new_costmap2d_plugin`. - -Costmap Layers -============== - -+--------------------------------+------------------------+----------------------------------+ -| Plugin Name | Creator | Description | -+================================+========================+==================================+ -| `Voxel Layer`_ | Eitan Marder-Eppstein | Maintains persistant | -| | | 3D voxel layer using depth and | -| | | laser sensor readings and | -| | | raycasting to clear free space | -+--------------------------------+------------------------+----------------------------------+ -| `Static Layer`_ | Eitan Marder-Eppstein | Gets static ``map`` and loads | -| | | occupancy information into | -| | | costmap | -+--------------------------------+------------------------+----------------------------------+ -| `Inflation Layer`_ | Eitan Marder-Eppstein | Inflates lethal obstacles in | -| | | costmap with exponential decay | -+--------------------------------+------------------------+----------------------------------+ -| `Obstacle Layer`_ | Eitan Marder-Eppstein | Maintains persistent 2D costmap | -| | | from 2D laser scans with | -| | | raycasting to clear free space | -+--------------------------------+------------------------+----------------------------------+ -| `Spatio-Temporal Voxel Layer`_ | Steve Macenski | Maintains temporal 3D sparse | -| | | volumetric voxel grid with decay | -| | | through sensor models | -+--------------------------------+------------------------+----------------------------------+ -| `Non-Persistent Voxel Layer`_ | Steve Macenski | Maintains 3D occupancy grid | -| | | consisting only of the most | -| | | sets of measurements | -+--------------------------------+------------------------+----------------------------------+ - -.. _Voxel Layer: https://github.com/ros-planning/navigation2/tree/master/nav2_costmap_2d/plugins -.. _Static Layer: https://github.com/ros-planning/navigation2/tree/master/nav2_costmap_2d/plugins -.. _Inflation Layer: https://github.com/ros-planning/navigation2/tree/master/nav2_costmap_2d/plugins -.. _Obstacle Layer: https://github.com/ros-planning/navigation2/tree/master/nav2_costmap_2d/plugins -.. _Spatio-Temporal Voxel Layer: https://github.com/SteveMacenski/spatio_temporal_voxel_layer/ -.. _Non-Persistent Voxel Layer: https://github.com/SteveMacenski/nonpersistent_voxel_layer - -Controllers -=========== - -+--------------------------+--------------------+----------------------------------+ -| Plugin Name | Creator | Description | -+==========================+====================+==================================+ -| `DWB Controller`_ | David Lu!! | A highly configurable DWA | -| | | implementation with plugin | -| | | interfaces | -+--------------------------+--------------------+----------------------------------+ -| `TEB Controller`_ | Christoph Rösmann | A MPC-like controller suitable | -| | | for ackermann, differential, and | -| | | holonomic robots. | -+--------------------------+--------------------+----------------------------------+ - -.. _DWB Controller: https://github.com/ros-planning/navigation2/tree/master/nav2_dwb_controller -.. _TEB Controller: https://github.com/rst-tu-dortmund/teb_local_planner - -Planners -======== - -+-------------------+---------------------------------------+------------------------------+ -| Plugin Name | Creator | Description | -+===================+=======================================+==============================+ -| `NavFn Planner`_ | Eitan Marder-Eppstein & Kurt Konolige | A navigation function | -| | | using A* or Dijkstras | -| | | expansion, assumes 2D | -| | | holonomic particle | -+-------------------+---------------------------------------+------------------------------+ - -.. _NavFn Planner: https://github.com/ros-planning/navigation2/tree/master/nav2_navfn_planner - -Recoveries -========== - -+----------------------+------------------------+----------------------------------+ -| Plugin Name | Creator | Description | -+======================+========================+==================================+ -| `Clear Costmap`_ | Eitan Marder-Eppstein | A service to clear the given | -| | | costmap in case of incorrect | -| | | perception or robot is stuck | -+----------------------+------------------------+----------------------------------+ -| `Spin`_ | Steve Macenski | Rotate recovery of configurable | -| | | angles to clear out free space | -| | | and nudge robot out of potential | -| | | local failures | -+----------------------+------------------------+----------------------------------+ -| `Back Up`_ | Brian Wilcox | Back up recovery of configurable | -| | | distance to back out of a | -| | | situation where the robot is | -| | | stuck | -+----------------------+------------------------+----------------------------------+ -| `Wait`_ | Steve Macenski | Wait recovery with configurable | -| | | time to wait in case of time | -| | | based obstacle like human traffic| -| | | or getting more sensor data | -+----------------------+------------------------+----------------------------------+ - -.. _Rotate: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Back Up: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Spin: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Wait: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Clear Costmap: https://github.com/ros-planning/navigation2/blob/master/nav2_costmap_2d/src/clear_costmap_service.cpp - -Behavior Tree Nodes -=================== - -+--------------------------------------------+---------------------+----------------------------------+ -| Action Plugin Name | Creator | Description | -+============================================+=====================+==================================+ -| `Back Up Action`_ | Michael Jeronimo | Calls backup recovery action | -+--------------------------------------------+---------------------+----------------------------------+ -| `Clear Costmap Service`_ | Carl Delsey | Calls clear costmap service | -+--------------------------------------------+---------------------+----------------------------------+ -| `Compute Path to Pose Action`_ | Michael Jeronimo | Calls Nav2 planner server | -+--------------------------------------------+---------------------+----------------------------------+ -| `Follow Path Action`_ | Michael Jeronimo | Calls Nav2 controller server | -+--------------------------------------------+---------------------+----------------------------------+ -| `Navigate to Pose Action`_ | Michael Jeronimo | BT Node for other | -| | | BehaviorTree.CPP BTs to call | -| | | Navigation2 as a subtree action | -+--------------------------------------------+---------------------+----------------------------------+ -| `Reinitalize Global Localization Service`_ | Carl Delsey | Reinitialize AMCL to a new pose | -+--------------------------------------------+---------------------+----------------------------------+ -| `Spin Action`_ | Carl Delsey | Calls spin recovery action | -+--------------------------------------------+---------------------+----------------------------------+ -| `Wait Action`_ | Steve Macenski | Calls wait recovery action | -+--------------------------------------------+---------------------+----------------------------------+ - -.. _Back Up Action: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Clear Costmap Service: https://github.com/ros-planning/navigation2/blob/master/nav2_costmap_2d/src/clear_costmap_service.cpp -.. _Compute Path to Pose Action: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Follow Path Action: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Navigate to Pose Action: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Reinitalize Global Localization Service: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Spin Action: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Wait Action: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins - - -+------------------------------------+--------------------+------------------------+ -| Condition Plugin Name | Creator | Description | -+====================================+====================+========================+ -| `Goal Reached Condition`_ | Carl Delsey | Checks if goal is | -| | | reached within tol. | -+------------------------------------+--------------------+------------------------+ -| `Initial Pose received Condition`_ | Carl Delsey | Checks if initial pose | -| | | has been set | -+------------------------------------+--------------------+------------------------+ -| `Is Stuck Condition`_ | Michael Jeronimo | Checks if robot is | -| | | making progress or | -| | | stuck | -+------------------------------------+--------------------+------------------------+ -| `Transform Available Condition`_ | Steve Macenski | Checks if a TF | -| | | transformation is | -| | | available. When | -| | | succeeds returns | -| | | sucess for subsequent | -| | | calls. | -+------------------------------------+--------------------+------------------------+ - -.. _Goal Reached Condition: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Initial Pose received Condition: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Is Stuck Condition: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Transform Available Condition: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins - -+-----------------------+-------------------+----------------------------------+ -| Decorator Plugin Name | Creator | Description | -+=======================+===================+==================================+ -| `Rate Controller`_ | Michael Jeronimo | Throttles child node to a given | -| | | rate | -+-----------------------+-------------------+----------------------------------+ - -.. _Rate Controller: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins - -+-----------------------+------------------------+----------------------------------+ -| Control Plugin Name | Creator | Description | -+=======================+========================+==================================+ -| `Pipeline Sequence`_ | Carl Delsey | A variant of a sequence node that| -| | | will re-tick previous children | -| | | even if another child is running | -+-----------------------+------------------------+----------------------------------+ -| `Recovery`_ | Carl Delsey | Node must contain 2 children | -| | | and returns success if first | -| | | succeeds. If first fails, the | -| | | second will be ticked. If | -| | | successful, it will retry the | -| | | first and then return its value | -+-----------------------+------------------------+----------------------------------+ -| `Round Robin`_ | Mohammad Haghighipanah | Will tick ``i`` th child until | -| | | a result and move on to ``i+1`` | -+-----------------------+------------------------+----------------------------------+ - -.. _Pipeline Sequence: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Recovery: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins -.. _Round Robin: https://github.com/ros-planning/navigation2/tree/master/nav2_recoveries/plugins diff --git a/sphinx_doc/scripts/.nojekyll b/sphinx_doc/scripts/.nojekyll deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sphinx_doc/scripts/_nojekyll b/sphinx_doc/scripts/_nojekyll deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sphinx_doc/scripts/docker/Dockerfile b/sphinx_doc/scripts/docker/Dockerfile deleted file mode 100644 index 9b6747f68a..0000000000 --- a/sphinx_doc/scripts/docker/Dockerfile +++ /dev/null @@ -1,13 +0,0 @@ -FROM alpine:latest - -#ENV http_proxy -#ENV https_proxy - -RUN apk --no-cache add python3 -RUN python3 -m ensurepip -RUN pip3 install sphinx==1.7.5 docutils==0.14 sphinx_rtd_theme breathe==4.9.1 sphinxcontrib-plantuml -RUN apk --no-cache add make -RUN apk --no-cache add doxygen -RUN apk --no-cache add graphviz -RUN apk --no-cache add ttf-dejavu -RUN apk --no-cache add openjdk8-jre diff --git a/sphinx_doc/scripts/plantuml.jar b/sphinx_doc/scripts/plantuml.jar deleted file mode 100644 index a84c5a0fcd..0000000000 Binary files a/sphinx_doc/scripts/plantuml.jar and /dev/null differ diff --git a/sphinx_doc/scripts/publish-README.md b/sphinx_doc/scripts/publish-README.md deleted file mode 100644 index 1104856605..0000000000 --- a/sphinx_doc/scripts/publish-README.md +++ /dev/null @@ -1,6 +0,0 @@ -# Documentation Generated by SPHINX -This Documentation was programmatically generated for GitHub Pages. -Do not make changes directly in this repo. Instead, edit content -in the -docs repo, re-generate the HTML with -Sphinx (make html), and push the updated content here for publishing -(make publish). diff --git a/sphinx_doc/scripts/publish-index.html b/sphinx_doc/scripts/publish-index.html deleted file mode 100644 index 11ffbf05eb..0000000000 --- a/sphinx_doc/scripts/publish-index.html +++ /dev/null @@ -1,10 +0,0 @@ - - -SOF Project documentation - - - - -Please visit the latest SOF documentation - - diff --git a/sphinx_doc/scripts/requirements.txt b/sphinx_doc/scripts/requirements.txt deleted file mode 100644 index 939919ae87..0000000000 --- a/sphinx_doc/scripts/requirements.txt +++ /dev/null @@ -1,5 +0,0 @@ -breathe==4.9.1 -sphinx==1.7.5 -docutils==0.14 -sphinx_rtd_theme -sphinxcontrib-plantuml diff --git a/sphinx_doc/substitutions.txt b/sphinx_doc/substitutions.txt deleted file mode 100644 index dc6836feaa..0000000000 --- a/sphinx_doc/substitutions.txt +++ /dev/null @@ -1,44 +0,0 @@ -.. |br| raw:: html .. force a line break in HTML output (blank lines needed here) - -
-.. |PN| replace:: Navigation 2 - -.. |LPN| replace:: ROS 2 Navigation - -.. _project repo: https://github.com/ros-planning/navigation2 - -.. _documentation repo: https://github.com/ros-planning/navigation2/doc - -.. _nav2_bringup pkg: https://github.com/ros-planning/navigation2/tree/master/nav2_bringup/bringup - -.. _nav2_params file: https://github.com/ros-planning/navigation2/blob/master/nav2_bringup/bringup/params/nav2_params.yaml - -.. _documentation site: https://rosplanning.github.io/navigation2/ - -.. _project website: https://rosplanning.github.io/navigation2/ - -.. |PP| replace:: https://github.com/ros-planning/navigation2/blob/master - -.. |Distro| replace:: Dashing - -.. |distro| replace:: dashing - -.. _`ROS 2 binary packages`: https://index.ros.org/doc/ros2/Installation/Dashing/#binary-packages - -.. _`official Turtlebot 3 manual`: http://emanual.robotis.com/docs/en/platform/turtlebot3/ros2/#ros2 - -.. These are replacement strings for non-ASCII characters used within the project - using the same name as the html entity names (e.g., ©) for that character - -.. |copy| unicode:: U+000A9 .. COPYRIGHT SIGN - :ltrim: -.. |trade| unicode:: U+02122 .. TRADEMARK SIGN - :ltrim: -.. |reg| unicode:: U+000AE .. REGISTERED TRADEMARK SIGN - :ltrim: -.. |deg| unicode:: U+000B0 .. DEGREE SIGN - :ltrim: -.. |plusminus| unicode:: U+000B1 .. PLUS-MINUS SIGN - :rtrim: -.. |micro| unicode:: U+000B5 .. MICRO SIGN - :rtrim: diff --git a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png b/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png deleted file mode 100644 index 8c91a80d58..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png b/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png deleted file mode 100644 index ed5f5d2080..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png b/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png deleted file mode 100644 index 1cb3a59609..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png b/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png deleted file mode 100644 index 2475fa6e9c..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png b/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png deleted file mode 100644 index 42fa031eb8..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_explanation.png b/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_explanation.png deleted file mode 100644 index c498429765..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_explanation.png and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_layer_preview.gif b/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_layer_preview.gif deleted file mode 100644 index 289e38207f..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_layer_preview.gif and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_layer_run.png b/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_layer_run.png deleted file mode 100644 index 900d5a9c57..0000000000 Binary files a/sphinx_doc/tutorials/docs/images/Writing_new_Costmap2D_plugin/gradient_layer_run.png and /dev/null differ diff --git a/sphinx_doc/tutorials/docs/navigation2_on_real_turtlebot3.rst b/sphinx_doc/tutorials/docs/navigation2_on_real_turtlebot3.rst deleted file mode 100644 index ebbe0cbf59..0000000000 --- a/sphinx_doc/tutorials/docs/navigation2_on_real_turtlebot3.rst +++ /dev/null @@ -1,132 +0,0 @@ -.. _navigation2-on-real-turtlebot3: - -Navigating with a Physical Turtlebot 3 -************************************** - -- `Overview`_ -- `Requirements`_ -- `Tutorial Steps`_ - -.. raw:: html - -

-
- -
-

- -Overview -======== - -This tutorial shows how to control and navigate Turtlebot 3 using the ROS2 Navigation2 on a physical Turtlebot 3 robot. -Before completing this tutorials, completing :ref:`getting_started` is highly recommended especially if you are new to ROS and Navigation2. - -``ROS2 Dashing`` and ``Navigation2 Dashing 0.2.4`` are used to create this tutorial. -You should be able to do this tutorial using other Navigation2 versions as well. - -This tutorial may take about 1 hour to complete. -It depends on your experience with ROS, robots, and what computer system you have. - -Requirements -============ - -You must install Navigation2, Turtlebot3. -If you don't have them installed, please follow :ref:`getting_started`. - -Tutorial Steps -============== - -0- Setup Your Enviroment Variables ----------------------------------- - -Run the following commands first whenever you open a new terminal during this tutorial. - -- ``source /opt/ros//setup.bash`` -- ``export TURTLEBOT3_MODEL=waffle`` - -1- Launch Turtlebot 3 ---------------------- - -You will need to launch your robot's interface, - - ``ros2 launch turtlebot3_bringup robot.launch.py use_sim_time:=False`` - -2- Launch Navigation2 ---------------------- - -You need to have a map of the environment where you want to Navigate Turtlebot 3, or create one live with SLAM. - -In case you are interested, there is a use case tutorial which shows how to use Navigation2 with SLAM. -`Navigation2 with SLAM `_ - -Required files: - - - ``your-map.map`` - - ``your-map.yaml`` - -``.yaml`` is the configuration file for the map we want to provide Navigation2. -In this case, it has the map resolution value, threshold values for obstacles and free spaces, and a map file location. -You need to make sure these values are correct. -More information about the map.yaml can be found `here `_. - -Launch Navigation 2. If you set autostart:=False, you need to click on the start button in RViz to initialize the nodes. -Make sure `use_sim time` is set to **False**, because we want to use the system time instead of the time simulation time from Gazebo. - -``ros2 launch nav2_bringup bringup_launch.py use_sim_time:=False autostart:=False map:=/path/to/your-map.yaml`` - -Note: Don't forget to change **/path/to/your-map.yaml** to the actual path to the your-map.yaml file. - -3- Launch RVIZ ---------------- - -Launch RVIZ with a pre-defined configuration file. - - ``ros2 run rviz2 rviz2 -d $(ros2 pkg prefix nav2_bringup)/share/nav2_bringup/launch/nav2_default_view.rviz`` - -Now, you should see a shadow of Turtlebot 3 robot model in the center of the plot in Rviz. -Click on the Start button (Bottom Left) if you set the auto_start parameter to false. -Then, the map should appear in RViz. - -.. image:: images/Navigation2_on_real_Turtlebot3/rviz_after_launch_view.png - :width: 48% -.. image:: images/Navigation2_on_real_Turtlebot3/rviz_slam_map_view.png - :width: 45% - -4- Initialize the Location of Turtlebot 3 ------------------------------------------ - -First, find where the robot is on the map. Check where your robot is in the room. - -Set the pose of the robot in RViz. -Click on the 2D Pose Estimate button and point the location of the robot on the map. -The direction of the green arrow is the orientation of Turtlebot. - -.. image:: images/Navigation2_on_real_Turtlebot3/rviz_set_initial_pose.png - :width: 700px - :align: center - :alt: Set initial pose in RViz - -Now, the 3D model of Turtlebot should move to that location. -A small error in the estimated location is tolerable. - -5- Send a Goal Pose --------------------- - -Pick a target location for Turtlebot on the map. -You can send Turtlebot 3 a goal position and a goal orientation by using the **Navigation2 Goal** or the **GoalTool** buttons. - -Note: Navigation2 Goal button uses a ROS2 Action to send the goal and the GoalTool publishes the goal to a topic. - -.. image:: images/Navigation2_on_real_Turtlebot3/rviz_send_goal.png - :width: 700px - :align: center - :alt: Send goal pose in RViz - -Once you define the target pose, Navigation2 will find a global path and start navigating the robot on the map. - -.. image:: images/Navigation2_on_real_Turtlebot3/rviz_robot_navigating.png - :width: 700px - :align: center - :alt: Robot navigating in RViz - -Now, you can see that Turtlebot 3 moves towards the goal position in the room. See the video below. diff --git a/sphinx_doc/tutorials/docs/navigation2_with_slam.rst b/sphinx_doc/tutorials/docs/navigation2_with_slam.rst deleted file mode 100644 index 7ccc4d25d8..0000000000 --- a/sphinx_doc/tutorials/docs/navigation2_with_slam.rst +++ /dev/null @@ -1,86 +0,0 @@ -.. _navigation2-with-slam: - -(SLAM) Navigating While Mapping -******************************* - -- `Overview`_ -- `Requirements`_ -- `Tutorial Steps`_ - -Overview -======== - -This document explains how to use Navigation 2 with SLAM. -The following steps show ROS 2 users how to generate occupancy grid maps and use Navigation 2 to move their robot around. -This tutorial applies to both simulated and physical robots, but will be completed here on physical robot. - -Before completing this tutorial, completing the :ref:`getting_started`. is highly recommended especially if you are new to ROS and Navigation2. - - -In this tutorial we'll be using SLAM Toolbox. More information can be found in the `ROSCon talk for SLAM Toolbox `_ - -Requirements -============ - -You must install Navigation2, Turtlebot3, and SLAM Toolbox. -If you don't have them installed, please follow :ref:`getting_started`. - -SLAM Toolbox can be installed via: - - ``sudo apt install ros-dashing-slam-toolbox`` - -or from built from source in your workspace with: - - ``git clone -b dashing-devel git@github.com:stevemacenski/slam_toolbox.git`` - - -Tutorial Steps -============== - -0- Launch Robot Interfaces --------------------------- - -For this tutorial, we will use the turtlebot3. -If you have another robot, replace with suitable instructions. - -Run the following commands first whenever you open a new terminal during this tutorial. - -- ``source /opt/ros//setup.bash`` -- ``export TURTLEBOT3_MODEL=waffle`` - - -Launch your robot's interface and robot state publisher, - - ``ros2 launch turtlebot3_bringup robot.launch.py`` - -1- Launch Navigation2 ---------------------- - -Launch Navigation without nav2_amcl and nav2_map_server. -It is assumed that the SLAM node(s) will publish to /map topic and provide the map->odom transform. - - ``ros2 launch nav2_bringup nav2_navigation_launch.py`` - -2- Launch SLAM --------------- - -Bring up your choice of SLAM implementation. -Make sure it provides the map->odom transform and /map topic. -Run Rviz and add the topics you want to visualize such as /map, /tf, /laserscan etc. -For this tutorial, we will use `SLAM Toolbox `_. - - - ``ros2 launch slam_toolbox online_async_launch.py`` - -3- Working with SLAM --------------------- - -Move your robot by requesting a goal through RViz or the ROS2 CLI, ie: - -.. code-block:: bash - - ros2 topic pub /goal_pose geometry_msgs/PoseStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {position: {x: 0.2, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}" - -You should see the map update live! To save this map to file: - - ``ros2 run nav2_map_server map_saver -f ~/map`` diff --git a/sphinx_doc/tutorials/docs/navigation2_with_stvl.rst b/sphinx_doc/tutorials/docs/navigation2_with_stvl.rst deleted file mode 100644 index 5ab293a8ad..0000000000 --- a/sphinx_doc/tutorials/docs/navigation2_with_stvl.rst +++ /dev/null @@ -1,147 +0,0 @@ -.. _stvl: - -(STVL) Using an External Costmap Plugin -*************************************** - -- `Overview`_ -- `Costmap2D and STVL`_ -- `Tutorial Steps`_ - -.. raw:: html - -

-
- -
-

- -Overview -======== - -This tutorial shows how to load and use an external plugin. -This example uses the `Spatio Temporal Voxel Layer `_ (STVL) costmap `pluginlib `_ plugin as an example. -STVL is a demonstrative pluginlib plugin and the same process can be followed for other costmap plugins as well as plugin planners, controllers, and recoveries. - -Before completing this tutorial, please look at the previous two tutorials on navigation in simulation and physical hardware, if available. -This tutorial assumes knowledge of navigation and basic understanding of costmaps. - -Costmap2D and STVL -================== - -Costmap 2D is the data object we use to buffer sensor information into a global view that the robot will use to create plans and control efforts. -Within Costmap2D, there are pluginlib plugin interfaces available to create custom behaviors loadable at runtime. -Examples of included pluginlib plugins for Costmap2D are the Obstacle Layer, Voxel Layer, Static Layer, and Inflation Layer. - -However, these are simply example plugins offered by the base implementation. -Another available pluginlib plugin for Costmap2D in Navigation2 is STVL. - -STVL is another 3D perception plugin similar to the Voxel Layer. -A more detailed overview of how it works can be found `in this repo `_, however it buffers 3D data from depth cameras, sonars, lidars, and more into a sparse volumetic world model and removes voxels over time proportional with a sensor model and time-based expiration. -This can be especially useful for robots in highly dynamic envrionments and decreases the resource utilization for 3D sensor processing by up to 2x. -STVL also treats 3D lidars and radars as first class citizens for support. -The ROSCon talk for STVL can be found `in this video `_. - -Tutorial Steps -============== - -0- Setup --------- - -Follow the same process as in :ref:`getting_started` for installing and setting up a robot for hardware testing or simulation, as applicable. Ensure ROS2, Navigation2, and Gazebo are installed. - -1- Install STVL ---------------- - -STVL can be installed in ROS2 Dashing and Eloquent via the ROS Build Farm: - -- ``sudo apt install ros--spatio-temporal-voxel-layer`` - -It can also be built from source by cloning the repository into your Navigation2 workspace: - -- ``git clone -b -devel git@github.com:stevemacenski/spatio_temporal_voxel_layer`` - -1- Modify Navigation2 Parameter -------------------------------- - -STVL is an optional plugin, like all plugins, in Costmap2D. Costmap Plugins in Navigation2 are loaded in the ``plugin_names`` and ``plugin_types`` variables inside of their respective costmaps. -For example, the following will load the static and obstacle layer plugins into the name ``static_layer`` and ``obstacle_layer``, respectively: - -.. code-block:: yaml - - global_costmap: - global_costmap: - ros__parameters: - use_sim_time: True - plugin_names: ["static_layer", "obstacle_layer"] - plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer"] - -To load the STVL plugin, a new plugin name and type must be added. -For example, if the application required an STVL layer and no obstacle layer, our file would be: - -.. code-block:: yaml - - global_costmap: - global_costmap: - ros__parameters: - use_sim_time: True - plugin_names: ["static_layer", "stvl_layer"] - plugin_types: ["nav2_costmap_2d::StaticLayer", "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"] - -Similar to the Voxel Layer, after registering the plugin, we can add the configuration of the STVL layer under the namespace ``stvl_layer``. -An example fully-described parameterization of an STVL configuration is: - -.. code-block:: yaml - - stvl_layer: - enabled: true - voxel_decay: 15. - decay_model: 0 - voxel_size: 0.05 - track_unknown_space: true - max_obstacle_height: 2.0 - unknown_threshold: 15 - mark_threshold: 0 - update_footprint_enabled: true - combination_method: 1 - obstacle_range: 3.0 - origin_z: 0.0 - publish_voxel_map: true - transform_tolerance: 0.2 - mapping_mode: false - map_save_duration: 60.0 - observation_sources: pointcloud - pointcloud: - data_type: PointCloud2 - topic: /intel_realsense_r200_depth/points - marking: true - clearing: true - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - expected_update_rate: 0.0 - observation_persistence: 0.0 - inf_is_valid: false - voxel_filter: false - clear_after_reading: true - max_z: 7.0 - min_z: 0.1 - vertical_fov_angle: 0.8745 - horizontal_fov_angle: 1.048 - decay_acceleration: 15.0 - model_type: 0 - -Please copy-paste the text above, with the ``plugin_names`` and ``plugin_types`` registration, into your ``nav2_params.yaml`` to enable STVL in your application. -Make sure to change both the local and global costmaps. - -Note: Pluginlib plugins for other Navigation2 servers such as planning, recovery, and control can be set in this same way. - -2- Launch Navigation2 ---------------------- - -Follow the same process as in :ref:`getting_started` to launch a simulated robot in gazebo with Navigation2. -Navigation2 is now using STVL as its 3D sensing costmap layer. - -3- RVIZ --------- - -With RViz open and ``publish_voxel_map: true``, you can visualize the underlying data structure's 3D grid using the ``{local, global}_costmap/voxel_grid`` topics. -Note: It is recommended in RViz to set the ``PointCloud2`` Size to your voxel size and the style to ``Boxes`` with a neutral color for best visualization. diff --git a/sphinx_doc/tutorials/docs/writing_new_costmap2d_plugin.rst b/sphinx_doc/tutorials/docs/writing_new_costmap2d_plugin.rst deleted file mode 100644 index b5a44d165d..0000000000 --- a/sphinx_doc/tutorials/docs/writing_new_costmap2d_plugin.rst +++ /dev/null @@ -1,247 +0,0 @@ -.. _writing_new_costmap2d_plugin: - -Writing a new Costmap2D plugin -****************************** - -- `Overview`_ -- `Requirements`_ -- `Tutorial Steps`_ - -.. image:: images/Writing_new_Costmap2D_plugin/gradient_layer_preview.gif - :width: 700px - :align: center - :alt: Animated gif with gradient demo - -Overview -======== - -This tutorial shows how to create your own simple `plugin `_ for Costmap2D. - -Before starting the tutorial, please check this `video `_ which contains information about Costmap2D layers design and plugins basic operational principals. - -Requirements -============ - -It is assumed that ROS2, Gazebo and TurtleBot3 packages are installed or built locally. Please make sure that Navigation2 project is also built locally as it was made in :ref:`build-instructions`. - -Tutorial Steps -============== - -1- Write a new Costmap2D plugin -------------------------------- - -For a demonstration, this example will creates a costmap plugin that puts repeating costs gradients in the costmap. -The annotated code for this tutorial can be found in `navigation2_tutorials `_ repository as the ``nav2_gradient_costmap_plugin`` ROS2-package. -Please refer to it when making your own layer plugin for Costmap2D. - -The plugin class ``nav2_gradient_costmap_plugin::GradientLayer`` is inherited from basic class ``nav2_costmap_2d::Layer``: - -.. code-block:: c - - namespace nav2_gradient_costmap_plugin - { - - class GradientLayer : public nav2_costmap_2d::Layer - -The basic class provides the set of virtual methods API for working with costmap layers in a plugin. These methods are calling in runtime by ``LayeredCostmap``. The list of methods, their description and necessity to have these methods in plugin's code is presented in the table below: - -+----------------------+----------------------------------------------------------------------------+-------------------------+ -| **Virtual method** | **Method description** | **Requires override?** | -+----------------------+----------------------------------------------------------------------------+-------------------------+ -| onInitialize() | Method is called at the end of plugin initialization. There is usually | No | -| | declarations of ROS parameters. This is where any required initialization | | -| | should occur. | | -+----------------------+----------------------------------------------------------------------------+-------------------------+ -| updateBounds() | Method is called to ask the plugin: which area of costmap layer it needs | Yes | -| | to update. There are 3 input parameters of method: robot position and | | -| | orientation and 4 output parameters: pointers to window bounds. | | -| | These bounds are used for performance reasons: to update the area | | -| | inside the window where is new info available, avoiding updates of whole | | -| | costmap on every iteration. | | -+----------------------+----------------------------------------------------------------------------+-------------------------+ -| updateCosts() | Method is called each time when costmap re-calculation is required. It | Yes | -| | updates the costmap layer only within its bounds window. There are 4 input | | -| | parameters of method: calculation window bounds and 1 output parameter: | | -| | reference to a resulting costmap ``master_grid``. The ``Layer`` class | | -| | provides the plugin with an internal costmap, ``costmap_``, for updates. | | -| | The ``master_grid`` should be updated with values within the window bounds | | -| | using one of the following update methods: ``updateWithAddition()``, | | -| | ``updateWithMax()``, ``updateWithOverwrite()`` or | | -| | ``updateWithTrueOverwrite()``. | | -+----------------------+----------------------------------------------------------------------------+-------------------------+ -| matchSize() | Method is called each time when map size was changed. | No | -+----------------------+----------------------------------------------------------------------------+-------------------------+ -| onFootprintChanged() | Method is called each time when footprint was changed. | No | -+----------------------+----------------------------------------------------------------------------+-------------------------+ -| reset() | It may have any code to be executed during costmap reset. | Yes | -+----------------------+----------------------------------------------------------------------------+-------------------------+ - -In our example these methods have the following functionality: - -1. ``GradientLayer::onInitialize()`` contains declaration of ROS parameter with its default value: - -.. code-block:: c - - declareParameter("enabled", rclcpp::ParameterValue(true)); - node_->get_parameter(name_ + "." + "enabled", enabled_); - -and sets ``need_recalculation_`` bounds recalculation indicator: - -.. code-block:: c - - need_recalculation_ = false; - -2. ``GradientLayer::updateBounds()`` re-calculates window bounds if ``need_recalculation_`` is ``true`` and updates them regardless of ``need_recalculation_`` value. - -3. ``GradientLayer::updateCosts()`` - in this method the gradient is writing directly to the resulting costmap ``master_grid`` without merging with previous layers. This is equal to working with internal ``costmap_`` and then calling ``updateWithTrueOverwrite()`` method. Here is the gradient making algorithm for master costmap: - -.. code-block:: c - - int gradient_index; - for (int j = min_j; j < max_j; j++) { - // Reset gradient_index each time when reaching the end of re-calculated window - // by OY axis. - gradient_index = 0; - for (int i = min_i; i < max_i; i++) { - int index = master_grid.getIndex(i, j); - // setting the gradient cost - unsigned char cost = (LETHAL_OBSTACLE - gradient_index*GRADIENT_FACTOR)%255; - if (gradient_index <= GRADIENT_SIZE) { - gradient_index++; - } else { - gradient_index = 0; - } - master_array[index] = cost; - } - } - -where the ``GRADIENT_SIZE`` is the size of each gradient period in map cells, ``GRADIENT_FACTOR`` - decrement of costmap's value per each step: - -.. image:: images/Writing_new_Costmap2D_plugin/gradient_explanation.png - -These parameters are defined in plugin's header file. - -4. ``GradientLayer::onFootprintChanged()`` just resets ``need_recalculation_`` value. - -5. ``GradientLayer::reset()`` method is dummy: it is not used in this example plugin. It remaining there since pure virtual function ``reset()`` in parent ``Layer`` class required to be overriden. - -2- Export and make GradientLayer plugin ---------------------------------------- - -The written plugin will be loaded in runtime as it's basic parent class and then will be called by plugin handling modules (for costmap2d by ``LayeredCostmap``). Pluginlib opens a given plugin in run-time and provides methods from exported classes to be callable. The mechanism of class exporting tells pluginlib which basic class should be used during these calls. This allows to extend an application by plugins without knowing application source code or recompiling it. - -In our example the ``nav2_gradient_costmap_plugin::GradientLayer`` plugin's class should be dynamically loaded as a ``nav2_costmap_2d::Layer`` basic class. For this the plugin should be registered as follows: - -1. Plugin's class should be registered with a basic type of loaded class. For this there is a special macro ``PLUGINLIB_EXPORT_CLASS`` should be added to any source-file composing the plugin library: - -.. code-block:: text - - #include "pluginlib/class_list_macros.hpp" - PLUGINLIB_EXPORT_CLASS(nav2_gradient_costmap_plugin::GradientLayer, nav2_costmap_2d::Layer) - -This part is usually placed at the end of cpp-file where the plugin class was written (in our example ``gradient_layer.cpp``). - -2. Plugin's inormation should be stored to plugin description file. This is done by using separate XML (in our example ``gradient_plugins.xml``) in the plugin's package. This file contains information about: - - - ``path``: Path and name of library where plugin is placed. - - ``name``: Plugin type referenced in ``plugin_types`` parameter (see next section for more details). It could be whatever you want. - - ``type``: Plugin class with namespace taken from the source code. - - ``basic_class_type``: Basic parent class from which plugin class was derived. - - ``description``: Plugin description in a text form. - -.. code-block:: xml - - - - This is an example plugin which puts repeating costs gradients to costmap - - - -The export of plugin is performed by including ``pluginlib_export_plugin_description_file()`` cmake-function into ``CMakeLists.txt``. This function installs plugin description file into ``share`` directory and sets ament indexes for plugin description XML to be discoverable as a plugin of selected type: - -.. code-block:: text - - pluginlib_export_plugin_description_file(nav2_costmap_2d gradient_layer.xml) - -Plugin description file is also should be added to ``package.xml``. ``costmap_2d`` is the package of the interface definition, for our case ``Layer``, and requires a path to the xml file: - -.. code-block:: text - - - - ... - - -After everything is done put the plugin package into ``src`` directory of a certain ROS2-workspace, build the plugin package (``colcon build --packages-select nav2_gradient_costmap_plugin --symlink-install``) and source ``setup.bash`` file when it necessary. - -Now the plugin is ready to use. - -3- Enable the plugin in Costmap2D ---------------------------------- - -At the next step it is required to tell Costmap2D about new plugin. For that the plugin should be added to ``plugin_names`` and ``plugin_types`` lists in ``nav2_params.yaml`` optionally for ``local_costmap``/``global_costmap`` in order to be enabled in run-time for Controller/Planner Server. ``plugin_names`` list contains the names of plugin objects. These names could be anything you want. ``plugin_types`` contains types of listed in ``plugin_names`` objects. These types should correspond to ``name`` field of plugin class specified in plugin description XML-file. - -For example: - -.. code-block:: diff - - --- a/nav2_bringup/bringup/params/nav2_params.yaml - +++ b/nav2_bringup/bringup/params/nav2_params.yaml - @@ -124,8 +124,8 @@ local_costmap: - width: 3 - height: 3 - resolution: 0.05 - - plugin_names: ["obstacle_layer", "voxel_layer", "inflation_layer"] - - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] - + plugin_names: ["obstacle_layer", "voxel_layer", "gradient_layer"] - + plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_gradient_costmap_plugin/GradientLayer"] - robot_radius: 0.22 - inflation_layer: - cost_scaling_factor: 3.0 - @@ -171,8 +171,8 @@ global_costmap: - robot_base_frame: base_link - global_frame: map - use_sim_time: True - - plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"] - - plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"] - + plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "gradient_layer"] - + plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_gradient_costmap_plugin/GradientLayer"] - robot_radius: 0.22 - resolution: 0.05 - obstacle_layer: - -YAML-file may also contain the list of parameters (if any) for each plugin, identified by plugins object name. - -NOTE: there could be many simultaneously loaded plugin objects of one type. For this, ``plugin_names`` list should contain different plugins names whether the ``plugin_types`` will remain the same types. For example: - -.. code-block:: text - - plugin_names: ["obstacle_layer", "gradient_layer_1", "gradient_layer_2"] - plugin_types: ["nav2_costmap_2d::ObstacleLayer", "nav2_gradient_costmap_plugin/GradientLayer", "nav2_gradient_costmap_plugin/GradientLayer"] - -In this case each plugin object will be handled by its own parameters tree in a YAML-file, like: - -.. code-block:: text - - gradient_layer_1: - enabled: True - ... - gradient_layer_2: - enabled: False - ... - -4- Run GradientLayer plugin ---------------------------- - -Run Turtlebot3 simulation with enabled navigation2. Detailed instructuction how to make it are written at :ref:`getting_started`. Below is shortcut command for that: - -.. code-block:: bash - - $ ros2 launch nav2_bringup tb3_simulation_launch.py - -Then goto RViz and click on the "2D Pose Estimate" button at the top and point the location on map as it was described in :ref:`getting_started`. Robot will be localized on map and the result should be as presented at picture below. There is could be seen the gradient costmap. There are also 2 noticeable things: dynamically updated by ``GradientLayer::updateCosts()`` costmap within its bounds and global path curved by gradient: - -.. image:: images/Writing_new_Costmap2D_plugin/gradient_layer_run.png - :width: 700px - :align: center - :alt: Image of gradient costmap used diff --git a/sphinx_doc/tutorials/index.rst b/sphinx_doc/tutorials/index.rst deleted file mode 100644 index 2496af84f0..0000000000 --- a/sphinx_doc/tutorials/index.rst +++ /dev/null @@ -1,14 +0,0 @@ -.. _tutorials: - -Tutorials -######### - -Navigation2 Tutorials - -.. toctree:: - :maxdepth: 1 - - docs/navigation2_on_real_turtlebot3.rst - docs/navigation2_with_slam.rst - docs/navigation2_with_stvl.rst - docs/writing_new_costmap2d_plugin.rst diff --git a/tools/bt2img.py b/tools/bt2img.py index 1092b047f4..5c2c06057e 100755 --- a/tools/bt2img.py +++ b/tools/bt2img.py @@ -37,6 +37,7 @@ "Subtree", "Timeout", "RateController", + "DistanceController", "RecoveryNode", "PipelineSequence", "RoundRobin" @@ -57,6 +58,7 @@ "IsStuck", "GoalReached", "initialPoseReceived", + "GoalUpdated", ] def main(): diff --git a/tools/build_all.sh b/tools/build_all.sh index 2a69fe1f3a..d592fc929b 100755 --- a/tools/build_all.sh +++ b/tools/build_all.sh @@ -7,11 +7,11 @@ # set at the time we start the last step, the ros1_bridge build. if [ "$ROS2_DISTRO" = "" ]; then - export ROS2_DISTRO=dashing + export ROS2_DISTRO=eloquent fi -if [ "$ROS2_DISTRO" != "dashing" ]; then - echo "ROS2_DISTRO variable must be set to dashing" - exit 1 +if [ "$ROS2_DISTRO" != "eloquent" ]; then + echo "ROS2_DISTRO variable must be set to eloquent" + exit 1 fi set -e diff --git a/tools/code_coverage_report.bash b/tools/code_coverage_report.bash index b0530c9f16..3867927bf9 100755 --- a/tools/code_coverage_report.bash +++ b/tools/code_coverage_report.bash @@ -43,20 +43,20 @@ mkdir -p $LCOVDIR lcov --capture --initial \ --directory build \ --output-file ${LCOVDIR}/initial_coverage.info \ - --rc lcov_branch_coverage=1 + --rc lcov_branch_coverage=0 # Capture executed code data. lcov --capture \ --directory build \ --output-file ${LCOVDIR}/test_coverage.info \ - --rc lcov_branch_coverage=1 + --rc lcov_branch_coverage=0 # Combine the initial zero-coverage report with the executed lines report. lcov \ --add-tracefile ${LCOVDIR}/initial_coverage.info \ --add-tracefile ${LCOVDIR}/test_coverage.info \ --output-file ${LCOVDIR}/full_coverage.info \ - --rc lcov_branch_coverage=1 + --rc lcov_branch_coverage=0 # Only include files that are within this workspace. # (eg filter out stdio.h etc) @@ -64,7 +64,7 @@ lcov \ --extract ${LCOVDIR}/full_coverage.info \ "${PWD}/*" \ --output-file ${LCOVDIR}/workspace_coverage.info \ - --rc lcov_branch_coverage=1 + --rc lcov_branch_coverage=0 # Remove files in the build subdirectory. # Those are generated files (like messages, services, etc) @@ -81,7 +81,7 @@ lcov \ --remove ${LCOVDIR}/workspace_coverage.info \ "${PWD}/*/nav2_system_tests/*" \ --output-file ${LCOVDIR}/project_coverage.info \ - --rc lcov_branch_coverage=1 + --rc lcov_branch_coverage=0 if [ $COVERAGE_REPORT_VIEW = codecovio ]; then bash <(curl -s https://codecov.io/bash) \ @@ -89,6 +89,5 @@ if [ $COVERAGE_REPORT_VIEW = codecovio ]; then -R src/navigation2 elif [ $COVERAGE_REPORT_VIEW = genhtml ]; then genhtml ${LCOVDIR}/project_coverage.info \ - --output-directory ${LCOVDIR}/html \ - --branch-coverage -p ${PWD} + --output-directory ${LCOVDIR}/html fi diff --git a/tools/initial_ros_setup.sh b/tools/initial_ros_setup.sh index 64f9dd4d53..b64e799d39 100755 --- a/tools/initial_ros_setup.sh +++ b/tools/initial_ros_setup.sh @@ -4,10 +4,10 @@ ENABLE_BUILD=true ENABLE_ROS2=true if [ "$ROS2_DISTRO" = "" ]; then - export ROS2_DISTRO=dashing + export ROS2_DISTRO=eloquent fi -if [ "$ROS2_DISTRO" != "dashing" ]; then - echo "ROS2_DISTRO variable must be set to dashing" +if [ "$ROS2_DISTRO" != "eloquent" ]; then + echo "ROS2_DISTRO variable must be set to eloquent" exit 1 fi @@ -24,7 +24,7 @@ for opt in "$@" ; do *) echo "Invalid option: $opt" echo "Valid options:" - echo "--no-ros2 Uses the binary distribution of ROS2 dashing" + echo "--no-ros2 Uses the binary distribution of ROS2 eloquent" echo "--download-only Skips the build step and only downloads the code" exit 1 ;; diff --git a/tools/release.Dockerfile b/tools/release.Dockerfile new file mode 100644 index 0000000000..2b89b53400 --- /dev/null +++ b/tools/release.Dockerfile @@ -0,0 +1,102 @@ +# syntax=docker/dockerfile:experimental + +# Use experimental buildkit for faster builds +# https://github.com/moby/buildkit/blob/master/frontend/dockerfile/docs/experimental.md +# Use `--progress=plain` to use plane stdout for docker build +# +# Example build command: +# export DOCKER_BUILDKIT=1 +# export FROM_IMAGE="ros:eloquent" +# export OVERLAY_MIXINS="release ccache" +# docker build -t nav2:release_branch \ +# --build-arg FROM_IMAGE \ +# --build-arg OVERLAY_MIXINS \ +# -f release_branch.Dockerfile ./ + +ARG FROM_IMAGE=ros:eloquent +ARG OVERLAY_WS=/opt/overlay_ws + +# multi-stage for caching +FROM $FROM_IMAGE AS cacher + +# copy overlay source +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS/src +RUN echo "\ +repositories: \n\ + ros-planning/navigation2: \n\ + type: git \n\ + url: https://github.com/ros-planning/navigation2.git \n\ + version: ${ROS_DISTRO}-devel \n\ +" > ../overlay.repos +RUN vcs import ./ < ../overlay.repos && \ + find ./ -name ".git" | xargs rm -rf +# COPY ./ ./ros-planning/navigation2 + +# copy manifests for caching +WORKDIR /opt +RUN mkdir -p /tmp/opt && \ + find ./ -name "package.xml" | \ + xargs cp --parents -t /tmp/opt && \ + find ./ -name "COLCON_IGNORE" | \ + xargs cp --parents -t /tmp/opt || true + +# multi-stage for building +FROM $FROM_IMAGE AS builder + +# edit apt for caching +RUN cp /etc/apt/apt.conf.d/docker-clean /etc/apt/ && \ + echo 'Binary::apt::APT::Keep-Downloaded-Packages "true";' \ + > /etc/apt/apt.conf.d/docker-clean + +# install CI dependencies +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + apt-get update && apt-get install -q -y \ + ccache \ + lcov \ + && rosdep update + +# install overlay dependencies +ARG OVERLAY_WS +WORKDIR $OVERLAY_WS +COPY --from=cacher /tmp/$OVERLAY_WS/src ./src +RUN --mount=type=cache,target=/var/cache/apt \ + --mount=type=cache,target=/var/lib/apt \ + . /opt/ros/$ROS_DISTRO/setup.sh && \ + apt-get update && rosdep install -q -y \ + --from-paths src \ + --ignore-src \ + && rm -rf /var/lib/apt/lists/* + +# build overlay source +COPY --from=cacher $OVERLAY_WS/src ./src +ARG OVERLAY_MIXINS="release ccache" +RUN --mount=type=cache,target=/root/.ccache \ + . /opt/ros/$ROS_DISTRO/setup.sh && \ + colcon build \ + --symlink-install \ + --mixin $OVERLAY_MIXINS + +# restore apt for docker +RUN mv /etc/apt/docker-clean /etc/apt/apt.conf.d/ && \ + rm -rf /var/lib/apt/lists/ + +# source overlay from entrypoint +ENV OVERLAY_WS $OVERLAY_WS +RUN sed --in-place \ + 's|^source .*|source "$OVERLAY_WS/install/setup.bash"|' \ + /ros_entrypoint.sh + +# ARG RUN_TESTS +# ARG FAIL_ON_TEST_FAILURE +# RUN if [ -z "$RUN_TESTS" ]; then \ +# colcon test \ +# --mixin $OVERLAY_MIXINS \ +# --ctest-args --test-regex "test_.*"; \ +# if [ -z "$FAIL_ON_TEST_FAILURE" ]; then \ +# colcon test-result; \ +# else \ +# colcon test-result || true; \ +# fi \ +# fi diff --git a/tools/ros2_dependencies.repos b/tools/ros2_dependencies.repos index d1df3d43b8..b6645197b0 100644 --- a/tools/ros2_dependencies.repos +++ b/tools/ros2_dependencies.repos @@ -1,21 +1,21 @@ repositories: - BehaviorTree.CPP: + BehaviorTree/BehaviorTree.CPP: type: git url: https://github.com/BehaviorTree/BehaviorTree.CPP.git - version: ros2-3.1.1 - angles: + version: master + ros/angles: type: git url: https://github.com/ros/angles.git version: ros2 - gazebo_ros_pkgs: + ros-simulation/gazebo_ros_pkgs: type: git url: https://github.com/ros-simulation/gazebo_ros_pkgs.git version: ros2 - image_common: + ros-perception/image_common: type: git url: https://github.com/ros-perception/image_common.git version: ros2 - vision_opencv: + ros-perception/vision_opencv: type: git url: https://github.com/ros-perception/vision_opencv.git version: ros2 diff --git a/full_ros_build.Dockerfile b/tools/source.Dockerfile similarity index 100% rename from full_ros_build.Dockerfile rename to tools/source.Dockerfile