diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index e1b1c67567..8544429466 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -47,6 +47,7 @@ add_library(${library_name} SHARED src/source.cpp src/scan.cpp src/pointcloud.cpp + src/range.cpp src/kinematics.cpp ) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index bb6fac5b75..54841675e9 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -35,7 +35,7 @@ The data may be obtained from different data sources: * Laser scanners (`sensor_msgs::msg::LaserScan` messages) * PointClouds (`sensor_msgs::msg::PointCloud2` messages) -* IR/Sonars (`sensor_msgs::msg::Range` messages, not implemented yet) +* IR/Sonars (`sensor_msgs::msg::Range` messages) ## Design diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index bd902c1ea4..807d4b3dc7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. 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 2de3c22246..87f498e401 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 @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. @@ -34,6 +34,7 @@ #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 { diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp index 8dc418bd0e..00038eca9b 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/kinematics.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index db3d5e1135..c316c065f7 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. @@ -35,7 +35,7 @@ class PointCloud : public Source /** * @brief PointCloud constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 91a95a1251..97423dc411 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp new file mode 100644 index 0000000000..c6e0e484ad --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -0,0 +1,97 @@ +// 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__RANGE_HPP_ +#define NAV2_COLLISION_MONITOR__RANGE_HPP_ + +#include "sensor_msgs/msg/range.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for IR/ultrasound range sensor source + */ +class Range : public Source +{ +public: + /** + * @brief Range constructor + * @param node Collision Monitor node pointer + * @param source_name Name of data source + * @param tf_buffer Shared pointer to a TF buffer + * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. + * @param global_frame_id Global frame ID for correct transform calculation + * @param transform_tolerance Transform tolerance + * @param source_timeout Maximum time interval in which data is considered valid + */ + Range( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout); + /** + * @brief Range destructor + */ + ~Range(); + + /** + * @brief Data source configuration routine. Obtains ROS-parameters + * and creates range sensor subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from range sensor to the data array. + * @param curr_time Current node time for data interpolation + * @param data Array where the data from source to be added. + * Added data is transformed to base_frame_id_ coordinate system at curr_time. + */ + void getData( + const rclcpp::Time & curr_time, + std::vector & data) const; + +protected: + /** + * @brief Getting sensor-specific ROS-parameters + * @param source_topic Output name of source subscription topic + */ + void getParameters(std::string & source_topic); + + /** + * @brief Range sensor data callback + * @param msg Shared pointer to Range sensor message + */ + void dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief Range sensor data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + /// @brief Angle increment (in rad) between two obstacle points at the range arc + double obstacles_angle_; + + /// @brief Latest data obtained from range sensor + sensor_msgs::msg::Range::ConstSharedPtr data_; +}; // class Range + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__RANGE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index f96ae211f8..29747e8131 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. @@ -35,7 +35,7 @@ class Scan : public Source /** * @brief Scan constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 32227e1df3..a24859bb4a 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. @@ -40,7 +40,7 @@ class Source /** * @brief Source constructor * @param node Collision Monitor node pointer - * @param polygon_name Name of data source + * @param source_name Name of data source * @param tf_buffer Shared pointer to a TF buffer * @param base_frame_id Robot base frame ID. The output data will be transformed into this frame. * @param global_frame_id Global frame ID for correct transform calculation diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp index 2f239d694b..39ba9d8c6d 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/types.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/launch/collision_monitor_node.launch.py b/nav2_collision_monitor/launch/collision_monitor_node.launch.py index 7e96aeec2b..d03d723498 100644 --- a/nav2_collision_monitor/launch/collision_monitor_node.launch.py +++ b/nav2_collision_monitor/launch/collision_monitor_node.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -# Copyright (c) 2022 Samsung Research Russia +# 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. diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 6a05477f49..7dc9708967 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 5088e2c5a2..313d71fb0a 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. @@ -302,6 +302,14 @@ bool CollisionMonitor::configureSources( 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(), diff --git a/nav2_collision_monitor/src/kinematics.cpp b/nav2_collision_monitor/src/kinematics.cpp index 1b6ee226b7..e51b7c4879 100644 --- a/nav2_collision_monitor/src/kinematics.cpp +++ b/nav2_collision_monitor/src/kinematics.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/src/main.cpp b/nav2_collision_monitor/src/main.cpp index 01ae87e353..6c23d63024 100644 --- a/nav2_collision_monitor/src/main.cpp +++ b/nav2_collision_monitor/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index eb25c91114..df4e86b63e 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 0e1c47b59c..d17c34b46a 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp new file mode 100644 index 0000000000..3ef51e2b69 --- /dev/null +++ b/nav2_collision_monitor/src/range.cpp @@ -0,0 +1,145 @@ +// 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/range.hpp" + +#include +#include +#include + +#include "nav2_util/node_utils.hpp" + +namespace nav2_collision_monitor +{ + +Range::Range( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & source_timeout) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout), + data_(nullptr) +{ + RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str()); +} + +Range::~Range() +{ + RCLCPP_INFO(logger_, "[%s]: Destroying Range", source_name_.c_str()); + data_sub_.reset(); +} + +void Range::configure() +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + getParameters(source_topic); + + rclcpp::QoS range_qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, range_qos, + std::bind(&Range::dataCallback, this, std::placeholders::_1)); +} + +void Range::getData( + const rclcpp::Time & curr_time, + std::vector & data) const +{ + // Ignore data from the source if it is not being published yet or + // not being published for a long time + if (data_ == nullptr) { + return; + } + if (!sourceValid(data_->header.stamp, curr_time)) { + return; + } + + // Ignore data, if its range is out of scope of range sensor abilities + if (data_->range < data_->min_range || data_->range > data_->max_range) { + RCLCPP_WARN( + logger_, + "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", + source_name_.c_str(), data_->range, data_->min_range, data_->max_range); + return; + } + + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + tf2::Transform tf_transform; + if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + return; + } + + // Calculate poses and refill data array + float angle; + for ( + angle = -data_->field_of_view / 2; + angle < data_->field_of_view / 2; + angle += obstacles_angle_) + { + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s( + data_->range * std::cos(angle), + data_->range * std::sin(angle), + 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + data.push_back({p_v3_b.x(), p_v3_b.y()}); + } + + // Make sure that last (field_of_view / 2) point will be in the data array + angle = data_->field_of_view / 2; + + // Transform point coordinates from source frame -> to base frame + tf2::Vector3 p_v3_s( + data_->range * std::cos(angle), + data_->range * std::sin(angle), + 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Refill data array + data.push_back({p_v3_b.x(), p_v3_b.y()}); +} + +void Range::getParameters(std::string & source_topic) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + getCommonParameters(source_topic); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 180)); + obstacles_angle_ = node->get_parameter(source_name_ + ".obstacles_angle").as_double(); +} + +void Range::dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg) +{ + data_ = msg; +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index a73d9e53cf..d1f52b31f1 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index e945301640..bd1028518c 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 094a3f1e91..0bf3c39003 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,7 @@ #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" @@ -45,6 +47,7 @@ static const char CMD_VEL_IN_TOPIC[]{"cmd_vel_in"}; static const char CMD_VEL_OUT_TOPIC[]{"cmd_vel_out"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; +static const char RANGE_NAME[]{"Range"}; static const int MAX_POINTS{1}; static const double SLOWDOWN_RATIO{0.7}; static const double TIME_BEFORE_COLLISION{1.0}; @@ -64,7 +67,8 @@ enum SourceType { SOURCE_UNKNOWN = 0, SCAN = 1, - POINTCLOUD = 2 + POINTCLOUD = 2, + RANGE = 3 }; class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor @@ -93,16 +97,19 @@ class CollisionMonitorWrapper : public nav2_collision_monitor::CollisionMonitor ASSERT_EQ(on_configure(get_current_state()), nav2_util::CallbackReturn::FAILURE); } - bool isDataReceived(const rclcpp::Time & stamp) + 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) { - return false; + 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 true; + return false; } }; // CollisionMonitorWrapper @@ -128,8 +135,12 @@ class Tester : public ::testing::Test // 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); void publishCmdVel(const double x, const double y, const double tw); - bool waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp); + bool waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp); bool waitCmdVel(const std::chrono::nanoseconds & timeout); protected: @@ -141,6 +152,7 @@ class Tester : public ::testing::Test // Data source publishers rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; // Working with cmd_vel_in/cmd_vel_out rclcpp::Publisher::SharedPtr cmd_vel_in_pub_; @@ -157,6 +169,8 @@ Tester::Tester() SCAN_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); pointcloud_pub_ = cm_->create_publisher( POINTCLOUD_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + range_pub_ = cm_->create_publisher( + RANGE_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); cmd_vel_in_pub_ = cm_->create_publisher( CMD_VEL_IN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -169,6 +183,7 @@ Tester::~Tester() { scan_pub_.reset(); pointcloud_pub_.reset(); + range_pub_.reset(); cmd_vel_in_pub_.reset(); cmd_vel_out_sub_.reset(); @@ -302,6 +317,16 @@ void Tester::addSource( source_name + ".max_height", rclcpp::ParameterValue(1.0)); cm_->set_parameter( rclcpp::Parameter(source_name + ".max_height", 1.0)); + } else if (type == RANGE) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("range")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "range")); + + cm_->declare_parameter( + source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); } else { // type == SOURCE_UNKNOWN cm_->declare_parameter( source_name + ".type", rclcpp::ParameterValue("unknown")); @@ -408,6 +433,23 @@ void Tester::publishPointCloud(const double dist, const rclcpp::Time & stamp) 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)); +} + void Tester::publishCmdVel(const double x, const double y, const double tw) { // Reset cmd_vel_out_ before calling CollisionMonitor::process() @@ -423,11 +465,14 @@ void Tester::publishCmdVel(const double x, const double y, const double tw) cmd_vel_in_pub_->publish(std::move(msg)); } -bool Tester::waitData(const std::chrono::nanoseconds & timeout, const rclcpp::Time & stamp) +bool Tester::waitData( + const double expected_dist, + const std::chrono::nanoseconds & timeout, + const rclcpp::Time & stamp) { rclcpp::Time start_time = cm_->now(); while (rclcpp::ok() && cm_->now() - start_time <= rclcpp::Duration(timeout)) { - if (cm_->isDataReceived(stamp)) { + if (cm_->correctDataReceived(expected_dist, stamp)) { return true; } rclcpp::spin_some(cm_->get_node_base_interface()); @@ -474,7 +519,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 1. Obstacle is far away from robot publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -483,7 +528,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 2. Obstacle is in slowdown robot zone publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); @@ -492,7 +537,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 3. Obstacle is inside stop zone publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); @@ -501,7 +546,7 @@ TEST_F(Tester, testProcessStopSlowdown) // 4. Restorint back normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -531,7 +576,7 @@ TEST_F(Tester, testProcessApproach) // 1. Obstacle is far away from robot publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -540,7 +585,7 @@ TEST_F(Tester, testProcessApproach) // 2. Approaching obstacle (0.2 m ahead from robot footprint) publishScan(1.2, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.2, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); // change_ratio = (0.2 m / speed m/s) / TIME_BEFORE_COLLISION s @@ -554,7 +599,7 @@ TEST_F(Tester, testProcessApproach) // 3. Obstacle is inside robot footprint publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); @@ -563,7 +608,7 @@ TEST_F(Tester, testProcessApproach) // 4. Restorint back normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -585,7 +630,8 @@ TEST_F(Tester, testCrossOver) addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); addPolygon("Approach", CIRCLE, 1.0, "approach"); addSource(POINTCLOUD_NAME, POINTCLOUD); - setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME}); + addSource(RANGE_NAME, RANGE); + setVectors({"SlowDown", "Approach"}, {POINTCLOUD_NAME, RANGE_NAME}); // Start Collision Monitor node cm_->start(); @@ -596,7 +642,7 @@ TEST_F(Tester, testCrossOver) // 1. Obstacle is not in the slowdown zone, but less than TIME_BEFORE_COLLISION (ahead in 1.5 m). // Robot should approach the obstacle. publishPointCloud(2.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(std::hypot(2.5, 0.01), 500ms, curr_time)); publishCmdVel(3.0, 0.0, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); // change_ratio = (1.5 m / 3.0 m/s) / TIME_BEFORE_COLLISION s @@ -607,8 +653,8 @@ TEST_F(Tester, testCrossOver) ASSERT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); // 2. Obstacle is inside slowdown zone, but speed is too slow for approach - publishPointCloud(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + publishRange(1.5, curr_time); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.1, 0.0, 0.0); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.1 * SLOWDOWN_RATIO, EPSILON); @@ -650,7 +696,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 1. Obstacle is inside approach footprint: robot should stop publishScan(1.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(1.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); @@ -666,7 +712,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 3. Release robot to normal operation publishScan(3.0, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(3.0, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); @@ -675,7 +721,7 @@ TEST_F(Tester, testCeasePublishZeroVel) // 4. Obstacle is inside stop zone publishScan(0.5, curr_time); - ASSERT_TRUE(waitData(500ms, curr_time)); + ASSERT_TRUE(waitData(0.5, 500ms, curr_time)); publishCmdVel(0.5, 0.2, 0.1); ASSERT_TRUE(waitCmdVel(500ms)); ASSERT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); diff --git a/nav2_collision_monitor/test/kinematics_test.cpp b/nav2_collision_monitor/test/kinematics_test.cpp index 2c1ec599a7..65933daa38 100644 --- a/nav2_collision_monitor/test/kinematics_test.cpp +++ b/nav2_collision_monitor/test/kinematics_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index ec7a19f124..87576ddda9 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index c199c02286..7101b61175 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 Samsung Research Russia +// 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. @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,7 @@ #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 "tf2_ros/buffer.h" @@ -35,6 +37,7 @@ #include "nav2_collision_monitor/types.hpp" #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" +#include "nav2_collision_monitor/range.hpp" using namespace std::chrono_literals; @@ -47,6 +50,8 @@ static const char SCAN_NAME[]{"LaserScan"}; static const char SCAN_TOPIC[]{"scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char POINTCLOUD_TOPIC[]{"pointcloud"}; +static const char RANGE_NAME[]{"Range"}; +static const char RANGE_TOPIC[]{"range"}; static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; @@ -61,9 +66,11 @@ class TestNode : public nav2_util::LifecycleNode ~TestNode() { scan_pub_.reset(); + pointcloud_pub_.reset(); + range_pub_.reset(); } - void publishScan(const rclcpp::Time & stamp) + void publishScan(const rclcpp::Time & stamp, const double range) { scan_pub_ = this->create_publisher( SCAN_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); @@ -81,7 +88,7 @@ class TestNode : public nav2_util::LifecycleNode msg->scan_time = 0.0; msg->range_min = 0.1; msg->range_max = 1.1; - std::vector ranges(4, 1.0); + std::vector ranges(4, range); msg->ranges = ranges; scan_pub_->publish(std::move(msg)); @@ -129,9 +136,30 @@ class TestNode : public nav2_util::LifecycleNode pointcloud_pub_->publish(std::move(msg)); } + void publishRange(const rclcpp::Time & stamp, const double range) + { + range_pub_ = this->create_publisher( + RANGE_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + + 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 = 1.1; + msg->range = range; + + range_pub_->publish(std::move(msg)); + } + private: rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; + rclcpp::Publisher::SharedPtr range_pub_; }; // TestNode class ScanWrapper : public nav2_collision_monitor::Scan @@ -178,6 +206,28 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud } }; // PointCloudWrapper +class RangeWrapper : public nav2_collision_monitor::Range +{ +public: + RangeWrapper( + const nav2_util::LifecycleNode::WeakPtr & node, + const std::string & source_name, + const std::shared_ptr tf_buffer, + const std::string & base_frame_id, + const std::string & global_frame_id, + const tf2::Duration & transform_tolerance, + const rclcpp::Duration & data_timeout) + : nav2_collision_monitor::Range( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout) + {} + + bool dataReceived() const + { + return data_ != nullptr; + } +}; // RangeWrapper + class Tester : public ::testing::Test { public: @@ -191,12 +241,15 @@ class Tester : public ::testing::Test // Data sources working routines bool waitScan(const std::chrono::nanoseconds & timeout); bool waitPointCloud(const std::chrono::nanoseconds & timeout); + bool waitRange(const std::chrono::nanoseconds & timeout); void checkScan(const std::vector & data); void checkPointCloud(const std::vector & data); + void checkRange(const std::vector & data); std::shared_ptr test_node_; std::shared_ptr scan_; std::shared_ptr pointcloud_; + std::shared_ptr range_; private: std::shared_ptr tf_buffer_; @@ -242,12 +295,28 @@ Tester::Tester() BASE_FRAME_ID, GLOBAL_FRAME_ID, TRANSFORM_TOLERANCE, DATA_TIMEOUT); pointcloud_->configure(); + + // Create Range object + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".topic", rclcpp::ParameterValue(RANGE_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(RANGE_NAME) + ".topic", RANGE_TOPIC)); + + test_node_->declare_parameter( + std::string(RANGE_NAME) + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 199)); + + range_ = std::make_shared( + test_node_, RANGE_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT); + range_->configure(); } Tester::~Tester() { scan_.reset(); pointcloud_.reset(); + range_.reset(); test_node_.reset(); @@ -313,6 +382,19 @@ bool Tester::waitPointCloud(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitRange(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (range_->dataReceived()) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + void Tester::checkScan(const std::vector & data) { ASSERT_EQ(data.size(), 4u); @@ -349,6 +431,24 @@ void Tester::checkPointCloud(const std::vector & // Point 2 should be out of scope by height } +void Tester::checkRange(const std::vector & data) +{ + ASSERT_EQ(data.size(), 21u); + + const double angle_increment = M_PI / 199; + double angle = -M_PI / (10 * 2); + int i; + for (i = 0; i < 199 / 10 + 1; i++) { + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); + angle += angle_increment; + } + // Check for the latest FoW/2 point + angle = M_PI / (10 * 2); + ASSERT_NEAR(data[i].x, 1.0 * std::cos(angle) + 0.1, EPSILON); + ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); +} + TEST_F(Tester, testGetData) { rclcpp::Time curr_time = test_node_->now(); @@ -356,12 +456,14 @@ TEST_F(Tester, testGetData) sendTransforms(curr_time); // Publish data for sources - test_node_->publishScan(curr_time); + test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Check Scan data std::vector data; @@ -372,6 +474,11 @@ TEST_F(Tester, testGetData) data.clear(); pointcloud_->getData(curr_time, data); checkPointCloud(data); + + // Check Range data + data.clear(); + range_->getData(curr_time, data); + checkRange(data); } TEST_F(Tester, testGetOutdatedData) @@ -381,12 +488,14 @@ TEST_F(Tester, testGetOutdatedData) sendTransforms(curr_time); // Publish outdated data for sources - test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishScan(curr_time - DATA_TIMEOUT - 1s, 1.0); test_node_->publishPointCloud(curr_time - DATA_TIMEOUT - 1s); + test_node_->publishRange(curr_time - DATA_TIMEOUT - 1s, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Scan data should be empty std::vector data; @@ -396,6 +505,10 @@ TEST_F(Tester, testGetOutdatedData) // Pointcloud data should be empty pointcloud_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } TEST_F(Tester, testIncorrectFrameData) @@ -406,12 +519,14 @@ TEST_F(Tester, testIncorrectFrameData) sendTransforms(curr_time - 1s); // Publish data for sources - test_node_->publishScan(curr_time); + test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 1.0); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); + ASSERT_TRUE(waitRange(500ms)); // Scan data should be empty std::vector data; @@ -421,6 +536,35 @@ TEST_F(Tester, testIncorrectFrameData) // Pointcloud data should be empty pointcloud_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); +} + +TEST_F(Tester, testIncorrectData) +{ + rclcpp::Time curr_time = test_node_->now(); + + sendTransforms(curr_time); + + // Publish data for sources + test_node_->publishScan(curr_time, 2.0); + test_node_->publishPointCloud(curr_time); + test_node_->publishRange(curr_time, 2.0); + + // Wait until all sources will receive the data + ASSERT_TRUE(waitScan(500ms)); + ASSERT_TRUE(waitRange(500ms)); + + // Scan data should be empty + std::vector data; + scan_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); + + // Range data should be empty + range_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } int main(int argc, char ** argv)