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.

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

Jump to bottom

[Draft] Set parameters result example #2576

Merged
merged 4 commits into from
Oct 5, 2021
Merged

[Draft] Set parameters result example #2576

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
b0d44d3
change dyn param method
Sep 30, 2021
ff977e9
typo
Sep 30, 2021
d25d14d
remove unnecessary log
Sep 30, 2021
3df6b1d
handler init in activate
Oct 4, 2021
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
9 changes: 5 additions & 4 deletions nav2_smac_planner/include/nav2_smac_planner/smac_planner_2d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/node_utils.hpp"
#include "tf2/utils.h"
#include "rcl_interfaces/msg/set_parameters_result.hpp"

namespace nav2_smac_planner
{
Expand Down Expand Up @@ -92,7 +93,8 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
* @brief Callback executed when a parameter change is detected
* @param event ParameterEvent message
*/
void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event);
rcl_interfaces::msg::SetParametersResult
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);

std::unique_ptr<AStarAlgorithm<Node2D>> _a_star;
GridCollisionChecker _collision_checker;
Expand All @@ -117,9 +119,8 @@ class SmacPlanner2D : public nav2_core::GlobalPlanner
std::mutex _mutex;
rclcpp_lifecycle::LifecycleNode::WeakPtr _node;

// Subscription for parameter change
rclcpp::AsyncParametersClient::SharedPtr _parameters_client;
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr _parameter_event_sub;
// Dynamic parameters handler
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;
};

} // namespace nav2_smac_planner
Expand Down
49 changes: 23 additions & 26 deletions nav2_smac_planner/src/smac_planner_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,16 +147,6 @@ void SmacPlanner2D::configure(

_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);

// Setup callback for changes to parameters.
_parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
node->get_node_base_interface(),
node->get_node_topics_interface(),
node->get_node_graph_interface(),
node->get_node_services_interface());

_parameter_event_sub = _parameters_client->on_parameter_event(
std::bind(&SmacPlanner2D::on_parameter_event_callback, this, _1));

RCLCPP_INFO(
_logger, "Configured plugin %s of type SmacPlanner2D with "
"tolerance %.2f, maximum iterations %i, "
Expand All @@ -175,6 +165,10 @@ void SmacPlanner2D::activate()
if (_costmap_downsampler) {
_costmap_downsampler->on_activate();
}
auto node = _node.lock();
// Add callback for dynamic parametrs
dyn_params_handler = node->add_on_set_parameters_callback(
std::bind(&SmacPlanner2D::dynamicParametersCallback, this, _1));
}

void SmacPlanner2D::deactivate()
Expand All @@ -186,6 +180,7 @@ void SmacPlanner2D::deactivate()
if (_costmap_downsampler) {
_costmap_downsampler->on_deactivate();
}
dyn_params_handler.reset();
}

void SmacPlanner2D::cleanup()
Expand Down Expand Up @@ -340,45 +335,45 @@ nav_msgs::msg::Path SmacPlanner2D::createPlan(
return plan;
}

void SmacPlanner2D::on_parameter_event_callback(
const rcl_interfaces::msg::ParameterEvent::SharedPtr event)
rcl_interfaces::msg::SetParametersResult
SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
{
rcl_interfaces::msg::SetParametersResult result;
std::lock_guard<std::mutex> lock_reinit(_mutex);

bool reinit_a_star = false;
bool reinit_downsampler = false;

for (auto & changed_parameter : event->changed_parameters) {
const auto & type = changed_parameter.value.type;
const auto & name = changed_parameter.name;
const auto & value = changed_parameter.value;
for (auto parameter : parameters) {
const auto & type = parameter.get_type();
const auto & name = parameter.get_name();

if (type == ParameterType::PARAMETER_DOUBLE) {
if (name == _name + ".tolerance") {
_tolerance = static_cast<float>(value.double_value);
_tolerance = static_cast<float>(parameter.as_double());
} else if (name == _name + ".cost_travel_multiplier") {
reinit_a_star = true;
_search_info.cost_penalty = value.double_value;
_search_info.cost_penalty = parameter.as_double();
} else if (name == _name + ".max_planning_time") {
_max_planning_time = value.double_value;
_max_planning_time = parameter.as_double();
}
} else if (type == ParameterType::PARAMETER_BOOL) {
if (name == _name + ".downsample_costmap") {
reinit_downsampler = true;
_downsample_costmap = value.bool_value;
_downsample_costmap = parameter.as_bool();
} else if (name == _name + ".allow_unknown") {
reinit_a_star = true;
_allow_unknown = value.bool_value;
_allow_unknown = parameter.as_bool();
} else if (name == _name + ".use_final_approach_orientation") {
_use_final_approach_orientation = value.bool_value;
_use_final_approach_orientation = parameter.as_bool();
}
} else if (type == ParameterType::PARAMETER_INTEGER) {
if (name == _name + ".downsampling_factor") {
reinit_downsampler = true;
_downsampling_factor = value.integer_value;
_downsampling_factor = parameter.as_int();
} else if (name == _name + ".max_iterations") {
reinit_a_star = true;
_max_iterations = value.integer_value;
_max_iterations = parameter.as_int();
if (_max_iterations <= 0) {
RCLCPP_INFO(
_logger, "maximum iteration selected as <= 0, "
Expand All @@ -387,7 +382,7 @@ void SmacPlanner2D::on_parameter_event_callback(
}
} else if (name == _name + ".max_on_approach_iterations") {
reinit_a_star = true;
_max_on_approach_iterations = value.integer_value;
_max_on_approach_iterations = parameter.as_int();
if (_max_on_approach_iterations <= 0) {
RCLCPP_INFO(
_logger, "On approach iteration selected as <= 0, "
Expand All @@ -398,7 +393,7 @@ void SmacPlanner2D::on_parameter_event_callback(
} else if (type == ParameterType::PARAMETER_STRING) {
if (name == _name + ".motion_model_for_search") {
reinit_a_star = true;
_motion_model = fromString(value.string_value);
_motion_model = fromString(parameter.as_string());
if (_motion_model == MotionModel::UNKNOWN) {
RCLCPP_WARN(
_logger,
Expand Down Expand Up @@ -434,6 +429,8 @@ void SmacPlanner2D::on_parameter_event_callback(
}
}
}
result.successful = true;
return result;
}

} // namespace nav2_smac_planner
Expand Down