Skip to content

Backport AMRNAV 4974 #33

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.

Sign up for GitHub

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

Merged
jplapp merged 2 commits into from
Jun 23, 2023
Merged

Backport AMRNAV 4974 #33

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
2c1eca1
Dynamically set max_points for collision monitor
HovorunB Jun 22, 2023
adc5f7b
remove log
HovorunB Jun 22, 2023
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
8 changes: 8 additions & 0 deletions nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,6 +162,12 @@ class Polygon
*/
virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic);

/**
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);

/**
* @brief Checks if point is inside polygon
* @param point Given point to check
Expand All @@ -175,6 +181,8 @@ class Polygon
nav2_util::LifecycleNode::WeakPtr node_;
/// @brief Collision monitor node logger stored for further usage
rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")};
/// @brief Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;

// Basic parameters
/// @brief Name of polygon
Expand Down
31 changes: 30 additions & 1 deletion nav2_collision_monitor/src/polygon.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include "nav2_util/node_utils.hpp"

#include "nav2_collision_monitor/kinematics.hpp"
using rcl_interfaces::msg::ParameterType;

namespace nav2_collision_monitor
{
Expand All @@ -44,6 +45,7 @@ Polygon::~Polygon()
{
RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str());
poly_.clear();
dyn_params_handler_.reset();
}

bool Polygon::configure()
Expand All @@ -58,6 +60,12 @@ bool Polygon::configure()
if (!getParameters(polygon_pub_topic, footprint_topic)) {
return false;
}
dyn_params_handler_ = node->add_on_set_parameters_callback(
std::bind(
&Polygon::dynamicParametersCallback,
this,
std::placeholders::_1)
);

if (!footprint_topic.empty()) {
footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
Expand Down Expand Up @@ -275,7 +283,7 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic)

nav2_util::declare_parameter_if_not_declared(
node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3));
max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int();
node->get_parameter(polygon_name_ + ".max_points", max_points_);

if (action_type_ == SLOWDOWN) {
nav2_util::declare_parameter_if_not_declared(
Expand Down Expand Up @@ -388,6 +396,27 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp
return true;
}

rcl_interfaces::msg::SetParametersResult
Polygon::dynamicParametersCallback(
std::vector<rclcpp::Parameter> parameters)
{
// std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
rcl_interfaces::msg::SetParametersResult result;

for (auto parameter : parameters) {
const auto & param_type = parameter.get_type();
const auto & param_name = parameter.get_name();

if (param_type == ParameterType::PARAMETER_INTEGER) {
if (param_name == polygon_name_ + "." + "max_points") {
max_points_ = parameter.as_int();
}
}
}
result.successful = true;
return result;
}

inline bool Polygon::isPointInside(const Point & point) const
{
// Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
Expand Down