From 11c1f914adad2d6048018dd63b636fa944551a8b Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov Date: Thu, 30 Nov 2023 15:55:23 +0300 Subject: [PATCH] Code clean-up * Corrected headers * Functions ordering * Comment fixes --- .../nav2_map_server/vector_object_server.hpp | 19 +- .../nav2_map_server/vector_object_shapes.hpp | 229 ++++++----- .../nav2_map_server/vector_object_utils.hpp | 1 + .../src/vo_server/vector_object_server.cpp | 121 +++--- .../src/vo_server/vector_object_shapes.cpp | 373 +++++++++--------- 5 files changed, 374 insertions(+), 369 deletions(-) diff --git a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp index 39afec55cfc..7e94072c8ee 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_server.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_server.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "tf2_ros/buffer.h" @@ -28,7 +29,6 @@ #include "nav2_msgs/srv/remove_shapes.hpp" #include "nav2_msgs/srv/get_shapes.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "nav2_util/occ_grid_values.hpp" #include "nav2_map_server/vector_object_utils.hpp" #include "nav2_map_server/vector_object_shapes.hpp" @@ -79,6 +79,12 @@ class VectorObjectServer : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief Supporting routine obtaining all ROS-parameters + * @return True if all parameters were obtained or false in the failure case + */ + bool obtainParams(); + /** * @brief Finds the shape with given UUID * @param uuid Given UUID to search with @@ -88,6 +94,7 @@ class VectorObjectServer : public nav2_util::LifecycleNode /** * @brief Transform all vector shapes from their local frame to output map frame + * @return Whether all vector objects were transformed successfully */ bool transformVectorObjects(); @@ -124,8 +131,8 @@ class VectorObjectServer : public nav2_util::LifecycleNode void publishMap(); /** - * @brief Calculates new map sizes, updates map, process all vector objects on it - * and publish output map one time + * @brief Calculates new map sizes, updates map, processes all vector objects on it + * and publishes output map one time */ void processMap(); @@ -174,12 +181,6 @@ class VectorObjectServer : public nav2_util::LifecycleNode std::shared_ptr response); protected: - /** - * @brief Supporting routine obtaining all ROS-parameters - * @return True if all parameters were obtained or false in the failure case - */ - bool obtainParams(); - /// @brief TF buffer std::shared_ptr tf_buffer_; /// @brief TF listener diff --git a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp index d7fabec4f91..aeae1855531 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp @@ -15,11 +15,10 @@ #ifndef NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ #define NAV2_MAP_SERVER__VECTOR_OBJECT_SHAPES_HPP_ -#include #include #include -#include +#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon.hpp" #include "geometry_msgs/msg/point32.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -71,42 +70,6 @@ class Shape */ bool obtainShapeUUID(const std::string & shape_name, unsigned char * out_uuid); - /** - * @brief Supporting routine obtaining ROS-parameters for the given vector object. - * Empty virtual method intended to be used in child implementations - * @param shape_name Name of the shape - * @return True if all parameters were obtained or false in failure case - */ - virtual bool obtainParams(const std::string & shape_name) = 0; - - /** - * @brief Gets shape boundaries. - * Empty virtual method intended to be used in child implementations - * @param min_x output min X-boundary of shape - * @param min_y output min Y-boundary of shape - * @param max_x output max X-boundary of shape - * @param max_y output max Y-boundary of shape - */ - virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0; - - /** - * @brief Is the point inside the shape. - * Empty virtual method intended to be used in child implementations - * @param px X-coordinate of the given point to check - * @param py Y-coordinate of the given point to check - * @return True if given point inside the shape - */ - virtual bool isPointInside(const double px, const double py) const = 0; - - /** - * @brief Puts shape borders on map. - * Empty virtual method intended to be used in child implementations - * @param map Output map pointer - * @param overlay_type Overlay type - */ - virtual void putBorders( - nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0; - /** * @brief Gets the value of the shape. * Empty virtual method intended to be used in child implementations @@ -143,6 +106,14 @@ class Shape */ virtual bool isFill() const = 0; + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * Empty virtual method intended to be used in child implementations + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + virtual bool obtainParams(const std::string & shape_name) = 0; + /** * @brief Transforms shape coordinates to a new frame. * Empty virtual method intended to be used in child implementations @@ -156,67 +127,52 @@ class Shape const std::shared_ptr tf_buffer, const double transform_tolerance) = 0; -protected: - /// @brief Type of shape - ShapeType type_; - - /// @brief VectorObjectServer node - nav2_util::LifecycleNode::WeakPtr node_; -}; - -/// @brief Polygon shape class -class Polygon : public Shape -{ -public: - /* - * @brief Polygon class constructor - * @param node Vector Object server node pointer - * @note setParams()/obtainParams() should be called after to configure the shape - */ - explicit Polygon(const nav2_util::LifecycleNode::WeakPtr & node); - - /** - * @brief Supporting routine obtaining ROS-parameters for the given vector object. - * @param shape_name Name of the shape - * @return True if all parameters were obtained or false in failure case - */ - bool obtainParams(const std::string & shape_name); - /** - * @brief Gets shape boundaries + * @brief Gets shape boundaries. + * Empty virtual method intended to be used in child implementations * @param min_x output min X-boundary of shape * @param min_y output min Y-boundary of shape * @param max_x output max X-boundary of shape * @param max_y output max Y-boundary of shape */ - void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + virtual void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) = 0; /** * @brief Is the point inside the shape. + * Empty virtual method intended to be used in child implementations * @param px X-coordinate of the given point to check * @param py Y-coordinate of the given point to check * @return True if given point inside the shape */ - bool isPointInside(const double px, const double py) const; + virtual bool isPointInside(const double px, const double py) const = 0; /** * @brief Puts shape borders on map. + * Empty virtual method intended to be used in child implementations * @param map Output map pointer * @param overlay_type Overlay type */ - void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + virtual void putBorders( + nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) = 0; - /** - * @brief Gets Polygon parameters - * @return Polygon parameters - */ - nav2_msgs::msg::PolygonObject::SharedPtr getParams() const; +protected: + /// @brief Type of shape + ShapeType type_; - /** - * @brief Tries to update Polygon parameters - * @return False in case of inconsistent shape + /// @brief VectorObjectServer node + nav2_util::LifecycleNode::WeakPtr node_; +}; + +/// @brief Polygon shape class +class Polygon : public Shape +{ +public: + /* + * @brief Polygon class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape */ - bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + explicit Polygon(const nav2_util::LifecycleNode::WeakPtr & node); /** * @brief Gets the value of the shape. @@ -249,6 +205,25 @@ class Polygon : public Shape */ bool isFill() const; + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Polygon parameters + * @return Polygon parameters + */ + nav2_msgs::msg::PolygonObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Polygon parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params); + /** * @brief Transforms shape coordinates to a new frame. * @param to_frame Frame ID to transform to @@ -261,37 +236,6 @@ class Polygon : public Shape const std::shared_ptr tf_buffer, const double transform_tolerance); -protected: - /** - * @brief Checks that shape is consistent for further operation - * @return False in case of inconsistent shape - */ - bool checkConsistency(); - - /// @brief Input polygon parameters (could be in any frame) - nav2_msgs::msg::PolygonObject::SharedPtr params_; - /// @brief Polygon in the map's frame - geometry_msgs::msg::Polygon::SharedPtr polygon_; -}; - -/// @brief Circle shape class -class Circle : public Shape -{ -public: - /* - * @brief Circle class constructor - * @param node Vector Object server node pointer - * @note setParams()/obtainParams() should be called after to configure the shape - */ - explicit Circle(const nav2_util::LifecycleNode::WeakPtr & node); - - /** - * @brief Supporting routine obtaining ROS-parameters for the given vector object. - * @param shape_name Name of the shape - * @return True if all parameters were obtained or false in failure case - */ - bool obtainParams(const std::string & shape_name); - /** * @brief Gets shape boundaries * @param min_x output min X-boundary of shape @@ -316,17 +260,29 @@ class Circle : public Shape */ void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); +protected: /** - * @brief Gets Circle parameters - * @return Circle parameters + * @brief Checks that shape is consistent for further operation + * @return False in case of inconsistent shape */ - nav2_msgs::msg::CircleObject::SharedPtr getParams() const; + bool checkConsistency(); - /** - * @brief Tries to update Circle parameters - * @return False in case of inconsistent shape + /// @brief Input polygon parameters (could be in any frame) + nav2_msgs::msg::PolygonObject::SharedPtr params_; + /// @brief Polygon in the map's frame + geometry_msgs::msg::Polygon::SharedPtr polygon_; +}; + +/// @brief Circle shape class +class Circle : public Shape +{ +public: + /* + * @brief Circle class constructor + * @param node Vector Object server node pointer + * @note setParams()/obtainParams() should be called after to configure the shape */ - bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + explicit Circle(const nav2_util::LifecycleNode::WeakPtr & node); /** * @brief Gets the value of the shape. @@ -359,6 +315,25 @@ class Circle : public Shape */ bool isFill() const; + /** + * @brief Supporting routine obtaining ROS-parameters for the given vector object. + * @param shape_name Name of the shape + * @return True if all parameters were obtained or false in failure case + */ + bool obtainParams(const std::string & shape_name); + + /** + * @brief Gets Circle parameters + * @return Circle parameters + */ + nav2_msgs::msg::CircleObject::SharedPtr getParams() const; + + /** + * @brief Tries to update Circle parameters + * @return False in case of inconsistent shape + */ + bool setParams(const nav2_msgs::msg::CircleObject::SharedPtr params); + /** * @brief Transforms shape coordinates to a new frame. * @param to_frame Frame ID to transform to @@ -371,6 +346,30 @@ class Circle : public Shape const std::shared_ptr tf_buffer, const double transform_tolerance); + /** + * @brief Gets shape boundaries + * @param min_x output min X-boundary of shape + * @param min_y output min Y-boundary of shape + * @param max_x output max X-boundary of shape + * @param max_y output max Y-boundary of shape + */ + void getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y); + + /** + * @brief Is the point inside the shape. + * @param px X-coordinate of the given point to check + * @param py Y-coordinate of the given point to check + * @return True if given point inside the shape + */ + bool isPointInside(const double px, const double py) const; + + /** + * @brief Puts shape borders on map. + * @param map Output map pointer + * @param overlay_type Overlay type + */ + void putBorders(nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type); + protected: /** * @brief Checks that shape is consistent for further operation diff --git a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp index 8814dc3253d..1903bc5d002 100644 --- a/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp +++ b/nav2_map_server/include/nav2_map_server/vector_object_utils.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_util/lifecycle_node.hpp" diff --git a/nav2_map_server/src/vo_server/vector_object_server.cpp b/nav2_map_server/src/vo_server/vector_object_server.cpp index c76de4c49d6..291b4622005 100644 --- a/nav2_map_server/src/vo_server/vector_object_server.cpp +++ b/nav2_map_server/src/vo_server/vector_object_server.cpp @@ -14,14 +14,19 @@ #include "nav2_map_server/vector_object_server.hpp" -#include +#include +#include #include #include #include +#include + +#include "rclcpp/create_timer.hpp" #include "tf2_ros/create_timer_ros.h" #include "nav2_util/occ_grid_utils.hpp" +#include "nav2_util/occ_grid_values.hpp" using namespace std::placeholders; @@ -133,6 +138,63 @@ VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } +bool VectorObjectServer::obtainParams() +{ + // Main ROS-parameters + map_topic_ = getParameter( + shared_from_this(), "map_topic", "vo_map").as_string(); + global_frame_id_ = getParameter( + shared_from_this(), "global_frame_id", "map").as_string(); + resolution_ = getParameter( + shared_from_this(), "resolution", 0.05).as_double(); + default_value_ = getParameter( + shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int(); + overlay_type_ = static_cast(getParameter( + shared_from_this(), "overlay_type", static_cast(OverlayType::OVERLAY_SEQ)).as_int()); + update_frequency_ = getParameter( + shared_from_this(), "update_frequency", 1.0).as_double(); + transform_tolerance_ = getParameter( + shared_from_this(), "transform_tolerance", 0.1).as_double(); + + // Shapes + std::vector shape_names = getParameter( + shared_from_this(), "shapes", std::vector()).as_string_array(); + for (std::string shape_name : shape_names) { + std::string shape_type; + + try { + shape_type = getParameter( + shared_from_this(), shape_name + ".type", rclcpp::PARAMETER_STRING).as_string(); + } catch (const std::exception & ex) { + RCLCPP_ERROR( + get_logger(), + "Error while getting shape %s type: %s", + shape_name.c_str(), ex.what()); + return false; + } + + if (shape_type == "polygon") { + std::shared_ptr polygon = std::make_shared(shared_from_this()); + if (!polygon->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(polygon); + } else if (shape_type == "circle") { + std::shared_ptr circle = std::make_shared(shared_from_this()); + + if (!circle->obtainParams(shape_name)) { + return false; + } + shapes_.push_back(circle); + } else { + RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str()); + return false; + } + } + + return true; +} + std::vector>::iterator VectorObjectServer::findShape(const unsigned char * uuid) { @@ -486,63 +548,6 @@ void VectorObjectServer::removeShapesCallback( switchMapUpdate(); } -bool VectorObjectServer::obtainParams() -{ - // Main ROS-parameters - map_topic_ = getParameter( - shared_from_this(), "map_topic", "vo_map").as_string(); - global_frame_id_ = getParameter( - shared_from_this(), "global_frame_id", "map").as_string(); - resolution_ = getParameter( - shared_from_this(), "resolution", 0.05).as_double(); - default_value_ = getParameter( - shared_from_this(), "default_value", nav2_util::OCC_GRID_UNKNOWN).as_int(); - overlay_type_ = static_cast(getParameter( - shared_from_this(), "overlay_type", static_cast(OverlayType::OVERLAY_SEQ)).as_int()); - update_frequency_ = getParameter( - shared_from_this(), "update_frequency", 1.0).as_double(); - transform_tolerance_ = getParameter( - shared_from_this(), "transform_tolerance", 0.1).as_double(); - - // Shapes - std::vector shape_names = getParameter( - shared_from_this(), "shapes", std::vector()).as_string_array(); - for (std::string shape_name : shape_names) { - std::string shape_type; - - try { - shape_type = getParameter( - shared_from_this(), shape_name + ".type", rclcpp::PARAMETER_STRING).as_string(); - } catch (const std::exception & ex) { - RCLCPP_ERROR( - get_logger(), - "Error while getting shape %s type: %s", - shape_name.c_str(), ex.what()); - return false; - } - - if (shape_type == "polygon") { - std::shared_ptr polygon = std::make_shared(shared_from_this()); - if (!polygon->obtainParams(shape_name)) { - return false; - } - shapes_.push_back(polygon); - } else if (shape_type == "circle") { - std::shared_ptr circle = std::make_shared(shared_from_this()); - - if (!circle->obtainParams(shape_name)) { - return false; - } - shapes_.push_back(circle); - } else { - RCLCPP_ERROR(get_logger(), "Please specify correct shape %s type", shape_name.c_str()); - return false; - } - } - - return true; -} - } // namespace nav2_map_server #include "rclcpp_components/register_node_macro.hpp" diff --git a/nav2_map_server/src/vo_server/vector_object_shapes.cpp b/nav2_map_server/src/vo_server/vector_object_shapes.cpp index 165e199cae9..44022bc061d 100644 --- a/nav2_map_server/src/vo_server/vector_object_shapes.cpp +++ b/nav2_map_server/src/vo_server/vector_object_shapes.cpp @@ -15,10 +15,11 @@ #include "nav2_map_server/vector_object_shapes.hpp" #include -#include #include +#include #include #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" @@ -87,6 +88,31 @@ Polygon::Polygon( type_ = POLYGON; } +int8_t Polygon::getValue() const +{ + return params_->value; +} + +std::string Polygon::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Polygon::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Polygon::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Polygon::isFill() const +{ + return params_->closed; +} + bool Polygon::obtainParams(const std::string & shape_name) { auto node = node_.lock(); @@ -149,6 +175,54 @@ bool Polygon::obtainParams(const std::string & shape_name) return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); } +nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const +{ + return params_; +} + +bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) +{ + params_ = params; + + if (!polygon_) { + polygon_ = std::make_shared(); + } + polygon_->points = params_->points; + + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); + } + + return checkConsistency(); +} + +bool Polygon::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + for (unsigned int i = 0; i < params_->points.size(); i++) { + from_pose.pose.position.x = params_->points[i].x; + from_pose.pose.position.y = params_->points[i].y; + from_pose.pose.position.z = params_->points[i].z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + polygon_->points[i].x = to_pose.pose.position.x; + polygon_->points[i].y = to_pose.pose.position.y; + polygon_->points[i].z = to_pose.pose.position.z; + } else { + return false; + } + } + + return true; +} + void Polygon::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) { min_x = std::numeric_limits::max(); @@ -210,79 +284,6 @@ void Polygon::putBorders( } } -nav2_msgs::msg::PolygonObject::SharedPtr Polygon::getParams() const -{ - return params_; -} - -bool Polygon::setParams(const nav2_msgs::msg::PolygonObject::SharedPtr params) -{ - params_ = params; - - if (!polygon_) { - polygon_ = std::make_shared(); - } - polygon_->points = params_->points; - - // If no UUID was specified, generate a new one - if (uuid_is_null(params_->uuid.uuid.data())) { - uuid_generate(params_->uuid.uuid.data()); - } - - return checkConsistency(); -} - -int8_t Polygon::getValue() const -{ - return params_->value; -} - -std::string Polygon::getFrameID() const -{ - return params_->header.frame_id; -} - -std::string Polygon::getUUID() const -{ - return unparseUUID(params_->uuid.uuid.data()); -} - -bool Polygon::isUUID(const unsigned char * uuid) const -{ - return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; -} - -bool Polygon::isFill() const -{ - return params_->closed; -} - -bool Polygon::toFrame( - const std::string & to_frame, - const std::shared_ptr tf_buffer, - const double transform_tolerance) -{ - geometry_msgs::msg::PoseStamped from_pose, to_pose; - from_pose.header = params_->header; - for (unsigned int i = 0; i < params_->points.size(); i++) { - from_pose.pose.position.x = params_->points[i].x; - from_pose.pose.position.y = params_->points[i].y; - from_pose.pose.position.z = params_->points[i].z; - if ( - nav2_util::transformPoseInTargetFrame( - from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) - { - polygon_->points[i].x = to_pose.pose.position.x; - polygon_->points[i].y = to_pose.pose.position.y; - polygon_->points[i].z = to_pose.pose.position.z; - } else { - return false; - } - } - - return true; -} - bool Polygon::checkConsistency() { if (params_->points.size() < 3) { @@ -310,6 +311,31 @@ Circle::Circle( type_ = CIRCLE; } +int8_t Circle::getValue() const +{ + return params_->value; +} + +std::string Circle::getFrameID() const +{ + return params_->header.frame_id; +} + +std::string Circle::getUUID() const +{ + return unparseUUID(params_->uuid.uuid.data()); +} + +bool Circle::isUUID(const unsigned char * uuid) const +{ + return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; +} + +bool Circle::isFill() const +{ + return params_->fill; +} + bool Circle::obtainParams(const std::string & shape_name) { auto node = node_.lock(); @@ -371,65 +397,66 @@ bool Circle::obtainParams(const std::string & shape_name) return obtainShapeUUID(shape_name, params_->uuid.uuid.data()); } -// Get/update shape boundaries -void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) +nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const { - min_x = center_->x - params_->radius; - min_y = center_->y - params_->radius; - max_x = center_->x + params_->radius; - max_y = center_->y + params_->radius; + return params_; } -bool Circle::isPointInside(const double px, const double py) const +bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) { - return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <= - params_->radius * params_->radius; -} + params_ = params; -bool Circle::centerToMap( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, - unsigned int & mcx, unsigned int & mcy) -{ - auto node = node_.lock(); - if (!node) { - throw std::runtime_error{"Failed to lock node"}; + if (!center_) { + center_ = std::make_shared(); } + *center_ = params_->center; - // Get center of circle in map coordinates - if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) { - RCLCPP_ERROR( - node->get_logger(), - "[UUID: %s] Can not convert (%f, %f) circle center to map", - getUUID().c_str(), center_->x, center_->y); - return false; + // If no UUID was specified, generate a new one + if (uuid_is_null(params_->uuid.uuid.data())) { + uuid_generate(params_->uuid.uuid.data()); } - // We need the circle center to be always shifted one cell less its logical center - // and to avoid any FP-accuracy loosing on small values, so we are using another - // than nav2_util::worldToMap() approach - mcx = static_cast( - std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1; - mcy = static_cast( - std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1; - if (mcx >= map->info.width || mcy >= map->info.height) { - RCLCPP_ERROR( - node->get_logger(), - "[UUID: %s] Can not convert (%f, %f) point to map", - getUUID().c_str(), center_->x, center_->y); + + return checkConsistency(); +} + +bool Circle::toFrame( + const std::string & to_frame, + const std::shared_ptr tf_buffer, + const double transform_tolerance) +{ + geometry_msgs::msg::PoseStamped from_pose, to_pose; + from_pose.header = params_->header; + from_pose.pose.position.x = params_->center.x; + from_pose.pose.position.y = params_->center.y; + from_pose.pose.position.z = params_->center.z; + if ( + nav2_util::transformPoseInTargetFrame( + from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) + { + center_->x = to_pose.pose.position.x; + center_->y = to_pose.pose.position.y; + center_->z = to_pose.pose.position.z; + } else { return false; } return true; } -inline void Circle::putPoint( - unsigned int mx, unsigned int my, - nav_msgs::msg::OccupancyGrid::SharedPtr map, - const OverlayType overlay_type) +void Circle::getBoundaries(double & min_x, double & min_y, double & max_x, double & max_y) { - fillMap(map, my * map->info.width + mx, params_->value, overlay_type); + min_x = center_->x - params_->radius; + min_y = center_->y - params_->radius; + max_x = center_->x + params_->radius; + max_y = center_->y + params_->radius; +} + +bool Circle::isPointInside(const double px, const double py) const +{ + return ( (px - center_->x) * (px - center_->x) + (py - center_->y) * (py - center_->y) ) <= + params_->radius * params_->radius; } -// Put params_gons line borders on map void Circle::putBorders( nav_msgs::msg::OccupancyGrid::SharedPtr map, const OverlayType overlay_type) { @@ -479,77 +506,6 @@ void Circle::putBorders( } } -nav2_msgs::msg::CircleObject::SharedPtr Circle::getParams() const -{ - return params_; -} - -bool Circle::setParams(const nav2_msgs::msg::CircleObject::SharedPtr params) -{ - params_ = params; - - if (!center_) { - center_ = std::make_shared(); - } - *center_ = params_->center; - - // If no UUID was specified, generate a new one - if (uuid_is_null(params_->uuid.uuid.data())) { - uuid_generate(params_->uuid.uuid.data()); - } - - return checkConsistency(); -} - -int8_t Circle::getValue() const -{ - return params_->value; -} - -std::string Circle::getFrameID() const -{ - return params_->header.frame_id; -} - -std::string Circle::getUUID() const -{ - return unparseUUID(params_->uuid.uuid.data()); -} - -bool Circle::isUUID(const unsigned char * uuid) const -{ - return uuid_compare(params_->uuid.uuid.data(), uuid) == 0; -} - -bool Circle::isFill() const -{ - return params_->fill; -} - -bool Circle::toFrame( - const std::string & to_frame, - const std::shared_ptr tf_buffer, - const double transform_tolerance) -{ - geometry_msgs::msg::PoseStamped from_pose, to_pose; - from_pose.header = params_->header; - from_pose.pose.position.x = params_->center.x; - from_pose.pose.position.y = params_->center.y; - from_pose.pose.position.z = params_->center.z; - if ( - nav2_util::transformPoseInTargetFrame( - from_pose, to_pose, *tf_buffer, to_frame, transform_tolerance)) - { - center_->x = to_pose.pose.position.x; - center_->y = to_pose.pose.position.y; - center_->z = to_pose.pose.position.z; - } else { - return false; - } - - return true; -} - bool Circle::checkConsistency() { if (params_->radius < 0.0) { @@ -567,4 +523,47 @@ bool Circle::checkConsistency() return true; } +bool Circle::centerToMap( + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map, + unsigned int & mcx, unsigned int & mcy) +{ + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + + // Get center of circle in map coordinates + if (center_->x < map->info.origin.position.x || center_->y < map->info.origin.position.y) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) circle center to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + // We need the circle center to be always shifted one cell less its logical center + // and to avoid any FP-accuracy loosing on small values, so we are using another + // than nav2_util::worldToMap() approach + mcx = static_cast( + std::round((center_->x - map->info.origin.position.x) / map->info.resolution)) - 1; + mcy = static_cast( + std::round((center_->y - map->info.origin.position.y) / map->info.resolution)) - 1; + if (mcx >= map->info.width || mcy >= map->info.height) { + RCLCPP_ERROR( + node->get_logger(), + "[UUID: %s] Can not convert (%f, %f) point to map", + getUUID().c_str(), center_->x, center_->y); + return false; + } + + return true; +} + +inline void Circle::putPoint( + unsigned int mx, unsigned int my, + nav_msgs::msg::OccupancyGrid::SharedPtr map, + const OverlayType overlay_type) +{ + fillMap(map, my * map->info.width + mx, params_->value, overlay_type); +} + } // namespace nav2_map_server