From c93080c492021adc126caa247fa608b2d6d5149b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 29 Apr 2024 12:55:38 -0700 Subject: [PATCH] Collision monitor: Add polygon source (#4275) * Rename PushRosNamespace to PushROSNamespace * Fix min_points checking * Add polygon source * Revert "Merge branch 'add-collision-points-debug' into add-polygon-source" This reverts commit 15c261c0f01b97957711a3812a7b109b644a56d2, reversing changes made to 5a635848a0dde4d5541dee139dafa95d5afdd059. * . * fix * fix * fix lint * PR comments fixes * fixes * add test * fix space * use standard msg Polygonstamped * draft * . * fixes * fixes * fixes * fix * revert * fixes * add detector test * rebasing fixes * Adding polygon source * adjusting tests * linting --------- Co-authored-by: Tony Najjar Co-authored-by: Tony Najjar --- nav2_collision_monitor/CMakeLists.txt | 2 + .../collision_detector_node.hpp | 1 + .../collision_monitor_node.hpp | 1 + .../nav2_collision_monitor/pointcloud.hpp | 2 +- .../nav2_collision_monitor/polygon_source.hpp | 115 +++++++++++ .../include/nav2_collision_monitor/range.hpp | 2 +- .../include/nav2_collision_monitor/scan.hpp | 2 +- .../include/nav2_collision_monitor/source.hpp | 2 +- .../src/collision_detector_node.cpp | 8 + .../src/collision_monitor_node.cpp | 8 + nav2_collision_monitor/src/pointcloud.cpp | 3 +- nav2_collision_monitor/src/polygon.cpp | 5 +- nav2_collision_monitor/src/polygon_source.cpp | 186 ++++++++++++++++++ nav2_collision_monitor/src/range.cpp | 4 +- nav2_collision_monitor/src/scan.cpp | 4 +- .../test/collision_detector_node_test.cpp | 78 +++++++- .../test/collision_monitor_node_test.cpp | 134 ++++++++++++- nav2_collision_monitor/test/sources_test.cpp | 122 ++++++++++++ nav2_util/include/nav2_util/robot_utils.hpp | 1 + nav2_util/src/robot_utils.cpp | 1 + 20 files changed, 670 insertions(+), 11 deletions(-) create mode 100644 nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp create mode 100644 nav2_collision_monitor/src/polygon_source.cpp diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index d812df7d65..5dfaf1576a 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -56,6 +56,7 @@ add_library(${monitor_library_name} SHARED src/source.cpp src/scan.cpp src/pointcloud.cpp + src/polygon_source.cpp src/range.cpp src/kinematics.cpp ) @@ -67,6 +68,7 @@ add_library(${detector_library_name} SHARED src/source.cpp src/scan.cpp src/pointcloud.cpp + src/polygon_source.cpp src/range.cpp src/kinematics.cpp ) 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 fae479d650..7c9785588e 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 @@ -38,6 +38,7 @@ #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" #include "nav2_collision_monitor/range.hpp" +#include "nav2_collision_monitor/polygon_source.hpp" namespace nav2_collision_monitor { 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 998dacb519..ce4d96504b 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 @@ -41,6 +41,7 @@ #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" #include "nav2_collision_monitor/range.hpp" +#include "nav2_collision_monitor/polygon_source.hpp" namespace nav2_collision_monitor { diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp index bfe8d09ed3..ea370f9acc 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp @@ -73,7 +73,7 @@ class PointCloud : public Source */ bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp new file mode 100644 index 0000000000..8855bb7241 --- /dev/null +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon_source.hpp @@ -0,0 +1,115 @@ +// 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. +// 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__POLYGON_SOURCE_HPP_ +#define NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/polygon_instance_stamped.hpp" +#include "geometry_msgs/msg/polygon_stamped.hpp" + +#include "nav2_collision_monitor/source.hpp" + +namespace nav2_collision_monitor +{ + +/** + * @brief Implementation for polygon source + */ +class PolygonSource : public Source +{ +public: + /** + * @brief PolygonSource 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 + * @param base_shift_correction Whether to correct source data towards to base frame movement, + * considering the difference between current time and latest source time + */ + PolygonSource( + 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, + const bool base_shift_correction); + /** + * @brief PolygonSource destructor + */ + ~PolygonSource(); + + /** + * @brief Data source configuration routine. Obtains ROS-parameters + * and creates subscriber. + */ + void configure(); + + /** + * @brief Adds latest data from polygon source 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. + * @return false if an invalid source should block the robot + */ + bool getData( + const rclcpp::Time & curr_time, + std::vector & data); + + /** + * @brief Converts a PolygonInstanceStamped to a std::vector + * @param polygon Input Polygon to be converted + * @param data Output vector of Point + */ + void convertPolygonStampedToPoints( + const geometry_msgs::msg::PolygonStamped & polygon, + 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 PolygonSource data callback + * @param msg Shared pointer to PolygonSource message + */ + void dataCallback(geometry_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg); + + // ----- Variables ----- + + /// @brief PolygonSource data subscriber + rclcpp::Subscription::SharedPtr data_sub_; + + /// @brief Latest data obtained + std::vector data_; + + /// @brief distance between sampled points on polygon edges + double sampling_distance_; +}; // class PolygonSource + +} // namespace nav2_collision_monitor + +#endif // NAV2_COLLISION_MONITOR__POLYGON_SOURCE_HPP_ diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp index 1dc15195f6..2bccaac108 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp @@ -73,7 +73,7 @@ class Range : public Source */ bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp index efaf82e0e3..5c57cdb014 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp @@ -73,7 +73,7 @@ class Scan : public Source */ bool getData( const rclcpp::Time & curr_time, - std::vector & data) const; + std::vector & data); protected: /** diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 62e0d60afa..4432ec243b 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -73,7 +73,7 @@ class Source */ virtual bool getData( const rclcpp::Time & curr_time, - std::vector & data) const = 0; + std::vector & data) = 0; /** * @brief Obtains source enabled state diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index 4f485520c7..4d8da1e241 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -287,6 +287,13 @@ bool CollisionDetector::configureSources( r->configure(); sources_.push_back(r); + } else if (source_type == "polygon") { + std::shared_ptr ps = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + ps->configure(); + + sources_.push_back(ps); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -345,6 +352,7 @@ void CollisionDetector::process() marker.color.r = 1.0; marker.color.a = 1.0; marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = true; for (const auto & point : collision_points) { geometry_msgs::msg::Point p; diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 94c116445a..766ed713cd 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -378,6 +378,13 @@ bool CollisionMonitor::configureSources( r->configure(); sources_.push_back(r); + } else if (source_type == "polygon") { + std::shared_ptr ps = std::make_shared( + node, source_name, tf_buffer_, base_frame_id, odom_frame_id, + transform_tolerance, source_timeout, base_shift_correction); + ps->configure(); + + sources_.push_back(ps); } else { // Error if something else RCLCPP_ERROR( get_logger(), @@ -444,6 +451,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: marker.color.r = 1.0; marker.color.a = 1.0; marker.lifetime = rclcpp::Duration(0, 0); + marker.frame_locked = true; for (const auto & point : collision_points) { geometry_msgs::msg::Point p; diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index 817fb48257..77daab6805 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -17,6 +17,7 @@ #include #include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2/transform_datatypes.h" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -67,7 +68,7 @@ void PointCloud::configure() bool PointCloud::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not published for a long time diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 7e0a69b84f..08910c9145 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -19,6 +19,7 @@ #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/point32.hpp" +#include "tf2/transform_datatypes.h" #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -195,7 +196,7 @@ void Polygon::updatePolygon(const Velocity & /*cmd_vel_in*/) std::size_t new_size = polygon_.polygon.points.size(); // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; + tf2::Stamped tf_transform; if ( !nav2_util::getTransform( polygon_.header.frame_id, base_frame_id_, @@ -478,7 +479,7 @@ void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr m } // Get the transform from PolygonStamped frame to base_frame_id_ - tf2::Transform tf_transform; + tf2::Stamped tf_transform; if ( !nav2_util::getTransform( msg->header.frame_id, base_frame_id_, diff --git a/nav2_collision_monitor/src/polygon_source.cpp b/nav2_collision_monitor/src/polygon_source.cpp new file mode 100644 index 0000000000..149ac43153 --- /dev/null +++ b/nav2_collision_monitor/src/polygon_source.cpp @@ -0,0 +1,186 @@ +// 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. +// 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/polygon_source.hpp" + +#include +#include + +#include "geometry_msgs/msg/polygon_stamped.hpp" +#include "tf2/transform_datatypes.h" + +#include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" + + +namespace nav2_collision_monitor +{ + +PolygonSource::PolygonSource( + 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, + const bool base_shift_correction) +: Source( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, source_timeout, base_shift_correction) +{ +} + +PolygonSource::~PolygonSource() +{ + data_sub_.reset(); +} + +void PolygonSource::configure() +{ + Source::configure(); + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + std::string source_topic; + + getParameters(source_topic); + + rclcpp::QoS qos = rclcpp::SensorDataQoS(); // set to default + data_sub_ = node->create_subscription( + source_topic, qos, + std::bind(&PolygonSource::dataCallback, this, std::placeholders::_1)); +} + +bool PolygonSource::getData( + const rclcpp::Time & curr_time, + std::vector & data) +{ + // Ignore data from the source if it is not being published yet or + // not published for a long time + if (data_.empty()) { + return false; + } + + // Remove stale data + data_.erase( + std::remove_if( + data_.begin(), data_.end(), + [this, curr_time](const geometry_msgs::msg::PolygonInstanceStamped & polygon_stamped) { + return curr_time - rclcpp::Time(polygon_stamped.header.stamp) > source_timeout_; + }), data_.end()); + + tf2::Stamped tf_transform; + for (const auto & polygon_instance : data_) { + if (base_shift_correction_) { + // Obtaining the transform to get data from source frame and time where it was received + // to the base frame and current time + if ( + !nav2_util::getTransform( + polygon_instance.header.frame_id, polygon_instance.header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } else { + // Obtaining the transform to get data from source frame to base frame without time shift + // considered. Less accurate but much more faster option not dependent on state estimation + // frames. + if ( + !nav2_util::getTransform( + polygon_instance.header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return false; + } + } + geometry_msgs::msg::PolygonStamped poly_out, polygon_stamped; + geometry_msgs::msg::TransformStamped tf = tf2::toMsg(tf_transform); + polygon_stamped.header = polygon_instance.header; + polygon_stamped.polygon = polygon_instance.polygon.polygon; + tf2::doTransform(polygon_stamped, poly_out, tf); + convertPolygonStampedToPoints(poly_out, data); + } + return true; +} + +void PolygonSource::convertPolygonStampedToPoints( + const geometry_msgs::msg::PolygonStamped & polygon, std::vector & data) const +{ + /* Full function generated by GPT */ + + // Iterate over the vertices of the polygon + for (size_t i = 0; i < polygon.polygon.points.size(); ++i) { + const auto & current_point = polygon.polygon.points[i]; + const auto & next_point = + polygon.polygon.points[(i + 1) % polygon.polygon.points.size()]; + + // Calculate the distance between the current and next points + double segment_length = + std::hypot(next_point.x - current_point.x, next_point.y - current_point.y); + + // Calculate the number of points to sample in the current segment + int num_points_in_segment = + std::max(static_cast(std::ceil(segment_length / sampling_distance_)), 1); + + // Calculate the step size for each pair of vertices + const double dx = (next_point.x - current_point.x) / num_points_in_segment; + const double dy = (next_point.y - current_point.y) / num_points_in_segment; + + // Sample the points with equal spacing + for (int j = 0; j <= num_points_in_segment; ++j) { + Point p; + p.x = current_point.x + j * dx; + p.y = current_point.y + j * dy; + data.push_back(p); + } + } +} + +void PolygonSource::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_ + ".sampling_distance", rclcpp::ParameterValue(0.1)); + sampling_distance_ = node->get_parameter(source_name_ + ".sampling_distance").as_double(); +} + +void PolygonSource::dataCallback(geometry_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + auto curr_time = node->now(); + + // check if older similar polygon exists already and replace it with the new one + for (auto & polygon_stamped : data_) { + if (msg->polygon.id == polygon_stamped.polygon.id) { + polygon_stamped = *msg; + return; + } + } + data_.push_back(*msg); +} + +} // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 8799827f04..fe555b171f 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -18,6 +18,8 @@ #include #include +#include "tf2/transform_datatypes.h" + #include "nav2_util/node_utils.hpp" #include "nav2_util/robot_utils.hpp" @@ -67,7 +69,7 @@ void Range::configure() bool Range::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not being published for a long time diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 660d66e916..f891df69a0 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -17,6 +17,8 @@ #include #include +#include "tf2/transform_datatypes.h" + #include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor @@ -66,7 +68,7 @@ void Scan::configure() bool Scan::getData( const rclcpp::Time & curr_time, - std::vector & data) const + std::vector & data) { // Ignore data from the source if it is not being published yet or // not being published for a long time diff --git a/nav2_collision_monitor/test/collision_detector_node_test.cpp b/nav2_collision_monitor/test/collision_detector_node_test.cpp index e36ef6e583..da2ecbc98e 100644 --- a/nav2_collision_monitor/test/collision_detector_node_test.cpp +++ b/nav2_collision_monitor/test/collision_detector_node_test.cpp @@ -49,6 +49,7 @@ static const char ODOM_FRAME_ID[]{"odom"}; static const char SCAN_NAME[]{"Scan"}; static const char POINTCLOUD_NAME[]{"PointCloud"}; static const char RANGE_NAME[]{"Range"}; +static const char POLYGON_NAME[]{"Polygon"}; static const char STATE_TOPIC[]{"collision_detector_state"}; static const char COLLISION_POINTS_MARKERS_TOPIC[]{"/collision_detector/collision_points_marker"}; static const int MIN_POINTS{1}; @@ -69,7 +70,8 @@ enum SourceType SOURCE_UNKNOWN = 0, SCAN = 1, POINTCLOUD = 2, - RANGE = 3 + RANGE = 3, + POLYGON_SOURCE = 4 }; class CollisionDetectorWrapper : public nav2_collision_monitor::CollisionDetector @@ -140,6 +142,7 @@ class Tester : public ::testing::Test 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 publishPolygon(const double dist, const rclcpp::Time & stamp); bool waitData( const double expected_dist, const std::chrono::nanoseconds & timeout, @@ -157,6 +160,7 @@ class Tester : public ::testing::Test rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_source_pub_; rclcpp::Subscription::SharedPtr state_sub_; nav2_msgs::msg::CollisionDetectorState::SharedPtr state_msg_; @@ -177,6 +181,8 @@ Tester::Tester() 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()); + polygon_source_pub_ = cd_->create_publisher( + POLYGON_NAME, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); state_sub_ = cd_->create_subscription( STATE_TOPIC, rclcpp::SystemDefaultsQoS(), @@ -192,6 +198,7 @@ Tester::~Tester() scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + polygon_source_pub_.reset(); collision_points_marker_sub_.reset(); cd_.reset(); @@ -353,6 +360,20 @@ void Tester::addSource( source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); cd_->set_parameter( rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); + } else if (type == POLYGON_SOURCE) { + cd_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("polygon")); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".type", "polygon")); + + cd_->declare_parameter( + source_name + ".sampling_distance", rclcpp::ParameterValue(0.1)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".sampling_distance", 0.1)); + cd_->declare_parameter( + source_name + ".polygon_similarity_threshold", rclcpp::ParameterValue(2.0)); + cd_->set_parameter( + rclcpp::Parameter(source_name + ".polygon_similarity_threshold", 2.0)); } else { // type == SOURCE_UNKNOWN cd_->declare_parameter( source_name + ".type", rclcpp::ParameterValue("unknown")); @@ -476,6 +497,32 @@ void Tester::publishRange(const double dist, const rclcpp::Time & stamp) range_pub_->publish(std::move(msg)); } +void Tester::publishPolygon(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; + + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + p.x = 1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + msg->polygon.id = 1u; + + polygon_source_pub_->publish(std::move(msg)); +} + bool Tester::waitData( const double expected_dist, const std::chrono::nanoseconds & timeout, @@ -714,6 +761,35 @@ TEST_F(Tester, testPointcloudDetection) cd_->stop(); } +TEST_F(Tester, testPolygonSourceDetection) +{ + rclcpp::Time curr_time = cd_->now(); + + // Set Collision Detector parameters. + setCommonParameters(); + // Create polygon + addPolygon("DetectionRegion", CIRCLE, 3.0, "none"); + addSource(POLYGON_NAME, POLYGON_SOURCE); + setVectors({"DetectionRegion"}, {POLYGON_NAME}); + + // Start Collision Detector node + cd_->start(); + + // Share TF + sendTransforms(curr_time); + + // Obstacle is in DetectionRegion + publishPolygon(2.5, curr_time); + + ASSERT_TRUE(waitData(std::hypot(2.5, 1.0), 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(); +} + TEST_F(Tester, testCollisionPointsMarkers) { rclcpp::Time curr_time = cd_->now(); diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index 34690a6fd1..69f8ac21e0 100644 --- a/nav2_collision_monitor/test/collision_monitor_node_test.cpp +++ b/nav2_collision_monitor/test/collision_monitor_node_test.cpp @@ -54,6 +54,7 @@ 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 POLYGON_NAME[]{"Polygon"}; static const int MIN_POINTS{2}; static const double SLOWDOWN_RATIO{0.7}; static const double LINEAR_LIMIT{0.4}; @@ -77,7 +78,8 @@ enum SourceType SOURCE_UNKNOWN = 0, SCAN = 1, POINTCLOUD = 2, - RANGE = 3 + RANGE = 3, + POLYGON_SOURCE = 4 }; enum ActionType @@ -165,6 +167,7 @@ class Tester : public ::testing::Test 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 publishPolygon(const double dist, const rclcpp::Time & stamp); void publishCmdVel(const double x, const double y, const double tw); bool waitData( const double expected_dist, @@ -192,6 +195,7 @@ class Tester : public ::testing::Test rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_source_pub_; // Working with cmd_vel_in/cmd_vel_out rclcpp::Publisher::SharedPtr cmd_vel_in_pub_; @@ -225,6 +229,8 @@ Tester::Tester() 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()); + polygon_source_pub_ = cm_->create_publisher( + POLYGON_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()); @@ -251,6 +257,7 @@ Tester::~Tester() scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + polygon_source_pub_.reset(); cmd_vel_in_pub_.reset(); cmd_vel_out_sub_.reset(); @@ -472,6 +479,20 @@ void Tester::addSource( source_name + ".obstacles_angle", rclcpp::ParameterValue(M_PI / 200)); cm_->set_parameter( rclcpp::Parameter(source_name + ".obstacles_angle", M_PI / 200)); + } else if (type == POLYGON_SOURCE) { + cm_->declare_parameter( + source_name + ".type", rclcpp::ParameterValue("polygon")); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".type", "polygon")); + + cm_->declare_parameter( + source_name + ".sampling_distance", rclcpp::ParameterValue(0.1)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".sampling_distance", 0.1)); + cm_->declare_parameter( + source_name + ".polygon_similarity_threshold", rclcpp::ParameterValue(2.0)); + cm_->set_parameter( + rclcpp::Parameter(source_name + ".polygon_similarity_threshold", 2.0)); } else { // type == SOURCE_UNKNOWN cm_->declare_parameter( source_name + ".type", rclcpp::ParameterValue("unknown")); @@ -628,6 +649,31 @@ void Tester::publishRange(const double dist, const rclcpp::Time & stamp) range_pub_->publish(std::move(msg)); } +void Tester::publishPolygon(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; + + geometry_msgs::msg::Point32 p; + p.x = 1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist; + msg->polygon.polygon.points.push_back(p); + p.x = -1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + p.x = 1.0; + p.y = dist + 1.0; + msg->polygon.polygon.points.push_back(p); + + polygon_source_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() @@ -813,6 +859,92 @@ TEST_F(Tester, testProcessStopSlowdownLimit) cm_->stop(); } +TEST_F(Tester, testPolygonSource) +{ + rclcpp::Time curr_time = cm_->now(); + + // Set Collision Monitor parameters. + // Making two polygons: outer polygon for slowdown and inner for robot stop. + setCommonParameters(); + // Set source_timeout to 0.0 to clear out quickly the polygons from test to test + cm_->set_parameter( + rclcpp::Parameter("source_timeout", 0.1)); + addPolygon("Limit", POLYGON, 3.0, "limit"); + addPolygon("SlowDown", POLYGON, 2.0, "slowdown"); + addPolygon("Stop", POLYGON, 1.0, "stop"); + addSource(POLYGON_NAME, POLYGON_SOURCE); + setVectors({"Limit", "SlowDown", "Stop"}, {POLYGON_NAME}); + + // Start Collision Monitor node + cm_->start(); + + // Share TF + sendTransforms(curr_time); + + // 1. Obstacle is far away from robot + publishPolygon(4.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + + // 2. Obstacle is in limit robot zone + publishPolygon(3.0, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 3.0), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + const double speed = std::sqrt(0.5 * 0.5 + 0.2 * 0.2); + const double ratio = LINEAR_LIMIT / speed; + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * ratio, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * ratio, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.09, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, LIMIT); + EXPECT_EQ(action_state_->polygon_name, "Limit"); + + // 3. Obstacle is in slowdown robot zone + publishPolygon(1.5, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 1.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.5 * SLOWDOWN_RATIO, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.2 * SLOWDOWN_RATIO, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.1 * SLOWDOWN_RATIO, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, SLOWDOWN); + EXPECT_EQ(action_state_->polygon_name, "SlowDown"); + + // 4. Obstacle is inside stop zone + curr_time = cm_->now(); + publishPolygon(0.5, curr_time); + EXPECT_TRUE(waitData(std::hypot(1.0, 0.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + EXPECT_TRUE(waitCmdVel(500ms)); + EXPECT_NEAR(cmd_vel_out_->linear.x, 0.0, EPSILON); + EXPECT_NEAR(cmd_vel_out_->linear.y, 0.0, EPSILON); + EXPECT_NEAR(cmd_vel_out_->angular.z, 0.0, EPSILON); + EXPECT_TRUE(waitActionState(500ms)); + EXPECT_EQ(action_state_->action_type, STOP); + EXPECT_EQ(action_state_->polygon_name, "Stop"); + + // 5. Restoring back normal operation + publishPolygon(4.5, curr_time); + ASSERT_TRUE(waitData(std::hypot(1.0, 4.5), 500ms, curr_time)); + publishCmdVel(0.5, 0.2, 0.1); + ASSERT_TRUE(waitCmdVel(500ms)); + ASSERT_NEAR(cmd_vel_out_->linear.x, 0.5, EPSILON); + ASSERT_NEAR(cmd_vel_out_->linear.y, 0.2, EPSILON); + ASSERT_NEAR(cmd_vel_out_->angular.z, 0.1, EPSILON); + ASSERT_TRUE(waitActionState(500ms)); + ASSERT_EQ(action_state_->action_type, DO_NOTHING); + ASSERT_EQ(action_state_->polygon_name, ""); + + // Stop Collision Monitor node + cm_->stop(); +} + TEST_F(Tester, testProcessApproach) { rclcpp::Time curr_time = cm_->now(); diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp index 0bfcd1a062..24de5cbe05 100644 --- a/nav2_collision_monitor/test/sources_test.cpp +++ b/nav2_collision_monitor/test/sources_test.cpp @@ -38,6 +38,7 @@ #include "nav2_collision_monitor/scan.hpp" #include "nav2_collision_monitor/pointcloud.hpp" #include "nav2_collision_monitor/range.hpp" +#include "nav2_collision_monitor/polygon_source.hpp" using namespace std::chrono_literals; @@ -52,6 +53,8 @@ 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 char POLYGON_NAME[]{"Polygon"}; +static const char POLYGON_TOPIC[]{"polygon"}; static const tf2::Duration TRANSFORM_TOLERANCE{tf2::durationFromSec(0.1)}; static const rclcpp::Duration DATA_TIMEOUT{rclcpp::Duration::from_seconds(5.0)}; @@ -68,6 +71,7 @@ class TestNode : public nav2_util::LifecycleNode scan_pub_.reset(); pointcloud_pub_.reset(); range_pub_.reset(); + polygon_pub_.reset(); } void publishScan(const rclcpp::Time & stamp, const double range) @@ -156,10 +160,43 @@ class TestNode : public nav2_util::LifecycleNode range_pub_->publish(std::move(msg)); } + void publishPolygon(const rclcpp::Time & stamp) + { + polygon_pub_ = this->create_publisher( + POLYGON_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; + + geometry_msgs::msg::Point32 point; + point.x = 1.0; + point.y = -1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = 1.0; + point.y = 1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = -1.0; + point.y = 1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + point.x = -1.0; + point.y = -1.0; + point.z = 0.0; + msg->polygon.polygon.points.push_back(point); + msg->polygon.id = 1u; + polygon_pub_->publish(std::move(msg)); + } + private: rclcpp::Publisher::SharedPtr scan_pub_; rclcpp::Publisher::SharedPtr pointcloud_pub_; rclcpp::Publisher::SharedPtr range_pub_; + rclcpp::Publisher::SharedPtr polygon_pub_; }; // TestNode class ScanWrapper : public nav2_collision_monitor::Scan @@ -231,6 +268,29 @@ class RangeWrapper : public nav2_collision_monitor::Range } }; // RangeWrapper +class PolygonWrapper : public nav2_collision_monitor::PolygonSource +{ +public: + PolygonWrapper( + 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, + const bool base_shift_correction) + : nav2_collision_monitor::PolygonSource( + node, source_name, tf_buffer, base_frame_id, global_frame_id, + transform_tolerance, data_timeout, base_shift_correction) + {} + + bool dataReceived() const + { + return data_.size() > 0; + } +}; // PolygonWrapper + class Tester : public ::testing::Test { public: @@ -248,14 +308,17 @@ class Tester : public ::testing::Test bool waitScan(const std::chrono::nanoseconds & timeout); bool waitPointCloud(const std::chrono::nanoseconds & timeout); bool waitRange(const std::chrono::nanoseconds & timeout); + bool waitPolygon(const std::chrono::nanoseconds & timeout); void checkScan(const std::vector & data); void checkPointCloud(const std::vector & data); void checkRange(const std::vector & data); + void checkPolygon(const std::vector & data); std::shared_ptr test_node_; std::shared_ptr scan_; std::shared_ptr pointcloud_; std::shared_ptr range_; + std::shared_ptr polygon_; private: std::shared_ptr tf_buffer_; @@ -331,6 +394,21 @@ void Tester::createSources(const bool base_shift_correction) BASE_FRAME_ID, GLOBAL_FRAME_ID, TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); range_->configure(); + + // Create Polygon object + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".topic", rclcpp::ParameterValue(POLYGON_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".topic", POLYGON_TOPIC)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".sampling_distance", rclcpp::ParameterValue(0.1)); + + polygon_ = std::make_shared( + test_node_, POLYGON_NAME, tf_buffer_, + BASE_FRAME_ID, GLOBAL_FRAME_ID, + TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction); + polygon_->configure(); } void Tester::sendTransforms(const rclcpp::Time & stamp) @@ -404,6 +482,19 @@ bool Tester::waitRange(const std::chrono::nanoseconds & timeout) return false; } +bool Tester::waitPolygon(const std::chrono::nanoseconds & timeout) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + if (polygon_->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); @@ -458,6 +549,11 @@ void Tester::checkRange(const std::vector & data) ASSERT_NEAR(data[i].y, 1.0 * std::sin(angle) + 0.1, EPSILON); } +void Tester::checkPolygon(const std::vector & data) +{ + ASSERT_EQ(data.size(), 84u); +} + TEST_F(Tester, testGetData) { rclcpp::Time curr_time = test_node_->now(); @@ -470,11 +566,13 @@ TEST_F(Tester, testGetData) test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Check Scan data std::vector data; @@ -490,6 +588,11 @@ TEST_F(Tester, testGetData) data.clear(); range_->getData(curr_time, data); checkRange(data); + + // Check Polygon data + data.clear(); + polygon_->getData(curr_time, data); + checkPolygon(data); } TEST_F(Tester, testGetOutdatedData) @@ -504,11 +607,13 @@ TEST_F(Tester, testGetOutdatedData) 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); + test_node_->publishPolygon(curr_time - DATA_TIMEOUT - 1s); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Scan data should be empty std::vector data; @@ -522,6 +627,10 @@ TEST_F(Tester, testGetOutdatedData) // Range data should be empty range_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Polygon data should be empty + polygon_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } TEST_F(Tester, testIncorrectFrameData) @@ -537,11 +646,13 @@ TEST_F(Tester, testIncorrectFrameData) test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Scan data should be empty std::vector data; @@ -555,6 +666,10 @@ TEST_F(Tester, testIncorrectFrameData) // Range data should be empty range_->getData(curr_time, data); ASSERT_EQ(data.size(), 0u); + + // Polygon data should be empty + polygon_->getData(curr_time, data); + ASSERT_EQ(data.size(), 0u); } TEST_F(Tester, testIncorrectData) @@ -597,11 +712,13 @@ TEST_F(Tester, testIgnoreTimeShift) test_node_->publishScan(curr_time, 1.0); test_node_->publishPointCloud(curr_time); test_node_->publishRange(curr_time, 1.0); + test_node_->publishPolygon(curr_time); // Wait until all sources will receive the data ASSERT_TRUE(waitScan(500ms)); ASSERT_TRUE(waitPointCloud(500ms)); ASSERT_TRUE(waitRange(500ms)); + ASSERT_TRUE(waitPolygon(500ms)); // Scan data should be consistent std::vector data; @@ -617,6 +734,11 @@ TEST_F(Tester, testIgnoreTimeShift) data.clear(); range_->getData(curr_time, data); checkRange(data); + + // Polygon data should be consistent + data.clear(); + polygon_->getData(curr_time, data); + checkPolygon(data); } int main(int argc, char ** argv) diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index 08c52af040..cb7d2b9fdf 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -25,6 +25,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2/time.h" +#include "tf2/transform_datatypes.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index 75aa37ada3..a9f4d137fa 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -169,6 +169,7 @@ bool getTransform( tf2::fromMsg(transform.transform, tf2_transform); return true; } + return false; }