Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Vector Object server #4680

Open
wants to merge 23 commits into
base: main
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
23 commits
Select commit Hold shift + click to select a range
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Switch to vector of shapes
Signed-off-by: Alexey Merzlyakov <alexey.merzlyakov@samsung.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
AlexeyMerzlyakov authored and ajtudela committed Sep 17, 2024
commit b4b303f238444dd8826c931edf2731960a9c0b23
13 changes: 10 additions & 3 deletions nav2_map_server/include/nav2_map_server/vector_object_server.hpp
Original file line number Diff line number Diff line change
@@ -17,7 +17,7 @@

#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include "nav_msgs/msg/occupancy_grid.hpp"

@@ -79,6 +79,13 @@ class VectorObjectServer : public nav2_util::LifecycleNode
*/
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;

/**
* @brief Finds the shape with given UUID
* @param uuid Given UUID to search with
* @return Iterator to the shape, if found. Otherwise past-the-end iterator.
*/
std::vector<std::shared_ptr<Shape>>::iterator findShape(const unsigned char * uuid);

/**
* @brief Transform all vector shapes from their local frame to output map frame
*/
@@ -178,8 +185,8 @@ class VectorObjectServer : public nav2_util::LifecycleNode
/// @brief TF listener
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

/// @brief All {UUID, Shape} vector objects map
std::unordered_map<std::string, std::shared_ptr<Shape>> shapes_;
/// @brief All shapes vector
std::vector<std::shared_ptr<Shape>> shapes_;

/// @brief Output map resolution
double resolution_;
22 changes: 22 additions & 0 deletions nav2_map_server/include/nav2_map_server/vector_object_shapes.hpp
Original file line number Diff line number Diff line change
@@ -128,6 +128,14 @@ class Shape
*/
virtual std::string getUUID() const = 0;

/**
* @brief Checks whether the shape is equal to a given UUID.
* Empty virtual method intended to be used in child implementations
* @param uuid Given UUID to check with
* @return True if the shape has the same as given UUID, otherwise false
*/
virtual bool isUUID(const unsigned char * uuid) const = 0;

/**
* @brief Whether the shape to be filled or only its borders to be put on map.
* Empty virtual method intended to be used in child implementations
@@ -228,6 +236,13 @@ class Polygon : public Shape
*/
std::string getUUID() const;

/**
* @brief Checks whether the shape is equal to a given UUID.
* @param uuid Given UUID to check with
* @return True if the shape has the same as given UUID, otherwise false
*/
bool isUUID(const unsigned char * uuid) const;

/**
* @brief Whether the shape to be filled or only its borders to be put on map.
* @return True if shape to be filled
@@ -331,6 +346,13 @@ class Circle : public Shape
*/
std::string getUUID() const;

/**
* @brief Checks whether the shape is equal to a given UUID.
* @param uuid Given UUID to check with
* @return True if the shape has the same as given UUID, otherwise false
*/
bool isUUID(const unsigned char * uuid) const;

/**
* @brief Whether the shape to be filled or only its borders to be put on map.
* @return True if shape to be filled
79 changes: 45 additions & 34 deletions nav2_map_server/src/vo_server/vector_object_server.cpp
Original file line number Diff line number Diff line change
@@ -133,15 +133,26 @@ VectorObjectServer::on_shutdown(const rclcpp_lifecycle::State & /*state*/)
return nav2_util::CallbackReturn::SUCCESS;
}

std::vector<std::shared_ptr<Shape>>::iterator
VectorObjectServer::findShape(const unsigned char * uuid)
{
for (auto it = shapes_.begin(); it != shapes_.end(); it++) {
if ((*it)->isUUID(uuid)) {
return it;
}
}
return shapes_.end();
}

bool VectorObjectServer::transformVectorObjects()
{
for (auto shape : shapes_) {
if (shape.second->getFrameID() != global_frame_id_ && !shape.second->getFrameID().empty()) {
if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) {
// Shape to be updated dynamically
if (!shape.second->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) {
if (!shape->toFrame(global_frame_id_, tf_buffer_, transform_tolerance_)) {
RCLCPP_ERROR(
get_logger(), "Can not transform vector object from %s to %s frame",
shape.second->getFrameID().c_str(), global_frame_id_.c_str());
shape->getFrameID().c_str(), global_frame_id_.c_str());
return false;
}
}
@@ -161,7 +172,7 @@ void VectorObjectServer::getMapBoundaries(
double min_p_x, min_p_y, max_p_x, max_p_y;

for (auto shape : shapes_) {
shape.second->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y);
shape->getBoundaries(min_p_x, min_p_y, max_p_x, max_p_y);
if (min_p_x < min_x) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::min and std::max would be more concise

min_x = min_p_x;
}
@@ -225,7 +236,7 @@ void VectorObjectServer::putVectorObjectsOnMap()
{
// Filling the shapes
for (auto shape : shapes_) {
if (shape.second->isFill()) {
if (shape->isFill()) {
// Put filled shape on map
double wx1 = std::numeric_limits<double>::max();
double wy1 = std::numeric_limits<double>::max();
@@ -236,15 +247,15 @@ void VectorObjectServer::putVectorObjectsOnMap()
unsigned int mx2 = 0;
unsigned int my2 = 0;

shape.second->getBoundaries(wx1, wy1, wx2, wy2);
shape->getBoundaries(wx1, wy1, wx2, wy2);
if (
!nav2_util::worldToMap(map_, wx1, wy1, mx1, my1) ||
!nav2_util::worldToMap(map_, wx2, wy2, mx2, my2))
{
RCLCPP_ERROR(
get_logger(),
"Error to get shape boundaries on map (UUID: %s)",
shape.second->getUUID().c_str());
shape->getUUID().c_str());
return;
}

@@ -254,14 +265,14 @@ void VectorObjectServer::putVectorObjectsOnMap()
it = my * map_->info.width + mx;
double wx, wy;
nav2_util::mapToWorld(map_, mx, my, wx, wy);
if (shape.second->isPointInside(wx, wy)) {
processVal(map_->data[it], shape.second->getValue(), overlay_type_);
if (shape->isPointInside(wx, wy)) {
processVal(map_->data[it], shape->getValue(), overlay_type_);
}
}
}
} else {
// Put shape borders on map
shape.second->putBorders(map_, overlay_type_);
shape->putBorders(map_, overlay_type_);
}
}
}
@@ -304,7 +315,7 @@ void VectorObjectServer::processMap()
void VectorObjectServer::switchMapUpdate()
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am a little baffled on this method.

Do you have an idea why we create a map_timer_ for each object who's frame ID doesn't match the current frame ID? It seems to override each other. We then instantly cancel it if it exists too... and processMap

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not exactly sure. I read this in the doc PR:

  • If at least one of the shape is set in different than map's frame, dynamic update model to be enabled: this shape can move over the time, output map will be published dynamically with a given rate.
  • If all shapes are set in the same as map frame, map is being published/updated once: during Vector Object server startup and per each shape changing call (AddShapes.srv or RemoveShapes.srv).

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh my bad, there's a return; in there I missed. Its not immediately canceled, the function returns if anything is dynamic -- only gets to the cancel part of everything is static

{
for (auto shape : shapes_) {
if (shape.second->getFrameID() != global_frame_id_ && !shape.second->getFrameID().empty()) {
if (shape->getFrameID() != global_frame_id_ && !shape->getFrameID().empty()) {
if (!map_timer_) {
map_timer_ = this->create_timer(
std::chrono::duration<double>(1.0 / update_frequency_),
@@ -339,21 +350,21 @@ void VectorObjectServer::addShapesCallback(
nav2_msgs::msg::PolygonObject::SharedPtr new_params =
std::make_shared<nav2_msgs::msg::PolygonObject>(req_poly);

auto shape = shapes_.find(unparseUUID(new_params->uuid.uuid.data()));
if (shape != shapes_.end()) {
auto it = findShape(new_params->uuid.uuid.data());
if (it != shapes_.end()) {
// Vector Object with given UUID was found: updating it
// Check that found shape has correct type
if (shape->second->getType() != POLYGON) {
if ((*it)->getType() != POLYGON) {
RCLCPP_ERROR(
get_logger(),
"Shape (UUID: %s) is not a polygon type",
shape->second->getUUID().c_str());
(*it)->getUUID().c_str());
response->success = false;
// Do not add this shape
continue;
}

std::shared_ptr<Polygon> polygon = std::static_pointer_cast<Polygon>(shape->second);
std::shared_ptr<Polygon> polygon = std::static_pointer_cast<Polygon>(*it);

// Preserving old parameters for the case, if new ones to be incorrect
nav2_msgs::msg::PolygonObject::SharedPtr old_params = polygon->getParams();
@@ -367,7 +378,7 @@ void VectorObjectServer::addShapesCallback(
// Vector Object with given UUID was not found: creating a new one
std::shared_ptr<Polygon> polygon = std::make_shared<Polygon>(node);
if (polygon->setParams(new_params)) {
shapes_.insert({polygon->getUUID(), polygon});
shapes_.push_back(polygon);
} else {
response->success = false;
}
@@ -379,21 +390,21 @@ void VectorObjectServer::addShapesCallback(
nav2_msgs::msg::CircleObject::SharedPtr new_params =
std::make_shared<nav2_msgs::msg::CircleObject>(req_crcl);

auto shape = shapes_.find(unparseUUID(new_params->uuid.uuid.data()));
if (shape != shapes_.end()) {
auto it = findShape(new_params->uuid.uuid.data());
if (it != shapes_.end()) {
// Vector object with given UUID was found: updating it
// Check that found shape has correct type
if (shape->second->getType() != CIRCLE) {
if ((*it)->getType() != CIRCLE) {
RCLCPP_ERROR(
get_logger(),
"Shape (UUID: %s) is not a circle type",
shape->second->getUUID().c_str());
(*it)->getUUID().c_str());
response->success = false;
// Do not add this shape
continue;
}

std::shared_ptr<Circle> circle = std::static_pointer_cast<Circle>(shape->second);
std::shared_ptr<Circle> circle = std::static_pointer_cast<Circle>(*it);

// Preserving old parameters for the case, if new ones to be incorrect
nav2_msgs::msg::CircleObject::SharedPtr old_params = circle->getParams();
@@ -407,7 +418,7 @@ void VectorObjectServer::addShapesCallback(
// Vector Object with given UUID was not found: creating a new one
std::shared_ptr<Circle> circle = std::make_shared<Circle>(node);
if (circle->setParams(new_params)) {
shapes_.insert({circle->getUUID(), circle});
shapes_.push_back(circle);
} else {
response->success = false;
}
@@ -426,17 +437,17 @@ void VectorObjectServer::getShapesCallback(
std::shared_ptr<Circle> circle;

for (auto shape : shapes_) {
switch (shape.second->getType()) {
switch (shape->getType()) {
case POLYGON:
polygon = std::static_pointer_cast<Polygon>(shape.second);
polygon = std::static_pointer_cast<Polygon>(shape);
response->polygons.push_back(*(polygon->getParams()));
break;
case CIRCLE:
circle = std::static_pointer_cast<Circle>(shape.second);
circle = std::static_pointer_cast<Circle>(shape);
response->circles.push_back(*(circle->getParams()));
break;
default:
RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape.second->getUUID().c_str());
RCLCPP_WARN(get_logger(), "Unknown shape type (UUID: %s)", shape->getUUID().c_str());
}
}
}
@@ -456,11 +467,11 @@ void VectorObjectServer::removeShapesCallback(
} else {
// Find objects to remove
for (auto req_uuid : request->uuids) {
auto shape = shapes_.find(unparseUUID(req_uuid.uuid.data()));
if (shape != shapes_.end()) {
// Polygon with given UUID was found: remove it
shape->second.reset();
shapes_.erase(shape);
auto it = findShape(req_uuid.uuid.data());
if (it != shapes_.end()) {
// Shape with given UUID was found: remove it
(*it).reset();
shapes_.erase(it);
} else {
// Required vector object was not found
RCLCPP_ERROR(
@@ -516,7 +527,7 @@ bool VectorObjectServer::obtainParams()
if (!polygon->obtainParams(shape_name)) {
return false;
}
shapes_.insert({polygon->getUUID(), polygon});
shapes_.push_back(polygon);
} catch (const std::exception & ex) {
RCLCPP_ERROR(get_logger(), "Can not create new polygon: %s", ex.what());
return false;
@@ -528,7 +539,7 @@ bool VectorObjectServer::obtainParams()
if (!circle->obtainParams(shape_name)) {
return false;
}
shapes_.insert({circle->getUUID(), circle});
shapes_.push_back(circle);
} catch (const std::exception & ex) {
RCLCPP_ERROR(get_logger(), "Can not create new circle: %s", ex.what());
return false;
10 changes: 10 additions & 0 deletions nav2_map_server/src/vo_server/vector_object_shapes.cpp
Original file line number Diff line number Diff line change
@@ -275,6 +275,11 @@ 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;
@@ -539,6 +544,11 @@ 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;