From cc5076f0bf1105200614144c8197afb88bf582f1 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 09:28:29 +0200 Subject: [PATCH 01/60] added drone yml file --- .drone.yml | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 .drone.yml diff --git a/.drone.yml b/.drone.yml new file mode 100644 index 00000000000..b8e7c1c54fd --- /dev/null +++ b/.drone.yml @@ -0,0 +1,57 @@ +--- +# This pipeline chain would be used on each open pull request to build new ml_all docker image and validate unit tests +kind: pipeline +name: PR validation build + +platform: + arch: amd64 + os: linux + +clone: + disable: true + +steps: +- name: Build ml_all docker image for pull request + image: plugins/docker + commands: + - git clone https://github.com/logivations/deep_cv.git + - git checkout ${DRONE_PULL_REQUEST} + volumes: + - name: dockersock + path: /var/run/docker.sock + settings: + dockerfile: deep_cv/docker/MlAllDockerfile + context: deep_cv/docker/ + registry: quay.io + repo: quay.io/logivations/ml_all + privileged: true + tags: + - nav2_${DRONE_PULL_REQUEST} + username: + from_secret: DOCKER_QUAY_USERNAME + password: + from_secret: DOCKER_QUAY_PASSWORD + +- name: unit-tests validation + image: quay.io/logivations/ml_all:latest + commands: + - pwd + - cd /data/workspace && git clone https://github.com/logivations/deep_cv.git + - cd /data/workspace/deep_cv && pytest -v --color=yes --forked -n 2 --ignore-glob=**/disabled_tests/* lv/ + environment: + NVIDIA_VISIBLE_DEVICES: all + depends_on: + - Build ml_all docker image for pull request + +trigger: + event: + include: + - pull_request + +volumes: +- name: dockersock + host: + path: /var/run/docker.sock + +image_pull_secrets: +- dockerconfig_prd \ No newline at end of file From b43c96814cba0d3dfabb69f57f5bfc28d79b4305 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 09:39:48 +0200 Subject: [PATCH 02/60] more updates to drone yaml file --- .drone.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.drone.yml b/.drone.yml index b8e7c1c54fd..6777de137f3 100644 --- a/.drone.yml +++ b/.drone.yml @@ -14,8 +14,8 @@ steps: - name: Build ml_all docker image for pull request image: plugins/docker commands: - - git clone https://github.com/logivations/deep_cv.git - - git checkout ${DRONE_PULL_REQUEST} + - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} + - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile volumes: - name: dockersock path: /var/run/docker.sock From d81421f2fe8f5949a546b3e76c0f773c17caf55e Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 09:58:20 +0200 Subject: [PATCH 03/60] updates to drone checkout logic --- .drone.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index 6777de137f3..e2763e97f62 100644 --- a/.drone.yml +++ b/.drone.yml @@ -14,7 +14,8 @@ steps: - name: Build ml_all docker image for pull request image: plugins/docker commands: - - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} + - pwd + - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile volumes: - name: dockersock From 989a760847e20e71132d3895c269ae0dbb36c185 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:00:18 +0200 Subject: [PATCH 04/60] triggering drone --- .drone.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index e2763e97f62..4dc23e103fd 100644 --- a/.drone.yml +++ b/.drone.yml @@ -36,7 +36,7 @@ steps: - name: unit-tests validation image: quay.io/logivations/ml_all:latest commands: - - pwd + - pwd && ls - cd /data/workspace && git clone https://github.com/logivations/deep_cv.git - cd /data/workspace/deep_cv && pytest -v --color=yes --forked -n 2 --ignore-glob=**/disabled_tests/* lv/ environment: From 7879ba655df200d84762e9d689cde3aa751a45f6 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:05:29 +0200 Subject: [PATCH 05/60] triggering drone --- .drone.yml | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/.drone.yml b/.drone.yml index 4dc23e103fd..fc94d4f445f 100644 --- a/.drone.yml +++ b/.drone.yml @@ -11,12 +11,19 @@ clone: disable: true steps: -- name: Build ml_all docker image for pull request - image: plugins/docker +- name: git checkout + image: plugins/github-release commands: - pwd - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile + - ls && pwd + + +- name: Build ml_all docker image for pull request + image: plugins/docker + commands: + - pwd && ls volumes: - name: dockersock path: /var/run/docker.sock From d666f45976d86b9355f26fa0feed67467f7e5fc5 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:13:17 +0200 Subject: [PATCH 06/60] triggering drone2 --- .drone.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index fc94d4f445f..0e1ef3142da 100644 --- a/.drone.yml +++ b/.drone.yml @@ -12,7 +12,7 @@ clone: steps: - name: git checkout - image: plugins/github-release + image: drone/git commands: - pwd - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git From 871ff76828ad02ac9d852962022318bbbfe819e8 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:14:00 +0200 Subject: [PATCH 07/60] triggering drone3 --- .drone.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index 0e1ef3142da..339a45a0c01 100644 --- a/.drone.yml +++ b/.drone.yml @@ -11,7 +11,7 @@ clone: disable: true steps: -- name: git checkout +- name: git checkout deep_cv repo image: drone/git commands: - pwd @@ -20,6 +20,7 @@ steps: - ls && pwd + - name: Build ml_all docker image for pull request image: plugins/docker commands: @@ -39,6 +40,8 @@ steps: from_secret: DOCKER_QUAY_USERNAME password: from_secret: DOCKER_QUAY_PASSWORD + depends_on: + - git checkout deep_cv repo - name: unit-tests validation image: quay.io/logivations/ml_all:latest From e0552ee9aca87daa4fafa5a4bbcc4bbaa3df256d Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:19:13 +0200 Subject: [PATCH 08/60] triggering drone4 --- .drone.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index 339a45a0c01..b3ee78b36f5 100644 --- a/.drone.yml +++ b/.drone.yml @@ -12,7 +12,7 @@ clone: steps: - name: git checkout deep_cv repo - image: drone/git + image: docker:git commands: - pwd - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git From 1c09f5df0b0d1407ed6b411fa6baacfb06249720 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:37:09 +0200 Subject: [PATCH 09/60] triggering drone5 --- .drone.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.drone.yml b/.drone.yml index b3ee78b36f5..2eedbfc2a69 100644 --- a/.drone.yml +++ b/.drone.yml @@ -15,6 +15,8 @@ steps: image: docker:git commands: - pwd + - git clone https://github.com/logivations/deep_cv.git + - echo "we are here" - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile - ls && pwd From 466180b0e288fd615879c75eeed821be8d8526df Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:42:41 +0200 Subject: [PATCH 10/60] triggering drone5 --- .drone.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index 2eedbfc2a69..9ea5deeb378 100644 --- a/.drone.yml +++ b/.drone.yml @@ -15,7 +15,7 @@ steps: image: docker:git commands: - pwd - - git clone https://github.com/logivations/deep_cv.git + - export DRONE_NETRC_MACHINE=github.com && export DRONE_NETRC_USERNAME=maksym.zubrytskyi@logivations.com && export DRONE_NETRC_PASSWORD=${DRONE_SECRET_PLUGIN_TOKEN} - echo "we are here" - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile From 78e73e5587dcf0c2dbb68e7e66b2dfc1faf5e7d3 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 12:44:19 +0200 Subject: [PATCH 11/60] triggering drone6 --- .drone.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index 9ea5deeb378..50487c2309e 100644 --- a/.drone.yml +++ b/.drone.yml @@ -15,7 +15,7 @@ steps: image: docker:git commands: - pwd - - export DRONE_NETRC_MACHINE=github.com && export DRONE_NETRC_USERNAME=maksym.zubrytskyi@logivations.com && export DRONE_NETRC_PASSWORD=${DRONE_SECRET_PLUGIN_TOKEN} + - export DRONE_NETRC_MACHINE=github.com && export DRONE_NETRC_USERNAME=maksym.zubrytskyi@logivations.com && export DRONE_NETRC_PASSWORD=${GIT_TOKEN_PERSONAL} - echo "we are here" - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile From 427986eb06f1e4590ab72ecd17ef767eb47276dd Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Tue, 1 Feb 2022 13:20:57 +0200 Subject: [PATCH 12/60] new changes1 --- .drone.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.drone.yml b/.drone.yml index 50487c2309e..166525b6b07 100644 --- a/.drone.yml +++ b/.drone.yml @@ -12,10 +12,8 @@ clone: steps: - name: git checkout deep_cv repo - image: docker:git commands: - pwd - - export DRONE_NETRC_MACHINE=github.com && export DRONE_NETRC_USERNAME=maksym.zubrytskyi@logivations.com && export DRONE_NETRC_PASSWORD=${GIT_TOKEN_PERSONAL} - echo "we are here" - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile From c1c9161e4406c50c995860a7211fc2b6c98dedb4 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Thu, 3 Feb 2022 17:45:21 +0200 Subject: [PATCH 13/60] add PR validattion checks for drone CI --- .drone.yml | 84 +++++++++++++++++++++++++++++++----------------------- 1 file changed, 49 insertions(+), 35 deletions(-) diff --git a/.drone.yml b/.drone.yml index 166525b6b07..507a09e8313 100644 --- a/.drone.yml +++ b/.drone.yml @@ -1,68 +1,82 @@ --- -# This pipeline chain would be used on each open pull request to build new ml_all docker image and validate unit tests +# 1. 1/2 Starting docker build with custom tag kind: pipeline -name: PR validation build +type: docker +name: PR validation build docker image platform: arch: amd64 os: linux -clone: - disable: true +trigger: + event: + include: + - pull_request -steps: -- name: git checkout deep_cv repo - commands: - - pwd - - echo "we are here" - - git clone https://github.com/logivations/deep_cv.git -b ${DRONE_PULL_REQUEST} || git clone https://github.com/logivations/deep_cv.git - - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_PULL_REQUEST}" /deep_cv/docker/MlAllDockerfile - - ls && pwd +volumes: +- name: dockersock + host: + path: /var/run/docker.sock +steps: +- name: Update Dockerfile with nav2 PR branch + commands: + - sed + - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_SOURCE_BRANCH}" docker/MlAllDockerfile - name: Build ml_all docker image for pull request image: plugins/docker - commands: - - pwd && ls volumes: - name: dockersock path: /var/run/docker.sock settings: - dockerfile: deep_cv/docker/MlAllDockerfile - context: deep_cv/docker/ + dockerfile: docker/MlAllDockerfile + context: docker/ registry: quay.io repo: quay.io/logivations/ml_all privileged: true tags: - - nav2_${DRONE_PULL_REQUEST} + - drone_nav2_${DRONE_PULL_REQUEST} username: from_secret: DOCKER_QUAY_USERNAME password: from_secret: DOCKER_QUAY_PASSWORD - depends_on: - - git checkout deep_cv repo + depends_on: + - Update Dockerfile with nav2 PR branch -- name: unit-tests validation - image: quay.io/logivations/ml_all:latest - commands: - - pwd && ls - - cd /data/workspace && git clone https://github.com/logivations/deep_cv.git - - cd /data/workspace/deep_cv && pytest -v --color=yes --forked -n 2 --ignore-glob=**/disabled_tests/* lv/ - environment: - NVIDIA_VISIBLE_DEVICES: all - depends_on: - - Build ml_all docker image for pull request +--- +# 1. 3/4 Run unit tests validation on each open pull request +kind: pipeline +type: exec +name: PR validation run unit tests + +platform: + arch: amd64 + os: linux trigger: event: include: - pull_request -volumes: -- name: dockersock - host: - path: /var/run/docker.sock - image_pull_secrets: -- dockerconfig_prd \ No newline at end of file +- dockerconfig_prd + +depends_on: +- PR validation build docker image + +steps: +- name: Start docker container + commands: + - docker pull quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} + - docker run -dti --name ${DRONE_COMMIT} --gpus=all -e NVIDIA_VISIBLE_DEVICES=all -e PYTHONHASHSEED=0 -v /opt/data/DeepCVTest:/data/DeepCVTest -v /opt/data/ml_models/:/data/ml_models -v $(pwd):/data/workspace/deep_cv quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} + + +- name: Start unit tests + commands: + - docker exec ${DRONE_COMMIT} bash -ic "export TEAMCITY=TRUE; export ROS_LOCALHOST_ONLY=1; export CYCLONEDDS_URI=/data/workspace/deep_cv/scripts/agv/cyclonedds_open_network.xml; cd /data/workspace/deep_cv; pytest -v --color=yes --forked -n 2 --ignore-glob=**/disabled_tests/* lv/" + +- name: Remove docker container + commands: + - docker rm -f ${DRONE_COMMIT} From ae07f461ed88e75755c9aea38547f0ac34d3b021 Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Wed, 16 Feb 2022 16:07:01 +0200 Subject: [PATCH 14/60] upadte to docker pull step --- .drone.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index 507a09e8313..9db6dc5a37f 100644 --- a/.drone.yml +++ b/.drone.yml @@ -46,7 +46,7 @@ steps: - Update Dockerfile with nav2 PR branch --- -# 1. 3/4 Run unit tests validation on each open pull request +# 1. 2/2 Run unit tests validation on each open pull request kind: pipeline type: exec name: PR validation run unit tests @@ -69,6 +69,7 @@ depends_on: steps: - name: Start docker container commands: + - docker rm -f ${DRONE_COMMIT} || true - docker pull quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} - docker run -dti --name ${DRONE_COMMIT} --gpus=all -e NVIDIA_VISIBLE_DEVICES=all -e PYTHONHASHSEED=0 -v /opt/data/DeepCVTest:/data/DeepCVTest -v /opt/data/ml_models/:/data/ml_models -v $(pwd):/data/workspace/deep_cv quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} From ab65a31124f45537fe3f065435a2d13bfe0e0f8b Mon Sep 17 00:00:00 2001 From: Maksym Zubrytskyi Date: Thu, 17 Feb 2022 11:45:45 +0200 Subject: [PATCH 15/60] triggering drone --- .drone.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.drone.yml b/.drone.yml index 9db6dc5a37f..f5301e7a2cd 100644 --- a/.drone.yml +++ b/.drone.yml @@ -73,7 +73,6 @@ steps: - docker pull quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} - docker run -dti --name ${DRONE_COMMIT} --gpus=all -e NVIDIA_VISIBLE_DEVICES=all -e PYTHONHASHSEED=0 -v /opt/data/DeepCVTest:/data/DeepCVTest -v /opt/data/ml_models/:/data/ml_models -v $(pwd):/data/workspace/deep_cv quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} - - name: Start unit tests commands: - docker exec ${DRONE_COMMIT} bash -ic "export TEAMCITY=TRUE; export ROS_LOCALHOST_ONLY=1; export CYCLONEDDS_URI=/data/workspace/deep_cv/scripts/agv/cyclonedds_open_network.xml; cd /data/workspace/deep_cv; pytest -v --color=yes --forked -n 2 --ignore-glob=**/disabled_tests/* lv/" From 5c9891cc6253839731084891ae2071599cb4380a Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Wed, 22 Mar 2023 13:59:11 +0100 Subject: [PATCH 16/60] Add Publish action to collision monitor --- .../collision_monitor_node.hpp | 12 ++++++ .../nav2_collision_monitor/polygon.hpp | 10 +++++ .../include/nav2_collision_monitor/types.hpp | 3 +- .../src/collision_monitor_node.cpp | 12 ++++++ nav2_collision_monitor/src/polygon.cpp | 41 +++++++++++++++++++ 5 files changed, 77 insertions(+), 1 deletion(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 2dc29958568..706c33081e5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -172,6 +172,18 @@ class CollisionMonitor : public nav2_util::LifecycleNode const Velocity & velocity, Action & robot_action) const; + /** + * @brief Processes Publish action type + * @param polygon Polygon to process + * @param collision_points Array of 2D obstacle points + * @param velocity Desired robot velocity + * @param robot_action Output processed robot action + * @return True if returned action is caused by current polygon, otherwise false + */ + bool processPublish( + const std::shared_ptr polygon, + const std::vector & collision_points) const; + /** * @brief Prints robot action and polygon caused it (if it was) * @param robot_action Robot action to print diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index e1354e434fb..11eb962f9f8 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" +#include "std_msgs/msg/bool.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -140,6 +141,11 @@ class Polygon const std::vector & collision_points, const Velocity & velocity) const; + /** + * @brief Publishes detection message + */ + bool publish_detection(std::vector collision_points) const; + /** * @brief Publishes polygon message into a its own topic */ @@ -210,6 +216,10 @@ class Polygon rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Detection topic + std::string detection_topic_; + /// @brief Detection publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr detection_pub_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index aa0b9d729ba..89f38134c89 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -65,7 +65,8 @@ enum ActionType DO_NOTHING = 0, // No action STOP = 1, // Stop the robot SLOWDOWN = 2, // Slowdown in percentage from current operating speed - APPROACH = 3 // Keep constant time interval before collision + APPROACH = 3, // Keep constant time interval before collision + PUBLISH = 4 // Publish to a topic }; /// @brief Action for robot diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 56a6e6b6a07..b2cef92115f 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -374,6 +374,11 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { action_polygon = polygon; } + } else if (at == PUBLISH) { + // Process PUBLISH for the selected polygon + if (processPublish(polygon, collision_points)) { + action_polygon = polygon; + } } } @@ -454,6 +459,13 @@ bool CollisionMonitor::processApproach( return false; } +bool CollisionMonitor::processPublish( + const std::shared_ptr polygon, + const std::vector & collision_points) const +{ + return polygon->publish_detection(collision_points); +} + void CollisionMonitor::printAction( const Action & robot_action, const std::shared_ptr action_polygon) const { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 824ee75f671..ba8539e66af 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -83,6 +83,11 @@ bool Polygon::configure() base_frame_id_, tf2::durationToSec(transform_tolerance_)); } + if (!detection_topic_.empty()) { + detection_pub_ = node->create_publisher( + detection_topic_, rclcpp::SystemDefaultsQoS()); + } + if (visualize_) { // Fill polygon_ for future usage polygon_.header.frame_id = base_frame_id_; @@ -109,6 +114,9 @@ void Polygon::activate() if (visualize_) { polygon_pub_->on_activate(); } + if (!detection_topic_.empty()) { + detection_pub_->on_activate(); + } } void Polygon::deactivate() @@ -116,6 +124,9 @@ void Polygon::deactivate() if (visualize_) { polygon_pub_->on_deactivate(); } + if (!detection_topic_.empty()) { + detection_pub_->on_deactivate(); + } } std::string Polygon::getName() const @@ -238,6 +249,27 @@ void Polygon::publish() polygon_pub_->publish(std::move(msg)); } +bool Polygon::publish_detection(std::vector collision_points) const +{ + if (detection_topic_.empty()) { + return false; + } + + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + bool detected = getPointsInside(collision_points) > getMaxPoints(); + + std::unique_ptr detection = + std::make_unique(); + detection->data = detected; + // Publish polygon + + detection_pub_->publish(std::move(detection)); + return detected; +} + bool Polygon::getCommonParameters(std::string & polygon_pub_topic) { auto node = node_.lock(); @@ -258,6 +290,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } else if (at_str == "approach") { action_type_ = APPROACH; + } else if (at_str == "publish") { + action_type_ = PUBLISH; } else { // Error if something else RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); return false; @@ -374,6 +408,13 @@ bool Polygon::getParameters( rclcpp::ParameterValue("local_costmap/published_footprint")); footprint_topic = node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + } else if (action_type_ == PUBLISH) { + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".detection_topic", rclcpp::ParameterValue(polygon_name_)); + detection_topic_ = + node->get_parameter(polygon_name_ + ".detection_topic").as_string(); + } else { + detection_topic_.clear(); } } catch (const std::exception & ex) { RCLCPP_ERROR( From d218133a368de9c39982ff56341136acecb7482e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Mar 2023 11:09:24 +0200 Subject: [PATCH 17/60] Revert "Add Publish action to collision monitor" This reverts commit 5c9891cc6253839731084891ae2071599cb4380a. --- .../collision_monitor_node.hpp | 12 ------ .../nav2_collision_monitor/polygon.hpp | 10 ----- .../include/nav2_collision_monitor/types.hpp | 3 +- .../src/collision_monitor_node.cpp | 12 ------ nav2_collision_monitor/src/polygon.cpp | 41 ------------------- 5 files changed, 1 insertion(+), 77 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 706c33081e5..2dc29958568 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -172,18 +172,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode const Velocity & velocity, Action & robot_action) const; - /** - * @brief Processes Publish action type - * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points - * @param velocity Desired robot velocity - * @param robot_action Output processed robot action - * @return True if returned action is caused by current polygon, otherwise false - */ - bool processPublish( - const std::shared_ptr polygon, - const std::vector & collision_points) const; - /** * @brief Prints robot action and polygon caused it (if it was) * @param robot_action Robot action to print diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 11eb962f9f8..e1354e434fb 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -21,7 +21,6 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "std_msgs/msg/bool.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -141,11 +140,6 @@ class Polygon const std::vector & collision_points, const Velocity & velocity) const; - /** - * @brief Publishes detection message - */ - bool publish_detection(std::vector collision_points) const; - /** * @brief Publishes polygon message into a its own topic */ @@ -216,10 +210,6 @@ class Polygon rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; - /// @brief Detection topic - std::string detection_topic_; - /// @brief Detection publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr detection_pub_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index 89f38134c89..aa0b9d729ba 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -65,8 +65,7 @@ enum ActionType DO_NOTHING = 0, // No action STOP = 1, // Stop the robot SLOWDOWN = 2, // Slowdown in percentage from current operating speed - APPROACH = 3, // Keep constant time interval before collision - PUBLISH = 4 // Publish to a topic + APPROACH = 3 // Keep constant time interval before collision }; /// @brief Action for robot diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index b2cef92115f..56a6e6b6a07 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -374,11 +374,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { action_polygon = polygon; } - } else if (at == PUBLISH) { - // Process PUBLISH for the selected polygon - if (processPublish(polygon, collision_points)) { - action_polygon = polygon; - } } } @@ -459,13 +454,6 @@ bool CollisionMonitor::processApproach( return false; } -bool CollisionMonitor::processPublish( - const std::shared_ptr polygon, - const std::vector & collision_points) const -{ - return polygon->publish_detection(collision_points); -} - void CollisionMonitor::printAction( const Action & robot_action, const std::shared_ptr action_polygon) const { diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index ba8539e66af..824ee75f671 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -83,11 +83,6 @@ bool Polygon::configure() base_frame_id_, tf2::durationToSec(transform_tolerance_)); } - if (!detection_topic_.empty()) { - detection_pub_ = node->create_publisher( - detection_topic_, rclcpp::SystemDefaultsQoS()); - } - if (visualize_) { // Fill polygon_ for future usage polygon_.header.frame_id = base_frame_id_; @@ -114,9 +109,6 @@ void Polygon::activate() if (visualize_) { polygon_pub_->on_activate(); } - if (!detection_topic_.empty()) { - detection_pub_->on_activate(); - } } void Polygon::deactivate() @@ -124,9 +116,6 @@ void Polygon::deactivate() if (visualize_) { polygon_pub_->on_deactivate(); } - if (!detection_topic_.empty()) { - detection_pub_->on_deactivate(); - } } std::string Polygon::getName() const @@ -249,27 +238,6 @@ void Polygon::publish() polygon_pub_->publish(std::move(msg)); } -bool Polygon::publish_detection(std::vector collision_points) const -{ - if (detection_topic_.empty()) { - return false; - } - - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; - } - bool detected = getPointsInside(collision_points) > getMaxPoints(); - - std::unique_ptr detection = - std::make_unique(); - detection->data = detected; - // Publish polygon - - detection_pub_->publish(std::move(detection)); - return detected; -} - bool Polygon::getCommonParameters(std::string & polygon_pub_topic) { auto node = node_.lock(); @@ -290,8 +258,6 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } else if (at_str == "approach") { action_type_ = APPROACH; - } else if (at_str == "publish") { - action_type_ = PUBLISH; } else { // Error if something else RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); return false; @@ -408,13 +374,6 @@ bool Polygon::getParameters( rclcpp::ParameterValue("local_costmap/published_footprint")); footprint_topic = node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); - } else if (action_type_ == PUBLISH) { - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".detection_topic", rclcpp::ParameterValue(polygon_name_)); - detection_topic_ = - node->get_parameter(polygon_name_ + ".detection_topic").as_string(); - } else { - detection_topic_.clear(); } } catch (const std::exception & ex) { RCLCPP_ERROR( From cc5077206d375028b8763094050516fe9e528823 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 23 Mar 2023 11:54:40 +0100 Subject: [PATCH 18/60] Initial try --- .../collision_monitor_node.hpp | 15 ++++++++ .../src/collision_monitor_node.cpp | 36 +++++++++++++++++-- nav2_collision_monitor/src/polygon.cpp | 2 ++ 3 files changed, 51 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 2dc29958568..9413456c26b 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -21,6 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "std_msgs/msg/string.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -172,6 +173,18 @@ class CollisionMonitor : public nav2_util::LifecycleNode const Velocity & velocity, Action & robot_action) const; + /** + * @brief Processes DoNothing action type + * @param polygon Polygon to process + * @param collision_points Array of 2D obstacle points + * @param robot_action Output processed robot action + * @return True if returned action is caused by current polygon, otherwise false + */ + bool processDoNothing( + const std::shared_ptr polygon, + const std::vector & collision_points, + Action & robot_action) const; + /** * @brief Prints robot action and polygon caused it (if it was) * @param robot_action Robot action to print @@ -203,6 +216,8 @@ class CollisionMonitor : public nav2_util::LifecycleNode rclcpp::Subscription::SharedPtr cmd_vel_in_sub_; /// @brief Output cmd_vel publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; + /// @brief collision monitor state publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 56a6e6b6a07..205046a3d54 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -67,6 +67,9 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); + state_pub_ = this->create_publisher( + "~/state", rclcpp::SystemDefaultsQoS()); + return nav2_util::CallbackReturn::SUCCESS; } @@ -75,8 +78,9 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - // Activating lifecycle publisher + // Activating lifecycle publishers cmd_vel_out_pub_->on_activate(); + state_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -114,6 +118,7 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Deactivating lifecycle publishers cmd_vel_out_pub_->on_deactivate(); + state_pub_->on_deactivate(); // Destroying bond connection destroyBond(); @@ -128,6 +133,7 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/) cmd_vel_in_sub_.reset(); cmd_vel_out_pub_.reset(); + state_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -374,6 +380,11 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { action_polygon = polygon; } + } else if (at == DO_NOTHING) { + // Process DO_NOTHING for the selected polygon + if (processDoNothing(polygon, collision_points, robot_action)) { + action_polygon = polygon; + } } } @@ -391,6 +402,18 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) robot_action_prev_ = robot_action; } +bool CollisionMonitor::processDoNothing( + const std::shared_ptr polygon, + const std::vector & collision_points, + Action & robot_action) const +{ + if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) { + robot_action.action_type = DO_NOTHING; + } + + return false; +} + bool CollisionMonitor::processStopSlowdown( const std::shared_ptr polygon, const std::vector & collision_points, @@ -457,27 +480,36 @@ bool CollisionMonitor::processApproach( void CollisionMonitor::printAction( const Action & robot_action, const std::shared_ptr action_polygon) const { + std::unique_ptr state_msg = + std::make_unique(); if (robot_action.action_type == STOP) { RCLCPP_INFO( get_logger(), "Robot to stop due to %s polygon", action_polygon->getName().c_str()); + state_msg->data = "stop"; + } else if (robot_action.action_type == SLOWDOWN) { RCLCPP_INFO( get_logger(), "Robot to slowdown for %f percents due to %s polygon", action_polygon->getSlowdownRatio() * 100, action_polygon->getName().c_str()); + state_msg->data = "slowdown"; } else if (robot_action.action_type == APPROACH) { RCLCPP_INFO( get_logger(), "Robot to approach for %f seconds away from collision", action_polygon->getTimeBeforeCollision()); - } else { // robot_action.action_type == DO_NOTHING + state_msg->data = "approach"; + + } else if (robot_action.action_type == DO_NOTHING) { RCLCPP_INFO( get_logger(), "Robot to continue normal operation"); + state_msg->data = "Do nothing"; } + state_pub_->publish(std::move(state_msg)); } void CollisionMonitor::publishPolygons() const diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 824ee75f671..35b7cfe1027 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -258,6 +258,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } else if (at_str == "approach") { action_type_ = APPROACH; + } else if (at_str == "do_nothing") { + action_type_ = DO_NOTHING; } else { // Error if something else RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); return false; From d1780cd6b653aae6001fc224b0c4ccce133c52c4 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Mar 2023 11:34:13 +0200 Subject: [PATCH 19/60] start other thread --- .../collision_monitor_node.hpp | 18 +++-- .../src/collision_monitor_node.cpp | 78 ++++++++++++------- 2 files changed, 61 insertions(+), 35 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 9413456c26b..86ab9651cd6 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/bool.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -145,6 +145,11 @@ class CollisionMonitor : public nav2_util::LifecycleNode */ void process(const Velocity & cmd_vel_in); + /** + * @brief Timer callback for actions not requiring vel + */ + void timer_callback(); + /** * @brief Processes the polygon of STOP and SLOWDOWN action type * @param polygon Polygon to process @@ -177,13 +182,11 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Processes DoNothing action type * @param polygon Polygon to process * @param collision_points Array of 2D obstacle points - * @param robot_action Output processed robot action * @return True if returned action is caused by current polygon, otherwise false */ - bool processDoNothing( + void processDoNothing( const std::shared_ptr polygon, - const std::vector & collision_points, - Action & robot_action) const; + const std::vector & collision_points) const; /** * @brief Prints robot action and polygon caused it (if it was) @@ -217,7 +220,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode /// @brief Output cmd_vel publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; /// @brief collision monitor state publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr trigger_pub_; + /// @brief timer that runs actions not dependent on cmd_vel + rclcpp::TimerBase::SharedPtr timer_; + /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 205046a3d54..2bf06c6c193 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -24,6 +24,8 @@ #include "nav2_collision_monitor/kinematics.hpp" +using namespace std::chrono_literals; + namespace nav2_collision_monitor { @@ -67,7 +69,16 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); - state_pub_ = this->create_publisher( + for (std::shared_ptr polygon : polygons_) { + if (polygon->getActionType() == DO_NOTHING) { + timer_ = this->create_wall_timer( + 100ms, + std::bind(&CollisionMonitor::timer_callback, this)); + break; + } + } + + trigger_pub_ = this->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); return nav2_util::CallbackReturn::SUCCESS; @@ -80,7 +91,7 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) // Activating lifecycle publishers cmd_vel_out_pub_->on_activate(); - state_pub_->on_activate(); + trigger_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -118,7 +129,7 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Deactivating lifecycle publishers cmd_vel_out_pub_->on_deactivate(); - state_pub_->on_deactivate(); + trigger_pub_->on_deactivate(); // Destroying bond connection destroyBond(); @@ -133,7 +144,7 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/) cmd_vel_in_sub_.reset(); cmd_vel_out_pub_.reset(); - state_pub_.reset(); + trigger_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -380,11 +391,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) if (processApproach(polygon, collision_points, cmd_vel_in, robot_action)) { action_polygon = polygon; } - } else if (at == DO_NOTHING) { - // Process DO_NOTHING for the selected polygon - if (processDoNothing(polygon, collision_points, robot_action)) { - action_polygon = polygon; - } } } @@ -402,16 +408,43 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) robot_action_prev_ = robot_action; } -bool CollisionMonitor::processDoNothing( - const std::shared_ptr polygon, - const std::vector & collision_points, - Action & robot_action) const +void CollisionMonitor::timer_callback() { - if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) { - robot_action.action_type = DO_NOTHING; + // Current timestamp for all inner routines prolongation + rclcpp::Time curr_time = this->now(); + + // Do nothing if main worker in non-active state + if (!process_active_) { + return; } - return false; + // Points array collected from different data sources in a robot base frame + std::vector collision_points; + + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + source->getData(curr_time, collision_points); + } + + for (std::shared_ptr polygon : polygons_) { + const ActionType at = polygon->getActionType(); + if (at == DO_NOTHING) { + // Process PUBLISH for the selected polygon + processDoNothing(polygon, collision_points); + } + } + publishPolygons(); +} + + +void CollisionMonitor::processDoNothing( + const std::shared_ptr polygon, + const std::vector & collision_points) const +{ + std::unique_ptr trigger_msg = + std::make_unique(); + trigger_msg->data = polygon->getPointsInside(collision_points) > polygon->getMaxPoints(); + trigger_pub_->publish(std::move(trigger_msg)); } bool CollisionMonitor::processStopSlowdown( @@ -480,36 +513,23 @@ bool CollisionMonitor::processApproach( void CollisionMonitor::printAction( const Action & robot_action, const std::shared_ptr action_polygon) const { - std::unique_ptr state_msg = - std::make_unique(); if (robot_action.action_type == STOP) { RCLCPP_INFO( get_logger(), "Robot to stop due to %s polygon", action_polygon->getName().c_str()); - state_msg->data = "stop"; - } else if (robot_action.action_type == SLOWDOWN) { RCLCPP_INFO( get_logger(), "Robot to slowdown for %f percents due to %s polygon", action_polygon->getSlowdownRatio() * 100, action_polygon->getName().c_str()); - state_msg->data = "slowdown"; } else if (robot_action.action_type == APPROACH) { RCLCPP_INFO( get_logger(), "Robot to approach for %f seconds away from collision", action_polygon->getTimeBeforeCollision()); - state_msg->data = "approach"; - - } else if (robot_action.action_type == DO_NOTHING) { - RCLCPP_INFO( - get_logger(), - "Robot to continue normal operation"); - state_msg->data = "Do nothing"; } - state_pub_->publish(std::move(state_msg)); } void CollisionMonitor::publishPolygons() const From 7f1b82a465d80e019cfd090a4828990ef4c21018 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Mar 2023 11:36:00 +0200 Subject: [PATCH 20/60] . --- nav2_collision_monitor/src/collision_monitor_node.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2bf06c6c193..00c81ffbf59 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -529,7 +529,10 @@ void CollisionMonitor::printAction( get_logger(), "Robot to approach for %f seconds away from collision", action_polygon->getTimeBeforeCollision()); - } + } else { // robot_action.action_type == DO_NOTHING + RCLCPP_INFO( + get_logger(), + "Robot to continue normal operation"); } void CollisionMonitor::publishPolygons() const From b9f4e5a50128c0197cdac940888e6df1a7765aaa Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Mar 2023 11:44:58 +0200 Subject: [PATCH 21/60] . --- .../src/collision_monitor_node.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 00c81ffbf59..5aaa2f1baad 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include "tf2_ros/create_timer_ros.h" @@ -69,14 +70,17 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); - for (std::shared_ptr polygon : polygons_) { - if (polygon->getActionType() == DO_NOTHING) { - timer_ = this->create_wall_timer( - 100ms, - std::bind(&CollisionMonitor::timer_callback, this)); - break; - } + + bool any_do_nothing = std::any_of(polygons_.begin(), polygons_.end(), + [](const std::shared_ptr polygon) { + return polygon->getActionType() == DO_NOTHING; }); + + if (any_do_nothing) { + timer_ = this->create_wall_timer( + 100ms, + std::bind(&CollisionMonitor::timer_callback, this)); } + trigger_pub_ = this->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); From 73e41d9b23b7fa16b4ab8d493f8ca06d881142c3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Mar 2023 11:50:15 +0200 Subject: [PATCH 22/60] fix --- nav2_collision_monitor/src/collision_monitor_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 5aaa2f1baad..a8cec9cb0de 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -537,6 +537,7 @@ void CollisionMonitor::printAction( RCLCPP_INFO( get_logger(), "Robot to continue normal operation"); + } } void CollisionMonitor::publishPolygons() const From d5de573f1d8f4feb13956da5be447b1536fb1d9c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Mar 2023 20:41:08 +0200 Subject: [PATCH 23/60] revert all changes --- .../collision_monitor_node.hpp | 21 ------- .../src/collision_monitor_node.cpp | 62 +------------------ nav2_collision_monitor/src/polygon.cpp | 2 - 3 files changed, 1 insertion(+), 84 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 86ab9651cd6..2dc29958568 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -21,7 +21,6 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "std_msgs/msg/bool.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -145,11 +144,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode */ void process(const Velocity & cmd_vel_in); - /** - * @brief Timer callback for actions not requiring vel - */ - void timer_callback(); - /** * @brief Processes the polygon of STOP and SLOWDOWN action type * @param polygon Polygon to process @@ -178,16 +172,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode const Velocity & velocity, Action & robot_action) const; - /** - * @brief Processes DoNothing action type - * @param polygon Polygon to process - * @param collision_points Array of 2D obstacle points - * @return True if returned action is caused by current polygon, otherwise false - */ - void processDoNothing( - const std::shared_ptr polygon, - const std::vector & collision_points) const; - /** * @brief Prints robot action and polygon caused it (if it was) * @param robot_action Robot action to print @@ -219,11 +203,6 @@ class CollisionMonitor : public nav2_util::LifecycleNode rclcpp::Subscription::SharedPtr cmd_vel_in_sub_; /// @brief Output cmd_vel publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr cmd_vel_out_pub_; - /// @brief collision monitor state publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr trigger_pub_; - /// @brief timer that runs actions not dependent on cmd_vel - rclcpp::TimerBase::SharedPtr timer_; - /// @brief Whether main routine is active bool process_active_; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index a8cec9cb0de..56a6e6b6a07 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -17,7 +17,6 @@ #include #include #include -#include #include "tf2_ros/create_timer_ros.h" @@ -25,8 +24,6 @@ #include "nav2_collision_monitor/kinematics.hpp" -using namespace std::chrono_literals; - namespace nav2_collision_monitor { @@ -70,21 +67,6 @@ CollisionMonitor::on_configure(const rclcpp_lifecycle::State & /*state*/) cmd_vel_out_pub_ = this->create_publisher( cmd_vel_out_topic, 1); - - bool any_do_nothing = std::any_of(polygons_.begin(), polygons_.end(), - [](const std::shared_ptr polygon) { - return polygon->getActionType() == DO_NOTHING; }); - - if (any_do_nothing) { - timer_ = this->create_wall_timer( - 100ms, - std::bind(&CollisionMonitor::timer_callback, this)); - } - - - trigger_pub_ = this->create_publisher( - "~/state", rclcpp::SystemDefaultsQoS()); - return nav2_util::CallbackReturn::SUCCESS; } @@ -93,9 +75,8 @@ CollisionMonitor::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - // Activating lifecycle publishers + // Activating lifecycle publisher cmd_vel_out_pub_->on_activate(); - trigger_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -133,7 +114,6 @@ CollisionMonitor::on_deactivate(const rclcpp_lifecycle::State & /*state*/) // Deactivating lifecycle publishers cmd_vel_out_pub_->on_deactivate(); - trigger_pub_->on_deactivate(); // Destroying bond connection destroyBond(); @@ -148,7 +128,6 @@ CollisionMonitor::on_cleanup(const rclcpp_lifecycle::State & /*state*/) cmd_vel_in_sub_.reset(); cmd_vel_out_pub_.reset(); - trigger_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -412,45 +391,6 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) robot_action_prev_ = robot_action; } -void CollisionMonitor::timer_callback() -{ - // Current timestamp for all inner routines prolongation - rclcpp::Time curr_time = this->now(); - - // Do nothing if main worker in non-active state - if (!process_active_) { - return; - } - - // Points array collected from different data sources in a robot base frame - std::vector collision_points; - - // Fill collision_points array from different data sources - for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); - } - - for (std::shared_ptr polygon : polygons_) { - const ActionType at = polygon->getActionType(); - if (at == DO_NOTHING) { - // Process PUBLISH for the selected polygon - processDoNothing(polygon, collision_points); - } - } - publishPolygons(); -} - - -void CollisionMonitor::processDoNothing( - const std::shared_ptr polygon, - const std::vector & collision_points) const -{ - std::unique_ptr trigger_msg = - std::make_unique(); - trigger_msg->data = polygon->getPointsInside(collision_points) > polygon->getMaxPoints(); - trigger_pub_->publish(std::move(trigger_msg)); -} - bool CollisionMonitor::processStopSlowdown( const std::shared_ptr polygon, const std::vector & collision_points, diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 35b7cfe1027..824ee75f671 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -258,8 +258,6 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } else if (at_str == "approach") { action_type_ = APPROACH; - } else if (at_str == "do_nothing") { - action_type_ = DO_NOTHING; } else { // Error if something else RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); return false; From 9ba1b072ce4fe47260f7696f65eebec211c4eea5 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 27 Mar 2023 20:46:12 +0200 Subject: [PATCH 24/60] Make new node --- nav2_collision_monitor/CMakeLists.txt | 24 +- .../collision_monitor_node.hpp | 4 +- .../nav2_collision_monitor/detector_node.hpp | 156 +++++++++ nav2_collision_monitor/src/detector_node.cpp | 324 ++++++++++++++++++ .../src/main_detector_node.cpp | 29 ++ nav2_collision_monitor/src/polygon.cpp | 2 + 6 files changed, 536 insertions(+), 3 deletions(-) create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/detector_node.hpp create mode 100644 nav2_collision_monitor/src/detector_node.cpp create mode 100644 nav2_collision_monitor/src/main_detector_node.cpp diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 8544429466a..be4efc37ec9 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -38,7 +38,9 @@ set(dependencies ) set(executable_name collision_monitor) +set(executable_name_detector detector) set(library_name ${executable_name}_core) +set(library_name_detector ${executable_name_detector}_core) add_library(${library_name} SHARED src/collision_monitor_node.cpp @@ -50,18 +52,37 @@ add_library(${library_name} SHARED src/range.cpp src/kinematics.cpp ) +add_library(${library_name_detector} SHARED + src/detector_node.cpp + src/polygon.cpp + src/circle.cpp + src/source.cpp + src/scan.cpp + src/pointcloud.cpp + src/range.cpp + src/kinematics.cpp +) add_executable(${executable_name} src/main.cpp ) +add_executable(${executable_name_detector} + src/main_detector_node.cpp +) ament_target_dependencies(${library_name} ${dependencies} ) +ament_target_dependencies(${library_name_detector} + ${dependencies} +) target_link_libraries(${executable_name} ${library_name} ) +target_link_libraries(${executable_name_detector} + ${library_name_detector} +) ament_target_dependencies(${executable_name} ${dependencies} @@ -77,7 +98,7 @@ install(TARGETS ${library_name} RUNTIME DESTINATION bin ) -install(TARGETS ${executable_name} +install(TARGETS ${executable_name} ${executable_name_detector} RUNTIME DESTINATION lib/${PROJECT_NAME} ) @@ -105,6 +126,7 @@ endif() ament_export_include_directories(include) ament_export_libraries(${library_name}) +ament_export_libraries(${library_name_detector}) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 2dc29958568..c2447994d91 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -46,12 +46,12 @@ class CollisionMonitor : public nav2_util::LifecycleNode { public: /** - * @brief Constructor for the nav2_collision_safery::CollisionMonitor + * @brief Constructor for the nav2_collision_monitor::CollisionMonitor * @param options Additional options to control creation of the node. */ explicit CollisionMonitor(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** - * @brief Destructor for the nav2_collision_safery::CollisionMonitor + * @brief Destructor for the nav2_collision_monitor::CollisionMonitor */ ~CollisionMonitor(); diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/detector_node.hpp new file mode 100644 index 00000000000..869712f7040 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/detector_node.hpp @@ -0,0 +1,156 @@ +// Copyright (c) 2022 Samsung R&D Institute 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. + +#ifndef NAV2_COLLISION_MONITOR__DETECTOR_NODE_HPP_ +#define NAV2_COLLISION_MONITOR__DETECTOR_NODE_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" + +#include "tf2/time.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "nav2_util/lifecycle_node.hpp" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/polygon.hpp" +#include "nav2_collision_monitor/circle.hpp" +#include "nav2_collision_monitor/source.hpp" +#include "nav2_collision_monitor/scan.hpp" +#include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Collision Monitor ROS2 node + */ +class Detector : public nav2_util::LifecycleNode +{ +public: + /** + * @brief Constructor for the nav2_collision_monitor::Detector + * @param options Additional options to control creation of the node. + */ + explicit Detector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** + * @brief Destructor for the nav2_collision_monitor::Detector + */ + ~Detector(); + +protected: + /** + * @brief: Initializes and obtains ROS-parameters, creates main subscribers and publishers, + * creates polygons and data sources objects + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Activates LifecyclePublishers, polygons and main processor, creates bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Deactivates LifecyclePublishers, polygons and main processor, destroys bond connection + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + /** + * @brief: Resets all subscribers/publishers, polygons/data sources arrays + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override; + /** + * @brief Called in shutdown state + * @param state Lifecycle Node's state + * @return Success or Failure + */ + nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + +protected: + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in failure case + */ + bool getParameters(); + /** + * @brief Supporting routine creating and configuring all polygons + * @param base_frame_id Robot base frame ID + * @param transform_tolerance Transform tolerance + * @return True if all polygons were configured successfully or false in failure case + */ + bool configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance); + /** + * @brief Supporting routine creating and configuring all data sources + * @param base_frame_id Robot base frame ID + * @param odom_frame_id Odometry frame ID. Used as global frame to get + * source->base time inerpolated transform. + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + * @return True if all sources were configured successfully or false in failure case + */ + bool configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + + /** + * @brief Main processing routine + */ + void process(); + + /** + * @brief Polygons publishing routine. Made for visualization. + */ + void publishPolygons() const; + + // ----- Variables ----- + + /// @brief TF buffer + std::shared_ptr tf_buffer_; + /// @brief TF listener + std::shared_ptr tf_listener_; + + /// @brief Polygons array + std::vector> polygons_; + + /// @brief Data sources array + std::vector> sources_; + + /// @brief Whether main routine is active + bool process_active_; + + /// @brief collision monitor state publisher + rclcpp_lifecycle::LifecyclePublisher::SharedPtr trigger_pub_; + /// @brief timer that runs actions + rclcpp::TimerBase::SharedPtr timer_; + +}; // class Detector + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__DETECTOR_NODE_HPP_ diff --git a/nav2_collision_monitor/src/detector_node.cpp b/nav2_collision_monitor/src/detector_node.cpp new file mode 100644 index 00000000000..b68503bf619 --- /dev/null +++ b/nav2_collision_monitor/src/detector_node.cpp @@ -0,0 +1,324 @@ +// Copyright (c) 2022 Samsung R&D Institute 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 "nav2_collision_monitor/detector_node.hpp" + +#include +#include +#include + +#include "tf2_ros/create_timer_ros.h" + +#include "nav2_util/node_utils.hpp" + +#include "nav2_collision_monitor/kinematics.hpp" + +using namespace std::chrono_literals; + +namespace nav2_collision_monitor +{ + +Detector::Detector(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("detector", "", options), + process_active_(false) +{ +} + +Detector::~Detector() +{ + polygons_.clear(); + sources_.clear(); +} + +nav2_util::CallbackReturn +Detector::on_configure(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Configuring"); + + // Transform buffer and listener initialization + tf_buffer_ = std::make_shared(this->get_clock()); + auto timer_interface = std::make_shared( + this->get_node_base_interface(), + this->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + tf_listener_ = std::make_shared(*tf_buffer_); + + // Creating timer + timer_ = this->create_wall_timer( + 100ms, + std::bind(&Detector::process, this)); + + trigger_pub_ = this->create_publisher( + "~/state", rclcpp::SystemDefaultsQoS()); + + // Obtaining ROS parameters + if (!getParameters()) { + return nav2_util::CallbackReturn::FAILURE; + } + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +Detector::on_activate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Activating"); + + // Activating lifecycle publisher + trigger_pub_->on_activate(); + + // Activating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->activate(); + } + + // Activating main worker + process_active_ = true; + + // Creating bond connection + createBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +Detector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Deactivating"); + + // Deactivating lifecycle publishers + trigger_pub_->on_deactivate(); + + // Deactivating main worker + process_active_ = false; + + // Deactivating polygons + for (std::shared_ptr polygon : polygons_) { + polygon->deactivate(); + } + + // Destroying bond connection + destroyBond(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +Detector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Cleaning up"); + + trigger_pub_.reset(); + + polygons_.clear(); + sources_.clear(); + + tf_listener_.reset(); + tf_buffer_.reset(); + + return nav2_util::CallbackReturn::SUCCESS; +} + +nav2_util::CallbackReturn +Detector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +{ + RCLCPP_INFO(get_logger(), "Shutting down"); + + return nav2_util::CallbackReturn::SUCCESS; +} + + +bool Detector::getParameters() +{ + std::string base_frame_id, odom_frame_id; + tf2::Duration transform_tolerance; + rclcpp::Duration source_timeout(2.0, 0.0); + + auto node = shared_from_this(); + + nav2_util::declare_parameter_if_not_declared( + node, "base_frame_id", rclcpp::ParameterValue("base_footprint")); + base_frame_id = get_parameter("base_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "odom_frame_id", rclcpp::ParameterValue("odom")); + odom_frame_id = get_parameter("odom_frame_id").as_string(); + nav2_util::declare_parameter_if_not_declared( + node, "transform_tolerance", rclcpp::ParameterValue(0.1)); + transform_tolerance = + tf2::durationFromSec(get_parameter("transform_tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "source_timeout", rclcpp::ParameterValue(2.0)); + source_timeout = + rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double()); + + if (!configurePolygons(base_frame_id, transform_tolerance)) { + return false; + } + + if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) { + return false; + } + + return true; +} + +bool Detector::configurePolygons( + const std::string & base_frame_id, + const tf2::Duration & transform_tolerance) +{ + try { + auto node = shared_from_this(); + + nav2_util::declare_parameter_if_not_declared( + node, "polygons", rclcpp::ParameterValue(std::vector())); + std::vector polygon_names = get_parameter("polygons").as_string_array(); + for (std::string polygon_name : polygon_names) { + // Leave it not initialized: the will cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, polygon_name + ".type", rclcpp::PARAMETER_STRING); + const std::string polygon_type = get_parameter(polygon_name + ".type").as_string(); + + if (polygon_type == "polygon") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else if (polygon_type == "circle") { + polygons_.push_back( + std::make_shared( + node, polygon_name, tf_buffer_, base_frame_id, transform_tolerance)); + } else { // Error if something else + RCLCPP_ERROR( + get_logger(), + "[%s]: Unknown polygon type: %s", + polygon_name.c_str(), polygon_type.c_str()); + return false; + } + + // Configure last added polygon + if (!polygons_.back()->configure()) { + return false; + } + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +bool Detector::configureSources( + const std::string & base_frame_id, + const std::string & odom_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +{ + try { + auto node = shared_from_this(); + + // Leave it to be not initialized: to intentionally cause an error if it will not set + nav2_util::declare_parameter_if_not_declared( + node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); + std::vector source_names = get_parameter("observation_sources").as_string_array(); + for (std::string source_name : source_names) { + nav2_util::declare_parameter_if_not_declared( + node, source_name + ".type", + rclcpp::ParameterValue("scan")); // Laser scanner by default + const std::string source_type = get_parameter(source_name + ".type").as_string(); + + if (source_type == "scan") { + std::shared_ptr s = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout); + + s->configure(); + + sources_.push_back(s); + } else if (source_type == "pointcloud") { + std::shared_ptr p = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout); + + p->configure(); + + sources_.push_back(p); + } else if (source_type == "range") { + std::shared_ptr r = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout); + + r->configure(); + + sources_.push_back(r); + } else { // Error if something else + RCLCPP_ERROR( + get_logger(), + "[%s]: Unknown source type: %s", + source_name.c_str(), source_type.c_str()); + return false; + } + } + } catch (const std::exception & ex) { + RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); + return false; + } + + return true; +} + +void Detector::process() +{ + // Current timestamp for all inner routines prolongation + rclcpp::Time curr_time = this->now(); + + // Do nothing if main worker in non-active state + if (!process_active_) { + return; + } + + // Points array collected from different data sources in a robot base frame + std::vector collision_points; + + // Fill collision_points array from different data sources + for (std::shared_ptr source : sources_) { + source->getData(curr_time, collision_points); + } + + std::unique_ptr trigger_msg = + std::make_unique(); + for (std::shared_ptr polygon : polygons_) { + trigger_msg->data = polygon->getPointsInside(collision_points) > polygon->getMaxPoints(); + } + + trigger_pub_->publish(std::move(trigger_msg)); + + // Publish polygons for better visualization + publishPolygons(); +} + +void Detector::publishPolygons() const +{ + for (std::shared_ptr polygon : polygons_) { + polygon->publish(); + } +} + +} // namespace nav2_collision_monitor + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::Detector) diff --git a/nav2_collision_monitor/src/main_detector_node.cpp b/nav2_collision_monitor/src/main_detector_node.cpp new file mode 100644 index 00000000000..a37aa0408a3 --- /dev/null +++ b/nav2_collision_monitor/src/main_detector_node.cpp @@ -0,0 +1,29 @@ +// Copyright (c) 2022 Samsung R&D Institute 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 "rclcpp/rclcpp.hpp" + +#include "nav2_collision_monitor/detector_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 824ee75f671..35b7cfe1027 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -258,6 +258,8 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } else if (at_str == "approach") { action_type_ = APPROACH; + } else if (at_str == "do_nothing") { + action_type_ = DO_NOTHING; } else { // Error if something else RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); return false; From 57837002c94cb703d5d22621d03f75b254bba90d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 28 Mar 2023 12:35:08 +0200 Subject: [PATCH 25/60] PR changes --- nav2_collision_monitor/CMakeLists.txt | 9 ++-- ...r_node.hpp => collision_detector_node.hpp} | 5 +- ...r_node.cpp => collision_detector_main.cpp} | 2 +- ...r_node.cpp => collision_detector_node.cpp} | 48 ++++++++----------- 4 files changed, 25 insertions(+), 39 deletions(-) rename nav2_collision_monitor/include/nav2_collision_monitor/{detector_node.hpp => collision_detector_node.hpp} (98%) rename nav2_collision_monitor/src/{main_detector_node.cpp => collision_detector_main.cpp} (93%) rename nav2_collision_monitor/src/{detector_node.cpp => collision_detector_node.cpp} (92%) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index be4efc37ec9..81f04f2975f 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -38,7 +38,7 @@ set(dependencies ) set(executable_name collision_monitor) -set(executable_name_detector detector) +set(executable_name_detector collision_detector) set(library_name ${executable_name}_core) set(library_name_detector ${executable_name_detector}_core) @@ -53,7 +53,7 @@ add_library(${library_name} SHARED src/kinematics.cpp ) add_library(${library_name_detector} SHARED - src/detector_node.cpp + src/collision_detector_node.cpp src/polygon.cpp src/circle.cpp src/source.cpp @@ -67,7 +67,7 @@ add_executable(${executable_name} src/main.cpp ) add_executable(${executable_name_detector} - src/main_detector_node.cpp + src/collision_detector_main.cpp ) ament_target_dependencies(${library_name} @@ -125,8 +125,7 @@ endif() ### Ament stuff ### ament_export_include_directories(include) -ament_export_libraries(${library_name}) -ament_export_libraries(${library_name_detector}) +ament_export_libraries(${library_name} ${library_name_detector}) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp similarity index 98% rename from nav2_collision_monitor/include/nav2_collision_monitor/detector_node.hpp rename to nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 869712f7040..5cf68dd46ac 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -140,12 +140,9 @@ class Detector : public nav2_util::LifecycleNode /// @brief Data sources array std::vector> sources_; - - /// @brief Whether main routine is active - bool process_active_; /// @brief collision monitor state publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr trigger_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr detections_pub_; /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; diff --git a/nav2_collision_monitor/src/main_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_main.cpp similarity index 93% rename from nav2_collision_monitor/src/main_detector_node.cpp rename to nav2_collision_monitor/src/collision_detector_main.cpp index a37aa0408a3..e68882faa94 100644 --- a/nav2_collision_monitor/src/main_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_main.cpp @@ -16,7 +16,7 @@ #include "rclcpp/rclcpp.hpp" -#include "nav2_collision_monitor/detector_node.hpp" +#include "nav2_collision_monitor/collision_detector_node.hpp" int main(int argc, char * argv[]) { diff --git a/nav2_collision_monitor/src/detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp similarity index 92% rename from nav2_collision_monitor/src/detector_node.cpp rename to nav2_collision_monitor/src/collision_detector_node.cpp index b68503bf619..06a6ab61196 100644 --- a/nav2_collision_monitor/src/detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nav2_collision_monitor/detector_node.hpp" +#include "nav2_collision_monitor/collision_detector_node.hpp" #include #include @@ -22,16 +22,13 @@ #include "nav2_util/node_utils.hpp" -#include "nav2_collision_monitor/kinematics.hpp" - using namespace std::chrono_literals; namespace nav2_collision_monitor { Detector::Detector(const rclcpp::NodeOptions & options) -: nav2_util::LifecycleNode("detector", "", options), - process_active_(false) +: nav2_util::LifecycleNode("collision_detector", "", options) { } @@ -54,16 +51,13 @@ Detector::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); - // Creating timer - timer_ = this->create_wall_timer( - 100ms, - std::bind(&Detector::process, this)); - - trigger_pub_ = this->create_publisher( + detections_pub_ = this->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); + std::string cmd_vel_in_topic; + std::string cmd_vel_out_topic; // Obtaining ROS parameters - if (!getParameters()) { + if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) { return nav2_util::CallbackReturn::FAILURE; } @@ -76,15 +70,17 @@ Detector::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); // Activating lifecycle publisher - trigger_pub_->on_activate(); + detections_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { polygon->activate(); } - // Activating main worker - process_active_ = true; + // Creating timer + timer_ = this->create_wall_timer( + 100ms, + std::bind(&Detector::process, this)); // Creating bond connection createBond(); @@ -98,16 +94,16 @@ Detector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); // Deactivating lifecycle publishers - trigger_pub_->on_deactivate(); - - // Deactivating main worker - process_active_ = false; + detections_pub_->on_deactivate(); // Deactivating polygons for (std::shared_ptr polygon : polygons_) { polygon->deactivate(); } + // Resetting timer + timer_.reset(); + // Destroying bond connection destroyBond(); @@ -119,7 +115,7 @@ Detector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - trigger_pub_.reset(); + detections_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -138,7 +134,6 @@ Detector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } - bool Detector::getParameters() { std::string base_frame_id, odom_frame_id; @@ -282,11 +277,6 @@ void Detector::process() // Current timestamp for all inner routines prolongation rclcpp::Time curr_time = this->now(); - // Do nothing if main worker in non-active state - if (!process_active_) { - return; - } - // Points array collected from different data sources in a robot base frame std::vector collision_points; @@ -295,13 +285,13 @@ void Detector::process() source->getData(curr_time, collision_points); } - std::unique_ptr trigger_msg = + std::unique_ptr detections_msg = std::make_unique(); for (std::shared_ptr polygon : polygons_) { - trigger_msg->data = polygon->getPointsInside(collision_points) > polygon->getMaxPoints(); + detections_msg->data = polygon->getPointsInside(collision_points) > polygon->getMaxPoints(); } - trigger_pub_->publish(std::move(trigger_msg)); + detections_pub_->publish(std::move(detections_msg)); // Publish polygons for better visualization publishPolygons(); From 2c163fdd0c024ef6d9ff8cea580960ba9de7926c Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 28 Mar 2023 12:47:51 +0200 Subject: [PATCH 26/60] fixes --- nav2_collision_monitor/CMakeLists.txt | 38 +++++++++---------- .../collision_detector_node.hpp | 12 +++--- .../src/collision_detector_main.cpp | 2 +- .../src/collision_detector_node.cpp | 32 ++++++++-------- 4 files changed, 41 insertions(+), 43 deletions(-) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 81f04f2975f..dd47909c26a 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -37,12 +37,12 @@ set(dependencies nav2_costmap_2d ) -set(executable_name collision_monitor) -set(executable_name_detector collision_detector) -set(library_name ${executable_name}_core) -set(library_name_detector ${executable_name_detector}_core) +set(monitor_executable_name collision_monitor) +set(detector_executable_name collision_detector) +set(monitor_library_name ${monitor_executable_name}_core) +set(detector_library_name ${detector_executable_name}_core) -add_library(${library_name} SHARED +add_library(${monitor_library_name} SHARED src/collision_monitor_node.cpp src/polygon.cpp src/circle.cpp @@ -52,7 +52,7 @@ add_library(${library_name} SHARED src/range.cpp src/kinematics.cpp ) -add_library(${library_name_detector} SHARED +add_library(${detector_library_name} SHARED src/collision_detector_node.cpp src/polygon.cpp src/circle.cpp @@ -63,42 +63,42 @@ add_library(${library_name_detector} SHARED src/kinematics.cpp ) -add_executable(${executable_name} +add_executable(${monitor_executable_name} src/main.cpp ) -add_executable(${executable_name_detector} +add_executable(${detector_executable_name} src/collision_detector_main.cpp ) -ament_target_dependencies(${library_name} +ament_target_dependencies(${monitor_library_name} ${dependencies} ) -ament_target_dependencies(${library_name_detector} +ament_target_dependencies(${detector_library_name} ${dependencies} ) -target_link_libraries(${executable_name} - ${library_name} +target_link_libraries(${monitor_executable_name} + ${monitor_library_name} ) -target_link_libraries(${executable_name_detector} - ${library_name_detector} +target_link_libraries(${detector_executable_name} + ${detector_library_name} ) -ament_target_dependencies(${executable_name} +ament_target_dependencies(${monitor_executable_name} ${dependencies} ) -rclcpp_components_register_nodes(${library_name} "nav2_collision_monitor::CollisionMonitor") +rclcpp_components_register_nodes(${monitor_library_name} "nav2_collision_monitor::CollisionMonitor") ### Install ### -install(TARGETS ${library_name} +install(TARGETS ${monitor_library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS ${executable_name} ${executable_name_detector} +install(TARGETS ${monitor_executable_name} ${detector_executable_name} RUNTIME DESTINATION lib/${PROJECT_NAME} ) @@ -125,7 +125,7 @@ endif() ### Ament stuff ### ament_export_include_directories(include) -ament_export_libraries(${library_name} ${library_name_detector}) +ament_export_libraries(${monitor_library_name} ${detector_library_name}) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 5cf68dd46ac..82226b6caa5 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -42,18 +42,18 @@ namespace nav2_collision_monitor /** * @brief Collision Monitor ROS2 node */ -class Detector : public nav2_util::LifecycleNode +class CollisionDetector : public nav2_util::LifecycleNode { public: /** - * @brief Constructor for the nav2_collision_monitor::Detector + * @brief Constructor for the nav2_collision_monitor::CollisionDetector * @param options Additional options to control creation of the node. */ - explicit Detector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + explicit CollisionDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** - * @brief Destructor for the nav2_collision_monitor::Detector + * @brief Destructor for the nav2_collision_monitor::CollisionDetector */ - ~Detector(); + ~CollisionDetector(); protected: /** @@ -146,7 +146,7 @@ class Detector : public nav2_util::LifecycleNode /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; -}; // class Detector +}; // class CollisionDetector } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/collision_detector_main.cpp b/nav2_collision_monitor/src/collision_detector_main.cpp index e68882faa94..e9aab093fcd 100644 --- a/nav2_collision_monitor/src/collision_detector_main.cpp +++ b/nav2_collision_monitor/src/collision_detector_main.cpp @@ -21,7 +21,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node->get_node_base_interface()); rclcpp::shutdown(); diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 06a6ab61196..9016652563d 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -27,19 +27,19 @@ using namespace std::chrono_literals; namespace nav2_collision_monitor { -Detector::Detector(const rclcpp::NodeOptions & options) +CollisionDetector::CollisionDetector(const rclcpp::NodeOptions & options) : nav2_util::LifecycleNode("collision_detector", "", options) { } -Detector::~Detector() +CollisionDetector::~CollisionDetector() { polygons_.clear(); sources_.clear(); } nav2_util::CallbackReturn -Detector::on_configure(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -54,10 +54,8 @@ Detector::on_configure(const rclcpp_lifecycle::State & /*state*/) detections_pub_ = this->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); - std::string cmd_vel_in_topic; - std::string cmd_vel_out_topic; // Obtaining ROS parameters - if (!getParameters(cmd_vel_in_topic, cmd_vel_out_topic)) { + if (!getParameters()) { return nav2_util::CallbackReturn::FAILURE; } @@ -65,7 +63,7 @@ Detector::on_configure(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -Detector::on_activate(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); @@ -80,7 +78,7 @@ Detector::on_activate(const rclcpp_lifecycle::State & /*state*/) // Creating timer timer_ = this->create_wall_timer( 100ms, - std::bind(&Detector::process, this)); + std::bind(&CollisionDetector::process, this)); // Creating bond connection createBond(); @@ -89,7 +87,7 @@ Detector::on_activate(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -Detector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); @@ -111,7 +109,7 @@ Detector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -Detector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); @@ -127,14 +125,14 @@ Detector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -Detector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) +CollisionDetector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Shutting down"); return nav2_util::CallbackReturn::SUCCESS; } -bool Detector::getParameters() +bool CollisionDetector::getParameters() { std::string base_frame_id, odom_frame_id; tf2::Duration transform_tolerance; @@ -168,7 +166,7 @@ bool Detector::getParameters() return true; } -bool Detector::configurePolygons( +bool CollisionDetector::configurePolygons( const std::string & base_frame_id, const tf2::Duration & transform_tolerance) { @@ -213,7 +211,7 @@ bool Detector::configurePolygons( return true; } -bool Detector::configureSources( +bool CollisionDetector::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, const tf2::Duration & transform_tolerance, @@ -272,7 +270,7 @@ bool Detector::configureSources( return true; } -void Detector::process() +void CollisionDetector::process() { // Current timestamp for all inner routines prolongation rclcpp::Time curr_time = this->now(); @@ -297,7 +295,7 @@ void Detector::process() publishPolygons(); } -void Detector::publishPolygons() const +void CollisionDetector::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { polygon->publish(); @@ -311,4 +309,4 @@ void Detector::publishPolygons() const // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::Detector) +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_collision_monitor::CollisionDetector) From d06566132c39ccb209c8febfc1f4879399e15f9e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 28 Mar 2023 13:04:09 +0200 Subject: [PATCH 27/60] add frequency as parameter --- .../nav2_collision_monitor/collision_detector_node.hpp | 4 +++- nav2_collision_monitor/src/collision_detector_node.cpp | 5 ++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 82226b6caa5..ce6205f4a7e 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -137,7 +137,6 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @brief Polygons array std::vector> polygons_; - /// @brief Data sources array std::vector> sources_; @@ -146,6 +145,9 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; + /// @brief main loop frequency + double frequency_; + }; // class CollisionDetector } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 9016652563d..ca8f12af93f 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -77,7 +77,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) // Creating timer timer_ = this->create_wall_timer( - 100ms, + std::chrono::duration{1.0 / frequency_}, std::bind(&CollisionDetector::process, this)); // Creating bond connection @@ -140,6 +140,9 @@ bool CollisionDetector::getParameters() auto node = shared_from_this(); + nav2_util::declare_parameter_if_not_declared( + node, "frequency", rclcpp::ParameterValue(10.0)); + frequency_ = get_parameter("frequency").as_double(); nav2_util::declare_parameter_if_not_declared( node, "base_frame_id", rclcpp::ParameterValue("base_footprint")); base_frame_id = get_parameter("base_frame_id").as_string(); From 8457e4849c47cd53bd1e80568100aad73907ddcc Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 28 Mar 2023 13:19:05 +0200 Subject: [PATCH 28/60] add CollisionDetectorState msg --- .../collision_detector_node.hpp | 4 ++-- .../src/collision_detector_node.cpp | 20 ++++++++++--------- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CollisionDetectorState.msg | 3 +++ 4 files changed, 17 insertions(+), 11 deletions(-) create mode 100644 nav2_msgs/msg/CollisionDetectorState.msg diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index ce6205f4a7e..5d3597e4351 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -20,13 +20,13 @@ #include #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "nav2_util/lifecycle_node.hpp" +#include "nav2_msgs/msg/collision_detector_state.hpp" #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/polygon.hpp" @@ -141,7 +141,7 @@ class CollisionDetector : public nav2_util::LifecycleNode std::vector> sources_; /// @brief collision monitor state publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr detections_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index ca8f12af93f..f35f4c08bd3 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -51,7 +51,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_buffer_->setCreateTimerInterface(timer_interface); tf_listener_ = std::make_shared(*tf_buffer_); - detections_pub_ = this->create_publisher( + state_pub_ = this->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); // Obtaining ROS parameters @@ -68,7 +68,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); // Activating lifecycle publisher - detections_pub_->on_activate(); + state_pub_->on_activate(); // Activating polygons for (std::shared_ptr polygon : polygons_) { @@ -76,7 +76,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) } // Creating timer - timer_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::duration{1.0 / frequency_}, std::bind(&CollisionDetector::process, this)); @@ -92,7 +92,7 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); // Deactivating lifecycle publishers - detections_pub_->on_deactivate(); + state_pub_->on_deactivate(); // Deactivating polygons for (std::shared_ptr polygon : polygons_) { @@ -113,7 +113,7 @@ CollisionDetector::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - detections_pub_.reset(); + state_pub_.reset(); polygons_.clear(); sources_.clear(); @@ -286,13 +286,15 @@ void CollisionDetector::process() source->getData(curr_time, collision_points); } - std::unique_ptr detections_msg = - std::make_unique(); + std::unique_ptr state_msg = + std::make_unique(); + for (std::shared_ptr polygon : polygons_) { - detections_msg->data = polygon->getPointsInside(collision_points) > polygon->getMaxPoints(); + state_msg->polygons.push_back(polygon->getName()); + state_msg->detections.push_back(polygon->getPointsInside(collision_points) > polygon->getMaxPoints()); } - detections_pub_->publish(std::move(detections_msg)); + state_pub_->publish(std::move(state_msg)); // Publish polygons for better visualization publishPolygons(); diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 72c84682eb1..2546603c9e1 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionMonitorState.msg" + "msg/CollisionDetectorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" "msg/CostmapFilterInfo.msg" diff --git a/nav2_msgs/msg/CollisionDetectorState.msg b/nav2_msgs/msg/CollisionDetectorState.msg new file mode 100644 index 00000000000..f6a56c01112 --- /dev/null +++ b/nav2_msgs/msg/CollisionDetectorState.msg @@ -0,0 +1,3 @@ +# Name of triggered polygon +string[] polygons +bool[] detections From 59872b6697f1be57f104a5ea2666e7d0a1a32c37 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 27 Jun 2023 17:10:16 +0200 Subject: [PATCH 29/60] change do_nothing to none --- nav2_collision_monitor/src/polygon.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 35b7cfe1027..cdd8796299e 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -258,7 +258,7 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) action_type_ = SLOWDOWN; } else if (at_str == "approach") { action_type_ = APPROACH; - } else if (at_str == "do_nothing") { + } else if (at_str == "none") { action_type_ = DO_NOTHING; } else { // Error if something else RCLCPP_ERROR(logger_, "[%s]: Unknown action type: %s", polygon_name_.c_str(), at_str.c_str()); From c2a178af74cd3623799d4b15f3bea582964316e8 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 27 Jun 2023 17:28:13 +0200 Subject: [PATCH 30/60] cmake fixes --- nav2_collision_monitor/CMakeLists.txt | 8 +++++++- nav2_collision_monitor/src/collision_detector_node.cpp | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 338b4d95a24..d013315e3c6 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -90,11 +90,17 @@ ament_target_dependencies(${monitor_executable_name} ${dependencies} ) +ament_target_dependencies(${detector_executable_name} + ${dependencies} +) + rclcpp_components_register_nodes(${monitor_library_name} "nav2_collision_monitor::CollisionMonitor") +rclcpp_components_register_nodes(${detector_library_name} "nav2_collision_monitor::CollisionDetector") + ### Install ### -install(TARGETS ${monitor_library_name} +install(TARGETS ${monitor_library_name} ${detector_library_name} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index f35f4c08bd3..a03e29b3fd4 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -76,7 +76,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) } // Creating timer - timer_ = this->create_timer( + timer_ = this->create_wall_timer( std::chrono::duration{1.0 / frequency_}, std::bind(&CollisionDetector::process, this)); From 95faccf84d38a270f9be187ad85625f11e3f0c89 Mon Sep 17 00:00:00 2001 From: tonynajjar Date: Fri, 30 Jun 2023 09:31:01 +0000 Subject: [PATCH 31/60] rename main --- nav2_collision_monitor/CMakeLists.txt | 2 +- .../src/{main.cpp => collision_monitor_main.cpp} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename nav2_collision_monitor/src/{main.cpp => collision_monitor_main.cpp} (100%) diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index d013315e3c6..51cafe71dd0 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -66,7 +66,7 @@ add_library(${detector_library_name} SHARED ) add_executable(${monitor_executable_name} - src/main.cpp + src/collision_monitor_main.cpp ) add_executable(${detector_executable_name} src/collision_detector_main.cpp diff --git a/nav2_collision_monitor/src/main.cpp b/nav2_collision_monitor/src/collision_monitor_main.cpp similarity index 100% rename from nav2_collision_monitor/src/main.cpp rename to nav2_collision_monitor/src/collision_monitor_main.cpp From 2fce80503842dbec5514f4cdc0d2598656eb5772 Mon Sep 17 00:00:00 2001 From: tonynajjar Date: Fri, 30 Jun 2023 09:35:41 +0000 Subject: [PATCH 32/60] fix uncrustify --- .../nav2_collision_monitor/collision_detector_node.hpp | 2 +- nav2_collision_monitor/src/collision_detector_node.cpp | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 5d3597e4351..216b53eae59 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -139,7 +139,7 @@ class CollisionDetector : public nav2_util::LifecycleNode std::vector> polygons_; /// @brief Data sources array std::vector> sources_; - + /// @brief collision monitor state publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; /// @brief timer that runs actions diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index a03e29b3fd4..4170f2f72a1 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -77,8 +77,8 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) // Creating timer timer_ = this->create_wall_timer( - std::chrono::duration{1.0 / frequency_}, - std::bind(&CollisionDetector::process, this)); + std::chrono::duration{1.0 / frequency_}, + std::bind(&CollisionDetector::process, this)); // Creating bond connection createBond(); @@ -291,7 +291,9 @@ void CollisionDetector::process() for (std::shared_ptr polygon : polygons_) { state_msg->polygons.push_back(polygon->getName()); - state_msg->detections.push_back(polygon->getPointsInside(collision_points) > polygon->getMaxPoints()); + state_msg->detections.push_back( + polygon->getPointsInside( + collision_points) > polygon->getMaxPoints()); } state_pub_->publish(std::move(state_msg)); From 97afa64a5a64ba884c556144d5d7f2b7a74db90c Mon Sep 17 00:00:00 2001 From: tonynajjar Date: Fri, 30 Jun 2023 09:49:54 +0000 Subject: [PATCH 33/60] fixes --- .../collision_detector_node.hpp | 7 +++++-- .../collision_monitor_node.hpp | 2 +- .../src/collision_detector_node.cpp | 20 +++++++++++++------ 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 216b53eae59..e5a86fa26fd 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -107,16 +107,19 @@ class CollisionDetector : public nav2_util::LifecycleNode * @brief Supporting routine creating and configuring all data sources * @param base_frame_id Robot base frame ID * @param odom_frame_id Odometry frame ID. Used as global frame to get - * source->base time inerpolated transform. + * source->base time interpolated transform. * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time * @return True if all sources were configured successfully or false in failure case */ bool configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout); + const rclcpp::Duration & source_timeout, + const bool base_shift_correction); /** * @brief Main processing routine diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp index 006b95028bf..c062abdcc35 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp @@ -127,7 +127,7 @@ class CollisionMonitor : public nav2_util::LifecycleNode * @brief Supporting routine creating and configuring all data sources * @param base_frame_id Robot base frame ID * @param odom_frame_id Odometry frame ID. Used as global frame to get - * source->base time inerpolated transform. + * source->base time interpolated transform. * @param transform_tolerance Transform tolerance * @param source_timeout Maximum time interval in which data is considered valid * @param base_shift_correction Whether to correct source data towards to base frame movement, diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4170f2f72a1..50a1cefd090 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -157,12 +157,19 @@ bool CollisionDetector::getParameters() node, "source_timeout", rclcpp::ParameterValue(2.0)); source_timeout = rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double()); + nav2_util::declare_parameter_if_not_declared( + node, "base_shift_correction", rclcpp::ParameterValue(true)); + const bool base_shift_correction = + get_parameter("base_shift_correction").as_bool(); if (!configurePolygons(base_frame_id, transform_tolerance)) { return false; } - if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) { + if (!configureSources( + base_frame_id, odom_frame_id, transform_tolerance, source_timeout, + base_shift_correction)) + { return false; } @@ -218,7 +225,8 @@ bool CollisionDetector::configureSources( const std::string & base_frame_id, const std::string & odom_frame_id, const tf2::Duration & transform_tolerance, - const rclcpp::Duration & source_timeout) + const rclcpp::Duration & source_timeout, + const bool base_shift_correction) { try { auto node = shared_from_this(); @@ -236,7 +244,7 @@ bool CollisionDetector::configureSources( if (source_type == "scan") { std::shared_ptr s = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); s->configure(); @@ -244,7 +252,7 @@ bool CollisionDetector::configureSources( } else if (source_type == "pointcloud") { std::shared_ptr p = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); p->configure(); @@ -252,7 +260,7 @@ bool CollisionDetector::configureSources( } else if (source_type == "range") { std::shared_ptr r = std::make_shared( node, source_name, tf_buffer_, base_frame_id, odom_frame_id, - transform_tolerance, source_timeout); + transform_tolerance, source_timeout, base_shift_correction); r->configure(); @@ -293,7 +301,7 @@ void CollisionDetector::process() state_msg->polygons.push_back(polygon->getName()); state_msg->detections.push_back( polygon->getPointsInside( - collision_points) > polygon->getMaxPoints()); + collision_points) > polygon->getMinPoints()); } state_pub_->publish(std::move(state_msg)); From 00c9cdd831f8a56fe44ffb215a3032d5d69b2377 Mon Sep 17 00:00:00 2001 From: tonynajjar Date: Fri, 30 Jun 2023 10:11:16 +0000 Subject: [PATCH 34/60] fix build --- nav2_collision_monitor/test/CMakeLists.txt | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index 77870826b88..f84e83ff218 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -4,7 +4,7 @@ ament_target_dependencies(kinematics_test ${dependencies} ) target_link_libraries(kinematics_test - ${library_name} + ${monitor_library_name} ) # Data sources test @@ -13,7 +13,7 @@ ament_target_dependencies(sources_test ${dependencies} ) target_link_libraries(sources_test - ${library_name} + ${monitor_library_name} ) # Polygon shapes test @@ -22,7 +22,7 @@ ament_target_dependencies(polygons_test ${dependencies} ) target_link_libraries(polygons_test - ${library_name} + ${monitor_library_name} ) # Collision Monitor node test @@ -31,5 +31,5 @@ ament_target_dependencies(collision_monitor_node_test ${dependencies} ) target_link_libraries(collision_monitor_node_test - ${library_name} + ${monitor_library_name} ) From ef5f8cf4222dacd20e59be0caf12872d0b596a91 Mon Sep 17 00:00:00 2001 From: root Date: Fri, 30 Jun 2023 12:10:45 +0000 Subject: [PATCH 35/60] add warning if action type is not none --- .../collision_detector_node.hpp | 1 + .../src/collision_detector_main.cpp | 2 +- .../src/collision_detector_node.cpp | 17 ++++++++++++++--- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index e5a86fa26fd..31371b3a2f6 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/collision_detector_main.cpp b/nav2_collision_monitor/src/collision_detector_main.cpp index e9aab093fcd..c4ef21b7d64 100644 --- a/nav2_collision_monitor/src/collision_detector_main.cpp +++ b/nav2_collision_monitor/src/collision_detector_main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 50a1cefd090..e7a16977a78 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -1,4 +1,5 @@ -// Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -52,7 +53,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_listener_ = std::make_shared(*tf_buffer_); state_pub_ = this->create_publisher( - "~/state", rclcpp::SystemDefaultsQoS()); + "collision_detector_state", rclcpp::SystemDefaultsQoS()); // Obtaining ROS parameters if (!getParameters()) { @@ -76,7 +77,7 @@ CollisionDetector::on_activate(const rclcpp_lifecycle::State & /*state*/) } // Creating timer - timer_ = this->create_wall_timer( + timer_ = this->create_timer( std::chrono::duration{1.0 / frequency_}, std::bind(&CollisionDetector::process, this)); @@ -208,6 +209,16 @@ bool CollisionDetector::configurePolygons( return false; } + // warn if the added polygon's action_type_ is not different than "none" + auto action_type = polygons_.back()->getActionType(); + if (action_type != DO_NOTHING) { + RCLCPP_WARN( + get_logger(), + "[%s]: The action_type of the polygon is different than \"none\" which is \ + not supported in the collision detector; it will be considered as \"none\".", + polygon_name.c_str()); + } + // Configure last added polygon if (!polygons_.back()->configure()) { return false; From 99a58ba69899b2d8e8721e8c532b236e2731d90d Mon Sep 17 00:00:00 2001 From: root Date: Fri, 30 Jun 2023 14:16:10 +0000 Subject: [PATCH 36/60] adding launch file --- .../launch/collision_detector_node.launch.py | 150 ++++++++++++++++++ .../params/collision_detector_params.yaml | 24 +++ 2 files changed, 174 insertions(+) create mode 100644 nav2_collision_monitor/launch/collision_detector_node.launch.py create mode 100644 nav2_collision_monitor/params/collision_detector_params.yaml diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py new file mode 100644 index 00000000000..e695c9a351a --- /dev/null +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2022 Samsung R&D Institute 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, GroupAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import NotEqualsSubstitution +from launch_ros.actions import LoadComposableNodes, SetParameter +from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Environment + package_dir = get_package_share_directory('nav2_collision_monitor') + + # Constant parameters + lifecycle_nodes = ['collision_detector'] + autostart = True + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Launch arguments + # 1. Create the launch configuration variables + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + + # 2. Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites={}, + convert_types=True) + + # Declare node launching commands + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(['not ', use_composition])), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushRosNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_collision_detector', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings), + Node( + package='nav2_collision_monitor', + executable='collision_detector', + output='screen', + emulate_tty=True, # https://github.com/ros2/launch/issues/188 + parameters=[configured_params], + remappings=remappings) + ] + ) + + load_composable_nodes = GroupAction( + condition=IfCondition(use_composition), + actions=[ + SetParameter('use_sim_time', use_sim_time), + PushRosNamespace( + condition=IfCondition(NotEqualsSubstitution(LaunchConfiguration('namespace'), '')), + namespace=namespace), + LoadComposableNodes( + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_collision_detector', + parameters=[{'autostart': autostart}, + {'node_names': lifecycle_nodes}], + remappings=remappings), + ComposableNode( + package='nav2_collision_monitor', + plugin='nav2_collision_monitor::CollisionDetector', + name='collision_detector', + parameters=[configured_params], + remappings=remappings) + ], + ) + ] + ) + + ld = LaunchDescription() + + # Launch arguments + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + + # Node launching commands + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml new file mode 100644 index 00000000000..f7fafabedb5 --- /dev/null +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -0,0 +1,24 @@ +collision_detector: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + transform_tolerance: 0.5 + source_timeout: 5.0 + base_shift_correction: True + polygons: ["PolygonFront"] + PolygonFront: + type: "polygon" + points: [0.3, 0.3, 0.3, -0.3, 0.0, -0.3, 0.0, 0.3] + action_type: "none" + min_points: 4 + visualize: True + polygon_pub_topic: "polygon_front" + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + pointcloud: + type: "pointcloud" + topic: "/intel_realsense_r200_depth/points" + min_height: 0.1 + max_height: 0.5 From 395fc51f4258e8aa1fa206ed415e771449f7da3b Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 20 Jul 2023 12:02:20 +0200 Subject: [PATCH 37/60] Delete .drone.yml --- .drone.yml | 82 ------------------------------------------------------ 1 file changed, 82 deletions(-) delete mode 100644 .drone.yml diff --git a/.drone.yml b/.drone.yml deleted file mode 100644 index f5301e7a2cd..00000000000 --- a/.drone.yml +++ /dev/null @@ -1,82 +0,0 @@ ---- -# 1. 1/2 Starting docker build with custom tag -kind: pipeline -type: docker -name: PR validation build docker image - -platform: - arch: amd64 - os: linux - -trigger: - event: - include: - - pull_request - -volumes: -- name: dockersock - host: - path: /var/run/docker.sock - - -steps: -- name: Update Dockerfile with nav2 PR branch - commands: - - sed - - sed -i "/ARG NAV2_DEFAULT_BRANCH=/c\ARG NAV2_DEFAULT_BRANCH=${DRONE_SOURCE_BRANCH}" docker/MlAllDockerfile - -- name: Build ml_all docker image for pull request - image: plugins/docker - volumes: - - name: dockersock - path: /var/run/docker.sock - settings: - dockerfile: docker/MlAllDockerfile - context: docker/ - registry: quay.io - repo: quay.io/logivations/ml_all - privileged: true - tags: - - drone_nav2_${DRONE_PULL_REQUEST} - username: - from_secret: DOCKER_QUAY_USERNAME - password: - from_secret: DOCKER_QUAY_PASSWORD - depends_on: - - Update Dockerfile with nav2 PR branch - ---- -# 1. 2/2 Run unit tests validation on each open pull request -kind: pipeline -type: exec -name: PR validation run unit tests - -platform: - arch: amd64 - os: linux - -trigger: - event: - include: - - pull_request - -image_pull_secrets: -- dockerconfig_prd - -depends_on: -- PR validation build docker image - -steps: -- name: Start docker container - commands: - - docker rm -f ${DRONE_COMMIT} || true - - docker pull quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} - - docker run -dti --name ${DRONE_COMMIT} --gpus=all -e NVIDIA_VISIBLE_DEVICES=all -e PYTHONHASHSEED=0 -v /opt/data/DeepCVTest:/data/DeepCVTest -v /opt/data/ml_models/:/data/ml_models -v $(pwd):/data/workspace/deep_cv quay.io/logivations/ml_all:drone_nav2_${DRONE_PULL_REQUEST} - -- name: Start unit tests - commands: - - docker exec ${DRONE_COMMIT} bash -ic "export TEAMCITY=TRUE; export ROS_LOCALHOST_ONLY=1; export CYCLONEDDS_URI=/data/workspace/deep_cv/scripts/agv/cyclonedds_open_network.xml; cd /data/workspace/deep_cv; pytest -v --color=yes --forked -n 2 --ignore-glob=**/disabled_tests/* lv/" - -- name: Remove docker container - commands: - - docker rm -f ${DRONE_COMMIT} From a2ec3a95e16b9d03d23173d2bb7083617c74acb3 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 20 Jul 2023 15:12:51 +0200 Subject: [PATCH 38/60] fixes --- nav2_collision_monitor/README.md | 27 ++++++++++++------- .../collision_detector_node.hpp | 10 +++---- .../launch/collision_detector_node.launch.py | 2 +- .../src/collision_detector_node.cpp | 7 +++-- 4 files changed, 26 insertions(+), 20 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 134a52cff61..c5560569496 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -1,5 +1,7 @@ # Nav2 Collision Monitor +## Collision Monitor + The Collision Monitor is a node providing an additional level of robot safety. It performs several collision avoidance related tasks using incoming data from the sensors, bypassing the costmap and trajectory planners, to monitor for and prevent potential collisions at the emergency-stop level. @@ -12,7 +14,7 @@ The costmaps / trajectory planners will handle most situations, but this is to h ![polygons.png](doc/polygons.png) -## Features +### Features The Collision Monitor uses polygons relative the robot's base frame origin to define "zones". Data that fall into these zones trigger an operation depending on the model being used. @@ -23,14 +25,13 @@ The following models of safety behaviors are employed by Collision Monitor: * **Stop model**: Define a zone and a point threshold. If more that `N` obstacle points appear inside this area, stop the robot until the obstacles will disappear. * **Slowdown model**: Define a zone around the robot and slow the maximum speed for a `%S` percent, if more than `N` points will appear inside the area. -* **Limit model**: Define a zone around the robot and clamp the maximum speed below a fixed value, if more than `N` points will appear inside the area. * **Approach model**: Using the current robot speed, estimate the time to collision to sensor data. If the time is less than `M` seconds (0.5, 2, 5, etc...), the robot will slow such that it is now at least `M` seconds to collision. The effect here would be to keep the robot always `M` seconds from any collision. The zones around the robot can take the following shapes: -* Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface. -* Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time. -* Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape. +* Arbitrary user-defined polygon relative to the robot base frame. +* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape. +* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time. The data may be obtained from different data sources: @@ -38,26 +39,26 @@ The data may be obtained from different data sources: * PointClouds (`sensor_msgs::msg::PointCloud2` messages) * IR/Sonars (`sensor_msgs::msg::Range` messages) -## Design +### Design The Collision Monitor is designed to operate below Nav2 as an independent safety node. This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate. The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. -![HLD.png](doc/HLD.png) +![HDL.png](doc/HLD.png) -## Configuration +### Configuration Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. -## Metrics +### Metrics Designed to be used in wide variety of robots (incl. moving fast) and have a high level of reliability, Collision Monitor node should operate at fast rates. Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points). The table below represents the details of operating times for different behavior models and shapes: -| | Stop/Slowdown/Limit model, Polygon area | Stop/Slowdown/Limit model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | +| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | |-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------| | LaserScan (360 points) processing time, ms | 4.45 | 4.45 | 4.93 | 4.86 | | PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 | @@ -66,3 +67,9 @@ The following notes could be made: * Due to sheer speed, circle shapes are preferred for the approach behavior models if you can approximately model your robot as circular. * More points mean lower performance. Pointclouds could be culled or filtered before the Collision Monitor to improve performance. + + +## Collision Detector + +Another node exists in the nav2_collision_monitor package called the Collision Detector. This node works similarly to the collision monitor +except that it does not affect the robot's velocity. It will only inform that data from the confirgured sources has been detected within the configured polygons via message to the `/collision_detector_state` topic. \ No newline at end of file diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 31371b3a2f6..618df25e622 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_COLLISION_MONITOR__DETECTOR_NODE_HPP_ -#define NAV2_COLLISION_MONITOR__DETECTOR_NODE_HPP_ +#ifndef NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ +#define NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ #include #include @@ -145,8 +145,8 @@ class CollisionDetector : public nav2_util::LifecycleNode std::vector> sources_; /// @brief collision monitor state publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; - /// @brief timer that runs actions + rclcpp_lifecycle::LifecyclePublisher::SharedPtr \ + state_pub_; /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; /// @brief main loop frequency @@ -156,4 +156,4 @@ class CollisionDetector : public nav2_util::LifecycleNode } // namespace nav2_collision_monitor -#endif // NAV2_COLLISION_MONITOR__DETECTOR_NODE_HPP_ +#endif // NAV2_COLLISION_MONITOR__COLLISION_DETECTOR_NODE_HPP_ diff --git a/nav2_collision_monitor/launch/collision_detector_node.launch.py b/nav2_collision_monitor/launch/collision_detector_node.launch.py index e695c9a351a..ccd58c2e158 100644 --- a/nav2_collision_monitor/launch/collision_detector_node.launch.py +++ b/nav2_collision_monitor/launch/collision_detector_node.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright (c) 2022 Samsung R&D Institute Russia +# Copyright (c) 2023 Pixel Robotics GmbH # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index e7a16977a78..570fafd7f46 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -92,6 +92,9 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); + // Resetting timer + timer_.reset(); + // Deactivating lifecycle publishers state_pub_->on_deactivate(); @@ -100,9 +103,6 @@ CollisionDetector::on_deactivate(const rclcpp_lifecycle::State & /*state*/) polygon->deactivate(); } - // Resetting timer - timer_.reset(); - // Destroying bond connection destroyBond(); @@ -129,7 +129,6 @@ nav2_util::CallbackReturn CollisionDetector::on_shutdown(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Shutting down"); - return nav2_util::CallbackReturn::SUCCESS; } From 49bd4f118545b8f4ae51d999e95e50173bb53a9d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 20 Jul 2023 15:14:47 +0200 Subject: [PATCH 39/60] uncrustify --- .../include/nav2_collision_monitor/collision_detector_node.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 618df25e622..f2aeec434b6 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -146,7 +146,8 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @brief collision monitor state publisher rclcpp_lifecycle::LifecyclePublisher::SharedPtr \ - state_pub_; /// @brief timer that runs actions + state_pub_; + /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; /// @brief main loop frequency From c5055ee696028c365e72513e884022e5a3816afa Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 20 Jul 2023 15:34:37 +0200 Subject: [PATCH 40/60] . --- .../nav2_collision_monitor/collision_detector_node.hpp | 1 - nav2_collision_monitor/src/collision_detector_node.cpp | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index f2aeec434b6..403376569a7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -152,7 +152,6 @@ class CollisionDetector : public nav2_util::LifecycleNode /// @brief main loop frequency double frequency_; - }; // class CollisionDetector } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 570fafd7f46..c1971d1a840 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -213,8 +213,8 @@ bool CollisionDetector::configurePolygons( if (action_type != DO_NOTHING) { RCLCPP_WARN( get_logger(), - "[%s]: The action_type of the polygon is different than \"none\" which is \ - not supported in the collision detector; it will be considered as \"none\".", + "[%s]: The action_type of the polygon is different than \"none\" which is " + + "not supported in the collision detector; it will be considered as \"none\".", polygon_name.c_str()); } From 57c212ee8f14020a73cf42e44a11114c9e5e4d70 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 20 Jul 2023 15:57:12 +0200 Subject: [PATCH 41/60] . --- nav2_collision_monitor/src/collision_detector_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index c1971d1a840..e2072b5ed3d 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -213,7 +213,7 @@ bool CollisionDetector::configurePolygons( if (action_type != DO_NOTHING) { RCLCPP_WARN( get_logger(), - "[%s]: The action_type of the polygon is different than \"none\" which is " + + "[%s]: The action_type of the polygon is different than \"none\" which is " "not supported in the collision detector; it will be considered as \"none\".", polygon_name.c_str()); } From d9073bc930b7b28d294732d0926f367dc70674cc Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 21 Jul 2023 10:36:30 +0200 Subject: [PATCH 42/60] Update README.md --- nav2_collision_monitor/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index c5560569496..87a553b45af 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -72,4 +72,4 @@ The following notes could be made: ## Collision Detector Another node exists in the nav2_collision_monitor package called the Collision Detector. This node works similarly to the collision monitor -except that it does not affect the robot's velocity. It will only inform that data from the confirgured sources has been detected within the configured polygons via message to the `/collision_detector_state` topic. \ No newline at end of file +except that it does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `/collision_detector_state` topic. From 20c73197e84b025963061ac19113414d47d88900 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 21 Jul 2023 16:59:49 +0200 Subject: [PATCH 43/60] Adding tests --- .../src/collision_detector_node.cpp | 3 +- .../src/collision_monitor_node.cpp | 5 +- nav2_collision_monitor/test/CMakeLists.txt | 12 +- .../test/collision_detector_node_test.cpp | 598 ++++++++++++++++++ 4 files changed, 610 insertions(+), 8 deletions(-) create mode 100644 nav2_collision_monitor/test/collision_detector_node_test.cpp diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index e2072b5ed3d..11f8a690df9 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -183,8 +183,9 @@ bool CollisionDetector::configurePolygons( try { auto node = shared_from_this(); + // Leave it to be not initialized: to intentionally cause an error if it will not set nav2_util::declare_parameter_if_not_declared( - node, "polygons", rclcpp::ParameterValue(std::vector())); + node, "polygons", rclcpp::PARAMETER_STRING_ARRAY); std::vector polygon_names = get_parameter("polygons").as_string_array(); for (std::string polygon_name : polygon_names) { // Leave it not initialized: the will cause an error if it will not set diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 2bae91e2a0c..ccf4b3d5f00 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -258,9 +258,10 @@ bool CollisionMonitor::configurePolygons( { try { auto node = shared_from_this(); - + + // Leave it to be not initialized: to intentionally cause an error if it will not set nav2_util::declare_parameter_if_not_declared( - node, "polygons", rclcpp::ParameterValue(std::vector())); + node, "polygons", rclcpp::PARAMETER_STRING_ARRAY); std::vector polygon_names = get_parameter("polygons").as_string_array(); for (std::string polygon_name : polygon_names) { // Leave it not initialized: the will cause an error if it will not set diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index f84e83ff218..602970c0340 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -26,10 +26,12 @@ target_link_libraries(polygons_test ) # Collision Monitor node test -ament_add_gtest(collision_monitor_node_test collision_monitor_node_test.cpp) -ament_target_dependencies(collision_monitor_node_test + +# Collision Detector node test +ament_add_gtest(collision_detector_node_test collision_detector_node_test.cpp) +ament_target_dependencies(collision_detector_node_test ${dependencies} ) -target_link_libraries(collision_monitor_node_test - ${monitor_library_name} -) +target_link_libraries(collision_detector_node_test + ${detector_library_name} +) \ No newline at end of file diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp new file mode 100644 index 00000000000..8016a91cd1d --- /dev/null +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -0,0 +1,598 @@ +// Copyright (c) 2022 Samsung R&D Institute 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 +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/range.hpp" +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" + +#include "tf2_ros/transform_broadcaster.h" + +#include "nav2_collision_monitor/types.hpp" +#include "nav2_collision_monitor/collision_detector_node.hpp" + +using namespace std::chrono_literals; + +static constexpr double EPSILON = 1e-5; + +static const char BASE_FRAME_ID[]{"base_link"}; +static const char SOURCE_FRAME_ID[]{"base_source"}; +static const char ODOM_FRAME_ID[]{"odom"}; +static const char FOOTPRINT_TOPIC[]{"footprint"}; +static const char SCAN_NAME[]{"Scan"}; +static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char STATE_TOPIC[]{"collision_detector_state"}; +static const int MAX_POINTS{1}; +static const double SIMULATION_TIME_STEP{0.01}; +static const double TRANSFORM_TOLERANCE{0.5}; +static const double SOURCE_TIMEOUT{5.0}; + +enum PolygonType +{ + POLYGON_UNKNOWN = 0, + POLYGON = 1, + CIRCLE = 2 +}; + +enum SourceType +{ + SOURCE_UNKNOWN = 0, + SCAN = 1, + POINTCLOUD = 2, + RANGE = 3 +}; + +class CollisionDetectorWrapper : public nav2_collision_monitor::CollisionDetector +{ +public: + void start() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_activate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void stop() + { + ASSERT_EQ(on_deactivate(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_cleanup(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + ASSERT_EQ(on_shutdown(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::SUCCESS); + } + + void cant_configure() + { + ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); + } + + bool correctDataReceived(const double expected_dist, const rclcpp::Time & stamp) + { + for (std::shared_ptr source : sources_) { + std::vector collision_points; + source->getData(stamp, collision_points); + if (collision_points.size() != 0) { + const double dist = std::hypot(collision_points[0].x, collision_points[0].y); + if (std::fabs(dist - expected_dist) <= EPSILON) { + return true; + } + } + } + return false; + } +}; // CollisionDetectorWrapper + +class Tester : public ::testing::Test +{ +public: + Tester(); + ~Tester(); + + // Configuring + void setCommonParameters(); + void addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at); + void addSource(const std::string & source_name, const SourceType type); + void setVectors( + const std::vector & polygons, + const std::vector & sources); + + // Setting TF chains + void sendTransforms(const rclcpp::Time & stamp); + + // Publish robot footprint + void publishFootprint(const double radius, const rclcpp::Time & stamp); + + // Main topic/data working routines + void publishScan(const double dist, const rclcpp::Time & stamp); + void publishPointCloud(const double dist, const rclcpp::Time & stamp); + void publishRange(const double dist, const rclcpp::Time & stamp); + bool waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp); + bool waitState(const std::chrono::nanoseconds & timeout); + void stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg); + +protected: + // CollisionMonitor node + std::shared_ptr cd_; + + // Data source publishers + rclcpp::Publisher::SharedPtr scan_pub_; + rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; + + rclcpp::Subscription::SharedPtr state_sub_; + nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; + + +}; // Tester + +Tester::Tester() +{ + cd_ = std::make_shared(); + + scan_pub_ = cd_->create_publisher( + SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + pointcloud_pub_ = cd_->create_publisher( + POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + range_pub_ = cd_->create_publisher( + RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + state_sub_ = cd_->create_subscription( + STATE_TOPIC, rclcpp::SystemDefaultsQoS(), + std::bind(&Tester::stateCallback, this, std::placeholders::_1)); +} + +Tester::~Tester() +{ + scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.reset(); + + cd_.reset(); +} + +bool Tester::waitState(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = cd_->now(); + while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) { + if (state_msg_) { + return true; + } + rclcpp::spin_some(cd_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + +void Tester::stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg) +{ + state_msg_ = msg; +} + + +void Tester::setCommonParameters() +{ + cd_->declare_parameter( + "base_frame_id", rclcpp::ParameterValue(BASE_FRAME_ID)); + cd_->set_parameter( + rclcpp::Parameter("base_frame_id", BASE_FRAME_ID)); + cd_->declare_parameter( + "odom_frame_id", rclcpp::ParameterValue(ODOM_FRAME_ID)); + cd_->set_parameter( + rclcpp::Parameter("odom_frame_id", ODOM_FRAME_ID)); + + cd_->declare_parameter( + "transform_tolerance", rclcpp::ParameterValue(TRANSFORM_TOLERANCE)); + cd_->set_parameter( + rclcpp::Parameter("transform_tolerance", TRANSFORM_TOLERANCE)); + cd_->declare_parameter( + "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); + cd_->set_parameter( + rclcpp::Parameter("source_timeout", SOURCE_TIMEOUT)); +} + +void Tester::addPolygon( + const std::string & polygon_name, const PolygonType type, + const double size, const std::string & at) +{ + if (type == POLYGON) { + cd_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("polygon")); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "polygon")); + + if (at != "approach") { + const std::vector points { + size, size, size, -size, -size, -size, -size, size}; + cd_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); + } else { // at == "approach" + cd_->declare_parameter( + polygon_name + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".footprint_topic", FOOTPRINT_TOPIC)); + } + } else if (type == CIRCLE) { + cd_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("circle")); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "circle")); + + cd_->declare_parameter( + polygon_name + ".radius", rclcpp::ParameterValue(size)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".radius", size)); + } else { // type == POLYGON_UNKNOWN + cd_->declare_parameter( + polygon_name + ".type", rclcpp::ParameterValue("unknown")); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".type", "unknown")); + } + + cd_->declare_parameter( + polygon_name + ".action_type", rclcpp::ParameterValue(at)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".action_type", at)); + + cd_->declare_parameter( + polygon_name + ".max_points", rclcpp::ParameterValue(MAX_POINTS)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".max_points", MAX_POINTS)); + + cd_->declare_parameter( + polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".simulation_time_step", SIMULATION_TIME_STEP)); + + cd_->declare_parameter( + polygon_name + ".visualize", rclcpp::ParameterValue(false)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".visualize", false)); + + cd_->declare_parameter( + polygon_name + ".polygon_pub_topic", rclcpp::ParameterValue(polygon_name)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".polygon_pub_topic", polygon_name)); +} + +void Tester::addSource( + const std::string & source_name, const SourceType type) +{ + if (type == SCAN) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("scan")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "scan")); + } else if (type == POINTCLOUD) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("pointcloud")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "pointcloud")); + + cd_->declare_parameter( + source_name + ".min_height", rclcpp::ParameterValue(0.1)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".min_height", 0.1)); + cd_->declare_parameter( + source_name + ".max_height", rclcpp::ParameterValue(1.0)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".max_height", 1.0)); + } else if (type == RANGE) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("range")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "range")); + + cd_->declare_parameter( + source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); + } else { // type == SOURCE_UNKNOWN + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("unknown")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "unknown")); + } + + cd_->declare_parameter( + source_name + ".topic", rclcpp::ParameterValue(source_name)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".topic", source_name)); +} + +void Tester::setVectors( + const std::vector & polygons, + const std::vector & sources) +{ + cd_->declare_parameter("polygons", rclcpp::ParameterValue(polygons)); + cd_->set_parameter(rclcpp::Parameter("polygons", polygons)); + + cd_->declare_parameter("observation_sources", rclcpp::ParameterValue(sources)); + cd_->set_parameter(rclcpp::Parameter("observation_sources", sources)); +} + +void Tester::sendTransforms(const rclcpp::Time & stamp) +{ + std::shared_ptr tf_broadcaster = + std::make_shared(cd_); + + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + // Fill TF buffer ahead for future transform usage in CollisionMonitor::process() + const rclcpp::Duration ahead = 1000ms; + for (rclcpp::Time t = stamp; t <= stamp + ahead; t += rclcpp::Duration(50ms)) { + transform.header.stamp = t; + + // base_frame -> source_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = SOURCE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + + // odom_frame -> base_frame transform + transform.header.frame_id = ODOM_FRAME_ID; + transform.child_frame_id = BASE_FRAME_ID; + tf_broadcaster->sendTransform(transform); + } +} + +void Tester::publishScan(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->angle_min = 0.0; + msg->angle_max = 2 * M_PI; + msg->angle_increment = M_PI / 180; + msg->time_increment = 0.0; + msg->scan_time = 0.0; + msg->range_min = 0.1; + msg->range_max = dist + 1.0; + std::vector ranges(360, dist); + msg->ranges = ranges; + + scan_pub_->publish(std::move(msg)); +} + +void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + sensor_msgs::PointCloud2Modifier modifier(*msg); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + modifier.setPointCloud2Fields( + 3, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32); + modifier.resize(2); + + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + + // Point 0: (dist, 0.01, 0.2) + *iter_x = dist; + *iter_y = 0.01; + *iter_z = 0.2; + ++iter_x; ++iter_y; ++iter_z; + + // Point 1: (dist, -0.01, 0.2) + *iter_x = dist; + *iter_y = -0.01; + *iter_z = 0.2; + + pointcloud_pub_->publish(std::move(msg)); +} + +void Tester::publishRange(const double dist, const rclcpp::Time & stamp) +{ + std::unique_ptr msg = + std::make_unique(); + + msg->header.frame_id = SOURCE_FRAME_ID; + msg->header.stamp = stamp; + + msg->radiation_type = 0; + msg->field_of_view = M_PI / 10; + msg->min_range = 0.1; + msg->max_range = dist + 0.1; + msg->range = dist; + + range_pub_->publish(std::move(msg)); +} + +bool Tester::waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp) +{ + rclcpp::Time start_time = cd_->now(); + while (rclcpp::ok() && cd_->now() - start_time <= rclcpp::Duration(timeout)) { + if (cd_->correctDataReceived(expected_dist, stamp)) { + return true; + } + rclcpp::spin_some(cd_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + + +TEST_F(Tester, testIncorrectPolygonType) +{ + setCommonParameters(); + addPolygon("UnknownShape", POLYGON_UNKNOWN, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"UnknownShape"}, {SCAN_NAME}); + + // Check that Collision Monitor node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testIncorrectSourceType) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource("UnknownSource", SOURCE_UNKNOWN); + setVectors({"DetectionRegion"}, {"UnknownSource"}); + + // Check that Collision Monitor node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testPolygonsNotSet) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + + // Check that Collision Monitor node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testSourcesNotSet) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + cd_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"DetectionRegion"})); + cd_->set_parameter(rclcpp::Parameter("polygons", std::vector{"DetectionRegion"})); + + // Check that Collision Monitor node can not be configured for this parameters set + cd_->cant_configure(); +} + +TEST_F(Tester, testSuccessfulConfigure) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Check that Collision Monitor node can not be configured for this parameters set + cd_->configure(); +} + +TEST_F(Tester, testProcessNonActive) +{ + rclcpp::Time curr_time = cd_->now(); + + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Configure Collision Detector node, but not activate + cd_->configure(); + + // ... and check that the detector state was not published + ASSERT_FALSE(waitState(1000ms)); + + // Stop Collision Monitor node + cd_->stop(); +} + +TEST_F(Tester, testProcessActive) +{ + rclcpp::Time curr_time = cd_->now(); + + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Configure Collision Detector node, but not activate + cd_->start(); + // ... and check that state is published + ASSERT_TRUE(waitState(1000ms)); + + // Stop Collision Monitor node + cd_->stop(); +} + +TEST_F(Tester, testdetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 2.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Start Collision Monitor node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in slowdown robot zone + publishScan(1.5, curr_time); + + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); + ASSERT_TRUE(waitState(1000ms)); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Monitor node + cd_->stop(); +} + +int main(int argc, char ** argv) +{ + // Initialize the system + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + + // Actual testing + bool test_result = RUN_ALL_TESTS(); + + // Shutdown + rclcpp::shutdown(); + + return test_result; +} From 348cb3bcd463579818e846385afed35d8cdb9533 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 21 Jul 2023 17:26:09 +0200 Subject: [PATCH 44/60] amend msg --- nav2_msgs/msg/CollisionDetectorState.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_msgs/msg/CollisionDetectorState.msg b/nav2_msgs/msg/CollisionDetectorState.msg index f6a56c01112..9e51b5f507f 100644 --- a/nav2_msgs/msg/CollisionDetectorState.msg +++ b/nav2_msgs/msg/CollisionDetectorState.msg @@ -1,3 +1,4 @@ -# Name of triggered polygon +# Name of configured polygons string[] polygons +# List of detections for each polygon bool[] detections From 0b46fabc0d04a1185d88bffcc03adb6ef8356acf Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 24 Jul 2023 11:47:50 +0200 Subject: [PATCH 45/60] PR comments --- nav2_collision_monitor/README.md | 5 +- .../src/collision_detector_node.cpp | 2 +- .../src/collision_monitor_node.cpp | 2 +- nav2_collision_monitor/test/CMakeLists.txt | 8 ++- .../test/collision_detector_node_test.cpp | 54 ++++++++++--------- 5 files changed, 41 insertions(+), 30 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 87a553b45af..4aeabe33d9c 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -71,5 +71,6 @@ The following notes could be made: ## Collision Detector -Another node exists in the nav2_collision_monitor package called the Collision Detector. This node works similarly to the collision monitor -except that it does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `/collision_detector_state` topic. +Another node exists in the nav2_collision_monitor package called the Collision Detector. This node works similarly to the collision monitor except that it does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `collision_detector_state` topic. It is important to note that unlike the Collision Monitor that uses different behavior models, the Collision Detector does not use any of them and therefore the `action_type` should always be set to `none`. + +The Collision Detector was introduced for the use cases where the robot's velocity should not be affected by the collision monitor, but the user still wants to be informed about the detected obstacles and act upon it in different ways; e.g. blink LEDs or sound an alarm when the robot is close to an obstacle. diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 11f8a690df9..9875cb1809a 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2023 Samsung R&D Institute Russia +// Copyright (c) 2022 Samsung R&D Institute Russia // Copyright (c) 2023 Pixel Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index ccf4b3d5f00..705069a301c 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -258,7 +258,7 @@ bool CollisionMonitor::configurePolygons( { try { auto node = shared_from_this(); - + // Leave it to be not initialized: to intentionally cause an error if it will not set nav2_util::declare_parameter_if_not_declared( node, "polygons", rclcpp::PARAMETER_STRING_ARRAY); diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index 602970c0340..bce329d4430 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -26,7 +26,13 @@ target_link_libraries(polygons_test ) # Collision Monitor node test - +ament_add_gtest(collision_monitor_node_test collision_monitor_node_test.cpp) +ament_target_dependencies(collision_monitor_node_test + ${dependencies} +) +target_link_libraries(collision_monitor_node_test + ${library_name} +) # Collision Detector node test ament_add_gtest(collision_detector_node_test collision_detector_node_test.cpp) ament_target_dependencies(collision_detector_node_test diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 8016a91cd1d..0948805a028 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2022 Samsung R&D Institute Russia +// Copyright (c) 2023 Pixel Robotics GmbH // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -49,10 +50,11 @@ static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; static const char STATE_TOPIC[]{"collision_detector_state"}; -static const int MAX_POINTS{1}; +static const int MIN_POINTS{2}; static const double SIMULATION_TIME_STEP{0.01}; static const double TRANSFORM_TOLERANCE{0.5}; static const double SOURCE_TIMEOUT{5.0}; +static const double FREQUENCY{10.0}; enum PolygonType { @@ -145,7 +147,7 @@ class Tester : public ::testing::Test void stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg); protected: - // CollisionMonitor node + // CollisionDetector node std::shared_ptr cd_; // Data source publishers @@ -155,8 +157,6 @@ class Tester : public ::testing::Test rclcpp::Subscription::SharedPtr state_sub_; nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; - - }; // Tester Tester::Tester() @@ -222,6 +222,10 @@ void Tester::setCommonParameters() "source_timeout", rclcpp::ParameterValue(SOURCE_TIMEOUT)); cd_->set_parameter( rclcpp::Parameter("source_timeout", SOURCE_TIMEOUT)); + cd_->declare_parameter( + "frequency", rclcpp::ParameterValue(FREQUENCY)); + cd_->set_parameter( + rclcpp::Parameter("frequency", FREQUENCY)); } void Tester::addPolygon( @@ -270,9 +274,9 @@ void Tester::addPolygon( rclcpp::Parameter(polygon_name + ".action_type", at)); cd_->declare_parameter( - polygon_name + ".max_points", rclcpp::ParameterValue(MAX_POINTS)); + polygon_name + ".min_points", rclcpp::ParameterValue(MIN_POINTS)); cd_->set_parameter( - rclcpp::Parameter(polygon_name + ".max_points", MAX_POINTS)); + rclcpp::Parameter(polygon_name + ".min_points", MIN_POINTS)); cd_->declare_parameter( polygon_name + ".simulation_time_step", rclcpp::ParameterValue(SIMULATION_TIME_STEP)); @@ -357,8 +361,8 @@ void Tester::sendTransforms(const rclcpp::Time & stamp) transform.transform.rotation.z = 0.0; transform.transform.rotation.w = 1.0; - // Fill TF buffer ahead for future transform usage in CollisionMonitor::process() - const rclcpp::Duration ahead = 1000ms; + // Fill TF buffer ahead for future transform usage in CollisionDetector::process() + const rclcpp::Duration ahead = 300ms; for (rclcpp::Time t = stamp; t <= stamp + ahead; t += rclcpp::Duration(50ms)) { transform.header.stamp = t; @@ -469,7 +473,7 @@ TEST_F(Tester, testIncorrectPolygonType) addSource(SCAN_NAME, SCAN); setVectors({"UnknownShape"}, {SCAN_NAME}); - // Check that Collision Monitor node can not be configured for this parameters set + // Check that Collision Detector node can not be configured for this parameters set cd_->cant_configure(); } @@ -480,7 +484,7 @@ TEST_F(Tester, testIncorrectSourceType) addSource("UnknownSource", SOURCE_UNKNOWN); setVectors({"DetectionRegion"}, {"UnknownSource"}); - // Check that Collision Monitor node can not be configured for this parameters set + // Check that Collision Detector node can not be configured for this parameters set cd_->cant_configure(); } @@ -490,7 +494,7 @@ TEST_F(Tester, testPolygonsNotSet) addPolygon("DetectionRegion", POLYGON, 1.0, "none"); addSource(SCAN_NAME, SCAN); - // Check that Collision Monitor node can not be configured for this parameters set + // Check that Collision Detector node can not be configured for this parameters set cd_->cant_configure(); } @@ -502,7 +506,7 @@ TEST_F(Tester, testSourcesNotSet) cd_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"DetectionRegion"})); cd_->set_parameter(rclcpp::Parameter("polygons", std::vector{"DetectionRegion"})); - // Check that Collision Monitor node can not be configured for this parameters set + // Check that Collision Detector node can not be configured for this parameters set cd_->cant_configure(); } @@ -513,7 +517,7 @@ TEST_F(Tester, testSuccessfulConfigure) addSource(SCAN_NAME, SCAN); setVectors({"DetectionRegion"}, {SCAN_NAME}); - // Check that Collision Monitor node can not be configured for this parameters set + // Check that Collision Detector node can be configured successfully for this parameters set cd_->configure(); } @@ -530,9 +534,9 @@ TEST_F(Tester, testProcessNonActive) cd_->configure(); // ... and check that the detector state was not published - ASSERT_FALSE(waitState(1000ms)); + ASSERT_FALSE(waitState(300ms)); - // Stop Collision Monitor node + // Stop Collision Detector node cd_->stop(); } @@ -545,40 +549,40 @@ TEST_F(Tester, testProcessActive) addSource(SCAN_NAME, SCAN); setVectors({"DetectionRegion"}, {SCAN_NAME}); - // Configure Collision Detector node, but not activate + // Configure and activate Collision Detector node cd_->start(); // ... and check that state is published - ASSERT_TRUE(waitState(1000ms)); + ASSERT_TRUE(waitState(300ms)); - // Stop Collision Monitor node + // Stop Collision Detector node cd_->stop(); } -TEST_F(Tester, testdetection) +TEST_F(Tester, testDetection) { rclcpp::Time curr_time = cd_->now(); - // Set Collision Monitor parameters. - // Making two polygons: outer polygon for slowdown and inner for robot stop. + // Set Collision Detector parameters. setCommonParameters(); + // Create polygon addPolygon("DetectionRegion", POLYGON, 2.0, "none"); addSource(SCAN_NAME, SCAN); setVectors({"DetectionRegion"}, {SCAN_NAME}); - // Start Collision Monitor node + // Start Collision Detector node cd_->start(); // Share TF sendTransforms(curr_time); - // Obstacle is in slowdown robot zone + // Obstacle is in DetectionRegion publishScan(1.5, curr_time); ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); - ASSERT_TRUE(waitState(1000ms)); + ASSERT_TRUE(waitState(300ms)); ASSERT_EQ(state_msg_->detections[0], true); - // Stop Collision Monitor node + // Stop Collision Detector node cd_->stop(); } From 2505c450516e63cf54e7006242ebf71cfa644cf2 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 24 Jul 2023 12:01:41 +0200 Subject: [PATCH 46/60] add test --- .../test/collision_detector_node_test.cpp | 34 +++++++++++++++++-- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 0948805a028..e745f0fac9b 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -558,7 +558,7 @@ TEST_F(Tester, testProcessActive) cd_->stop(); } -TEST_F(Tester, testDetection) +TEST_F(Tester, testPolygonDetection) { rclcpp::Time curr_time = cd_->now(); @@ -566,8 +566,36 @@ TEST_F(Tester, testDetection) setCommonParameters(); // Create polygon addPolygon("DetectionRegion", POLYGON, 2.0, "none"); - addSource(SCAN_NAME, SCAN); - setVectors({"DetectionRegion"}, {SCAN_NAME}); + addSource(RANGE_NAME, RANGE); + setVectors({"DetectionRegion"}, {RANGE_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishScan(1.5, curr_time); + + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testCircleDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + setVectors({"DetectionRegion"}, {POINTCLOUD_NAME}); // Start Collision Detector node cd_->start(); From 661766a90df3df7cad8d8b45644958b3e4034ff6 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 24 Jul 2023 12:22:05 +0200 Subject: [PATCH 47/60] fix --- nav2_collision_monitor/test/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/test/CMakeLists.txt b/nav2_collision_monitor/test/CMakeLists.txt index bce329d4430..f2b4619bbd7 100644 --- a/nav2_collision_monitor/test/CMakeLists.txt +++ b/nav2_collision_monitor/test/CMakeLists.txt @@ -31,7 +31,7 @@ ament_target_dependencies(collision_monitor_node_test ${dependencies} ) target_link_libraries(collision_monitor_node_test - ${library_name} + ${monitor_library_name} ) # Collision Detector node test ament_add_gtest(collision_detector_node_test collision_detector_node_test.cpp) From b5fc97074ed48eaa12e5ed3335a7f98cc03de65d Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 24 Jul 2023 12:29:35 +0200 Subject: [PATCH 48/60] fix --- .../src/collision_monitor_node.cpp | 2 +- .../test/collision_detector_node_test.cpp | 12 +++++++----- .../test/collision_monitor_node_test.cpp | 1 - 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 705069a301c..6b678c9c768 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -449,7 +449,7 @@ bool CollisionMonitor::processStopSlowdownLimit( } } else { // Limit // Compute linear velocity - const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute + const double linear_vel = std::hypot(velocity.x, velocity.y); // absolute Velocity safe_vel; double ratio = 1.0; if (linear_vel != 0.0) { diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index e745f0fac9b..60861c90df2 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -503,7 +503,9 @@ TEST_F(Tester, testSourcesNotSet) setCommonParameters(); addPolygon("DetectionRegion", POLYGON, 1.0, "none"); addSource(SCAN_NAME, SCAN); - cd_->declare_parameter("polygons", rclcpp::ParameterValue(std::vector{"DetectionRegion"})); + cd_->declare_parameter( + "polygons", + rclcpp::ParameterValue(std::vector{"DetectionRegion"})); cd_->set_parameter(rclcpp::Parameter("polygons", std::vector{"DetectionRegion"})); // Check that Collision Detector node can not be configured for this parameters set @@ -574,10 +576,10 @@ TEST_F(Tester, testPolygonDetection) // Share TF sendTransforms(curr_time); - + // Obstacle is in DetectionRegion publishScan(1.5, curr_time); - + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); ASSERT_TRUE(waitState(300ms)); ASSERT_EQ(state_msg_->detections[0], true); @@ -602,10 +604,10 @@ TEST_F(Tester, testCircleDetection) // Share TF sendTransforms(curr_time); - + // Obstacle is in DetectionRegion publishScan(1.5, curr_time); - + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); ASSERT_TRUE(waitState(300ms)); ASSERT_EQ(state_msg_->detections[0], true); diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 5491e948e31..e11f3b1b3d9 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -186,7 +186,6 @@ class Tester : public ::testing::Test // CollisionMonitor Action state rclcpp::Subscription::SharedPtr action_state_sub_; nav2_msgs::msg::CollisionMonitorState::SharedPtr action_state_; - }; // Tester Tester::Tester() From b3e9a78888b8030591dca174bc5e5dd3a45882ba Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 24 Jul 2023 14:03:36 +0200 Subject: [PATCH 49/60] fix --- .../test/collision_detector_node_test.cpp | 34 +++++++++++++++++-- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 60861c90df2..e23b28b98bc 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -362,7 +362,7 @@ void Tester::sendTransforms(const rclcpp::Time & stamp) transform.transform.rotation.w = 1.0; // Fill TF buffer ahead for future transform usage in CollisionDetector::process() - const rclcpp::Duration ahead = 300ms; + const rclcpp::Duration ahead = 1000ms; for (rclcpp::Time t = stamp; t <= stamp + ahead; t += rclcpp::Duration(50ms)) { transform.header.stamp = t; @@ -580,7 +580,7 @@ TEST_F(Tester, testPolygonDetection) // Obstacle is in DetectionRegion publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); ASSERT_EQ(state_msg_->detections[0], true); @@ -608,7 +608,35 @@ TEST_F(Tester, testCircleDetection) // Obstacle is in DetectionRegion publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); + ASSERT_TRUE(waitData(std::hypot(1.5, 0.01), 300ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + +TEST_F(Tester, testScanDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishScan(1.5, curr_time); + + ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); ASSERT_EQ(state_msg_->detections[0], true); From 6c3904eea9d33c4ef534851d41ef7e5f15d3533f Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 24 Jul 2023 14:15:42 +0200 Subject: [PATCH 50/60] add check --- nav2_collision_monitor/test/collision_detector_node_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index e23b28b98bc..cf3c23bcafb 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -582,6 +582,7 @@ TEST_F(Tester, testPolygonDetection) ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); + ASSERT_GT(static_cast(state_msg_->detections.size()), 0); ASSERT_EQ(state_msg_->detections[0], true); // Stop Collision Detector node @@ -638,6 +639,7 @@ TEST_F(Tester, testScanDetection) ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); + ASSERT_GT(static_cast(state_msg_->detections.size()), 0); ASSERT_EQ(state_msg_->detections[0], true); // Stop Collision Detector node From 44fba19d0bb40486d4043ccf7b85db3df541d619 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 24 Jul 2023 15:19:01 +0200 Subject: [PATCH 51/60] fix --- nav2_collision_monitor/test/collision_detector_node_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index cf3c23bcafb..4d4341f2be8 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -578,7 +578,7 @@ TEST_F(Tester, testPolygonDetection) sendTransforms(curr_time); // Obstacle is in DetectionRegion - publishScan(1.5, curr_time); + publishRange(1.5, curr_time); ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); @@ -607,7 +607,7 @@ TEST_F(Tester, testCircleDetection) sendTransforms(curr_time); // Obstacle is in DetectionRegion - publishScan(1.5, curr_time); + publishPointCloud(1.5, curr_time); ASSERT_TRUE(waitData(std::hypot(1.5, 0.01), 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); From 5012d6d470a3fc1e9af29cecb5b5c2ff045515bc Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 28 Jul 2023 11:43:31 +0200 Subject: [PATCH 52/60] fixes --- nav2_collision_monitor/README.md | 8 ++++---- .../nav2_collision_monitor/collision_detector_node.hpp | 2 +- .../test/collision_detector_node_test.cpp | 5 ++--- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 4aeabe33d9c..0a39d783a7b 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -29,9 +29,9 @@ The following models of safety behaviors are employed by Collision Monitor: The zones around the robot can take the following shapes: -* Arbitrary user-defined polygon relative to the robot base frame. -* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape. -* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time. +* Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface. +* Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time. +* Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape. The data may be obtained from different data sources: @@ -58,7 +58,7 @@ Designed to be used in wide variety of robots (incl. moving fast) and have a hig Typical one frame processing time is ~4-5ms for laser scanner (with 360 points) and ~4-20ms for PointClouds (having 24K points). The table below represents the details of operating times for different behavior models and shapes: -| | Stop/Slowdown model, Polygon area | Stop/Slowdown model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | +| | Stop/Slowdown/Limit model, Polygon area | Stop/Slowdown/Limit model, Circle area | Approach model, Polygon footprint | Approach model, Circle footprint | |-|-----------------------------------|----------------------------------|-----------------------------------|----------------------------------| | LaserScan (360 points) processing time, ms | 4.45 | 4.45 | 4.93 | 4.86 | | PointCloud (24K points) processing time, ms | 4.94 | 4.06 | 20.67 | 10.87 | diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp index 403376569a7..c4c5906b22a 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_detector_node.hpp @@ -145,7 +145,7 @@ class CollisionDetector : public nav2_util::LifecycleNode std::vector> sources_; /// @brief collision monitor state publisher - rclcpp_lifecycle::LifecyclePublisher::SharedPtr \ + rclcpp_lifecycle::LifecyclePublisher::SharedPtr state_pub_; /// @brief timer that runs actions rclcpp::TimerBase::SharedPtr timer_; diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 4d4341f2be8..b6e79a7d2e6 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -202,7 +202,6 @@ void Tester::stateCallback(nav2_msgs::msg::CollisionDetectorState::SharedPtr msg state_msg_ = msg; } - void Tester::setCommonParameters() { cd_->declare_parameter( @@ -582,7 +581,7 @@ TEST_F(Tester, testPolygonDetection) ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); - ASSERT_GT(static_cast(state_msg_->detections.size()), 0); + ASSERT_NE(state_msg_->detections.size(), 0u); ASSERT_EQ(state_msg_->detections[0], true); // Stop Collision Detector node @@ -639,7 +638,7 @@ TEST_F(Tester, testScanDetection) ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); - ASSERT_GT(static_cast(state_msg_->detections.size()), 0); + ASSERT_NE(state_msg_->detections.size(), 0u); ASSERT_EQ(state_msg_->detections[0], true); // Stop Collision Detector node From cbcdb64ad84363b9a4a3c7eab33cad689d1d9108 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 28 Jul 2023 11:44:47 +0200 Subject: [PATCH 53/60] docu fix --- nav2_collision_monitor/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 0a39d783a7b..611b8c039c9 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -45,8 +45,7 @@ The Collision Monitor is designed to operate below Nav2 as an independent safety This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate. The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. -![HDL.png](doc/HLD.png) - +![HLD.png](doc/HLD.png) ### Configuration Detailed configuration parameters, their description and how to setup a Collision Monitor could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-monitor.html) and [Using Collision Monitor tutorial](https://navigation.ros.org/tutorials/docs/using_collision_monitor.html) pages. From 3ed84fcc4b7ba54a838e35ee1e25eea01148d783 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 12:06:54 +0200 Subject: [PATCH 54/60] Add docs --- nav2_collision_monitor/README.md | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 611b8c039c9..03ff362931e 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -70,6 +70,17 @@ The following notes could be made: ## Collision Detector -Another node exists in the nav2_collision_monitor package called the Collision Detector. This node works similarly to the collision monitor except that it does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `collision_detector_state` topic. It is important to note that unlike the Collision Monitor that uses different behavior models, the Collision Detector does not use any of them and therefore the `action_type` should always be set to `none`. +In some cases, the user may want to be informed about the detected obstacles without affecting the robot's velocity and instead take a different action within an external node. For example, the user may want to blink LEDs or sound an alarm when the robot is close to an obstacle. Another use case could be to detect data points in particular regions (e.g within the robot's footprint or extremely close to the sensor) and warn of malfunctioning sensors. For this purpose, the Collision Detector node was introduced. -The Collision Detector was introduced for the use cases where the robot's velocity should not be affected by the collision monitor, but the user still wants to be informed about the detected obstacles and act upon it in different ways; e.g. blink LEDs or sound an alarm when the robot is close to an obstacle. +It works similarly to the Collision Monitor, but does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `collision_detector_state` topic. + +### Features + +Similarly to the Collision Monitor, the Collision Detector uses polygons relative the robot's base frame origin to define "zones". +However, unlike the Collision Monitor that uses different behavior models, the Collision Detector does not use any of them and therefore the `action_type` should always be set to `none`. If set to anything else, it will implicitly be set to `none` and yield a warning. + +The zones around the robot and the data sources are the same as for the Collision Monitor, with the exception of the footprint polygon, which is not supported by the Collision Detector. + +### Configuration + +Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/configuring-collision-detector.html). \ No newline at end of file From 19b14b8bdda54343d6ad0e36dff72fde8007bed5 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 13:29:51 +0200 Subject: [PATCH 55/60] fixes --- nav2_collision_monitor/README.md | 2 +- nav2_collision_monitor/params/collision_detector_params.yaml | 1 + nav2_collision_monitor/src/polygon.cpp | 4 +++- nav2_collision_monitor/test/collision_detector_node_test.cpp | 1 + 4 files changed, 6 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 03ff362931e..864b28bbe41 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -70,7 +70,7 @@ The following notes could be made: ## Collision Detector -In some cases, the user may want to be informed about the detected obstacles without affecting the robot's velocity and instead take a different action within an external node. For example, the user may want to blink LEDs or sound an alarm when the robot is close to an obstacle. Another use case could be to detect data points in particular regions (e.g within the robot's footprint or extremely close to the sensor) and warn of malfunctioning sensors. For this purpose, the Collision Detector node was introduced. +In some cases, the user may want to be informed about the detected obstacles without affecting the robot's velocity and instead take a different action within an external node. For example, the user may want to blink LEDs or sound an alarm when the robot is close to an obstacle. Another use case could be to detect data points in particular regions (e.g extremely close to the sensor) and warn of malfunctioning sensors. For this purpose, the Collision Detector node was introduced. It works similarly to the Collision Monitor, but does not affect the robot's velocity. It will only inform that data from the configured sources has been detected within the configured polygons via message to the `collision_detector_state` topic. diff --git a/nav2_collision_monitor/params/collision_detector_params.yaml b/nav2_collision_monitor/params/collision_detector_params.yaml index f7fafabedb5..218ce452397 100644 --- a/nav2_collision_monitor/params/collision_detector_params.yaml +++ b/nav2_collision_monitor/params/collision_detector_params.yaml @@ -1,5 +1,6 @@ collision_detector: ros__parameters: + frequency: 10.0 base_frame_id: "base_footprint" odom_frame_id: "odom" transform_tolerance: 0.5 diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index e88c2902277..8b7a3c7c002 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -422,7 +422,9 @@ bool Polygon::getParameters( polygon_name_.c_str()); } - if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT) { + if (action_type_ == STOP || action_type_ == SLOWDOWN || action_type_ == LIMIT || + action_type_ == DO_NOTHING) + { // Dynamic polygon will be used nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index b6e79a7d2e6..187a3f648f6 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -610,6 +610,7 @@ TEST_F(Tester, testCircleDetection) ASSERT_TRUE(waitData(std::hypot(1.5, 0.01), 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); ASSERT_EQ(state_msg_->detections[0], true); // Stop Collision Detector node From c101712135195a17c90f6263c8ff37aaeed96741 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 13:39:03 +0200 Subject: [PATCH 56/60] fix test --- .../test/collision_detector_node_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 187a3f648f6..eb91ddf4748 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -594,10 +594,10 @@ TEST_F(Tester, testCircleDetection) // Set Collision Detector parameters. setCommonParameters(); - // Create polygon + // Create Circle addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); - addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"DetectionRegion"}, {POINTCLOUD_NAME}); + addSource(RANGE_NAME, RANGE); + setVectors({"DetectionRegion"}, {RANGE_NAME}); // Start Collision Detector node cd_->start(); @@ -606,9 +606,9 @@ TEST_F(Tester, testCircleDetection) sendTransforms(curr_time); // Obstacle is in DetectionRegion - publishPointCloud(1.5, curr_time); + publishRange(1.5, curr_time); - ASSERT_TRUE(waitData(std::hypot(1.5, 0.01), 300ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 300ms, curr_time)); ASSERT_TRUE(waitState(300ms)); ASSERT_NE(state_msg_->detections.size(), 0u); ASSERT_EQ(state_msg_->detections[0], true); From 70e886b85f0a543283837f9e0e1d8ca3bb8d262e Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 3 Aug 2023 18:01:53 +0200 Subject: [PATCH 57/60] fix --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index f0eda5648b7..9d3e0f490d8 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v15\ + - "<< parameters.key >>-v16\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v15\ + - "<< parameters.key >>-v16\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v15\ + key: "<< parameters.key >>-v16\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ From bf4d5084a7f2edb8556c61d4d5198efd5dd45404 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Fri, 4 Aug 2023 13:29:48 +0200 Subject: [PATCH 58/60] trigger CI --- nav2_collision_monitor/src/collision_detector_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 9875cb1809a..338e7dc154e 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -242,7 +242,7 @@ bool CollisionDetector::configureSources( try { auto node = shared_from_this(); - // Leave it to be not initialized: to intentionally cause an error if it will not set + // Leave it to be not initialized to intentionally cause an error if it will not set nav2_util::declare_parameter_if_not_declared( node, "observation_sources", rclcpp::PARAMETER_STRING_ARRAY); std::vector source_names = get_parameter("observation_sources").as_string_array(); From b5dd2f2c5d0db1f75f99ecf5a21c61369a822079 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 10 Aug 2023 14:56:08 +0200 Subject: [PATCH 59/60] Fixes --- .../src/collision_detector_node.cpp | 5 ++- .../test/collision_detector_node_test.cpp | 40 +++++++++++++++++++ 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 338e7dc154e..782c9d9910a 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -212,11 +212,12 @@ bool CollisionDetector::configurePolygons( // warn if the added polygon's action_type_ is not different than "none" auto action_type = polygons_.back()->getActionType(); if (action_type != DO_NOTHING) { - RCLCPP_WARN( + RCLCPP_ERROR( get_logger(), "[%s]: The action_type of the polygon is different than \"none\" which is " - "not supported in the collision detector; it will be considered as \"none\".", + "not supported in the collision detector.", polygon_name.c_str()); + return false; } // Configure last added polygon diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index eb91ddf4748..884385917fc 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -476,6 +476,17 @@ TEST_F(Tester, testIncorrectPolygonType) cd_->cant_configure(); } +TEST_F(Tester, testIncorrectActionType) +{ + setCommonParameters(); + addPolygon("DetectionRegion", POLYGON, 1.0, "approach"); + addSource(SCAN_NAME, SCAN); + setVectors({"DetectionRegion"}, {SCAN_NAME}); + + // Check that Collision Detector node can not be configured for this action type + cd_->cant_configure(); +} + TEST_F(Tester, testIncorrectSourceType) { setCommonParameters(); @@ -646,6 +657,35 @@ TEST_F(Tester, testScanDetection) cd_->stop(); } +TEST_F(Tester, testPointcloudDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(POINTCLOUD_NAME, POINTCLOUD); + setVectors({"DetectionRegion"}, {POINTCLOUD_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishPointCloud(2.5, curr_time); + + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); + ASSERT_TRUE(waitState(300ms)); + ASSERT_NE(state_msg_->detections.size(), 0u); + ASSERT_EQ(state_msg_->detections[0], true); + + // Stop Collision Detector node + cd_->stop(); +} + int main(int argc, char ** argv) { // Initialize the system From 2d565ca5f61029907491ba70e079c4da245f6b30 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Thu, 10 Aug 2023 15:27:48 +0200 Subject: [PATCH 60/60] fixes --- .../src/collision_detector_node.cpp | 9 ++++---- .../test/collision_detector_node_test.cpp | 21 +++++++------------ 2 files changed, 12 insertions(+), 18 deletions(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 782c9d9910a..e12d9795db3 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -209,6 +209,11 @@ bool CollisionDetector::configurePolygons( return false; } + // Configure last added polygon + if (!polygons_.back()->configure()) { + return false; + } + // warn if the added polygon's action_type_ is not different than "none" auto action_type = polygons_.back()->getActionType(); if (action_type != DO_NOTHING) { @@ -220,10 +225,6 @@ bool CollisionDetector::configurePolygons( return false; } - // Configure last added polygon - if (!polygons_.back()->configure()) { - return false; - } } } catch (const std::exception & ex) { RCLCPP_ERROR(get_logger(), "Error while getting parameters: %s", ex.what()); diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index 884385917fc..3400f1d50d4 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -50,7 +50,7 @@ static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; static const char STATE_TOPIC[]{"collision_detector_state"}; -static const int MIN_POINTS{2}; +static const int MIN_POINTS{1}; static const double SIMULATION_TIME_STEP{0.01}; static const double TRANSFORM_TOLERANCE{0.5}; static const double SOURCE_TIMEOUT{5.0}; @@ -237,19 +237,12 @@ void Tester::addPolygon( cd_->set_parameter( rclcpp::Parameter(polygon_name + ".type", "polygon")); - if (at != "approach") { - const std::vector points { - size, size, size, -size, -size, -size, -size, size}; - cd_->declare_parameter( - polygon_name + ".points", rclcpp::ParameterValue(points)); - cd_->set_parameter( - rclcpp::Parameter(polygon_name + ".points", points)); - } else { // at == "approach" - cd_->declare_parameter( - polygon_name + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); - cd_->set_parameter( - rclcpp::Parameter(polygon_name + ".footprint_topic", FOOTPRINT_TOPIC)); - } + const std::vector points { + size, size, size, -size, -size, -size, -size, size}; + cd_->declare_parameter( + polygon_name + ".points", rclcpp::ParameterValue(points)); + cd_->set_parameter( + rclcpp::Parameter(polygon_name + ".points", points)); } else if (type == CIRCLE) { cd_->declare_parameter( polygon_name + ".type", rclcpp::ParameterValue("circle"));