Skip to content

Commit b8db839

Browse files
Tomas BacaTomas Baca
Tomas Baca
authored and
Tomas Baca
committed
refactored controller's dynamic params
1 parent 14053e1 commit b8db839

File tree

2 files changed

+23
-343
lines changed

2 files changed

+23
-343
lines changed

‎src/mpc_controller.cpp‎

Lines changed: 22 additions & 237 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <mrs_lib/subscriber_handler.h>
2020
#include <mrs_lib/timer_handler.h>
2121
#include <mrs_lib/utils.h>
22+
#include <mrs_lib/dynparam_mgr.h>
2223

2324
#include <sensor_msgs/msg/imu.hpp>
2425

@@ -113,6 +114,8 @@ class MpcController : public mrs_uav_managers::Controller {
113114

114115
// | --------------- dynamic reconfigure server --------------- |
115116

117+
std::shared_ptr<mrs_lib::DynparamMgr> dynparam_mgr_;
118+
116119
struct DrsParams_t
117120
{
118121
double kiwxy;
@@ -131,14 +134,6 @@ class MpcController : public mrs_uav_managers::Controller {
131134
DrsParams_t drs_params_;
132135
std::mutex mutex_drs_params_;
133136

134-
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
135-
136-
rcl_interfaces::msg::SetParametersResult callbackParameters(std::vector<rclcpp::Parameter> parameters);
137-
138-
void setParamsToServer(const DrsParams_t &drs_params);
139-
140-
std::atomic<bool> params_setting_running_ = false;
141-
142137
// | ----------------------- controllers ---------------------- |
143138

144139
void positionPassthrough(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs::msg::TrackerCommand &tracker_command);
@@ -311,6 +306,8 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
311306
return false;
312307
}
313308

309+
dynparam_mgr_ = std::make_shared<mrs_lib::DynparamMgr>(node_, mutex_drs_params_);
310+
314311
// | -------------------- loading my params ------------------- |
315312

316313
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/private/mpc_controller.yaml");
@@ -368,7 +365,9 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
368365

369366
// mass estimator
370367
private_handlers->param_loader->loadParam("so3/mass_estimator/km", gains_.km);
371-
private_handlers->param_loader->loadParam("so3/mass_estimator/fuse_acceleration", drs_params_.fuse_acceleration);
368+
369+
dynparam_mgr_->register_param("se3/mass_estimator/fuse_acceleration", &drs_params_.fuse_acceleration, drs_params_.fuse_acceleration);
370+
372371
private_handlers->param_loader->loadParam("so3/mass_estimator/km_lim", gains_.km_lim);
373372

374373
// constraints
@@ -392,9 +391,11 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
392391

393392
// angular rate feed forward
394393
private_handlers->param_loader->loadParam("so3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
394+
dynparam_mgr_->register_param("so3/angular_rate_feedforward/jerk", &drs_params_.jerk_feedforward, drs_params_.jerk_feedforward);
395395

396396
// output mode
397397
private_handlers->param_loader->loadParam("so3/preferred_output", drs_params_.preferred_output_mode);
398+
dynparam_mgr_->register_param("so3/preferred_output", &drs_params_.preferred_output_mode, drs_params_.preferred_output_mode, mrs_lib::DynparamMgr::range_t<int>(0, 3));
398399

399400
// | ------------------- position pid params ------------------ |
400401

@@ -446,127 +447,29 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
446447

447448
// | --------------- declare dynamic parameters --------------- |
448449

449-
{
450-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
451-
452-
rcl_interfaces::msg::FloatingPointRange range;
453-
454-
range.from_value = 0.0;
455-
range.to_value = 10.0;
456-
457-
param_desc.floating_point_range = {range};
450+
dynparam_mgr_->register_param("horizontal.kiwxy", &drs_params_.kiwxy, gains_.kiwxy, mrs_lib::DynparamMgr::range_t<double>(0.0, 10.0));
451+
dynparam_mgr_->register_param("horizontal.kibxy", &drs_params_.kibxy, gains_.kibxy, mrs_lib::DynparamMgr::range_t<double>(0.0, 10.0));
452+
dynparam_mgr_->register_param("horizontal.kiwxy_lim", &drs_params_.kiwxy_lim, gains_.kiwxy_lim, mrs_lib::DynparamMgr::range_t<double>(0.0, 10.0));
453+
dynparam_mgr_->register_param("horizontal.kibxy_lim", &drs_params_.kibxy_lim, gains_.kibxy_lim, mrs_lib::DynparamMgr::range_t<double>(0.0, 10.0));
458454

459-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
460-
461-
node_->declare_parameter(node_->get_sub_namespace() + "/horizontal.kiwxy", 0.0, param_desc);
462-
node_->declare_parameter(node_->get_sub_namespace() + "/horizontal.kibxy", 0.0, param_desc);
463-
node_->declare_parameter(node_->get_sub_namespace() + "/horizontal.kiwxy_lim", 0.0, param_desc);
464-
node_->declare_parameter(node_->get_sub_namespace() + "/horizontal.kibxy_lim", 0.0, param_desc);
465-
}
466-
467-
{
468-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
455+
dynparam_mgr_->register_param("attitude.kq_roll_pitch", &drs_params_.kq_roll_pitch, gains_.kq_roll_pitch, mrs_lib::DynparamMgr::range_t<double>(0.0, 20.0));
469456

470-
rcl_interfaces::msg::FloatingPointRange range;
471-
472-
range.from_value = 0.0;
473-
range.to_value = 20.0;
474-
475-
param_desc.floating_point_range = {range};
476-
477-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
478-
479-
node_->declare_parameter(node_->get_sub_namespace() + "/attitude.kq_roll_pitch", 0.0, param_desc);
480-
}
481-
482-
{
483-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
484-
485-
rcl_interfaces::msg::FloatingPointRange range;
486-
487-
range.from_value = 0.0;
488-
range.to_value = 40.0;
489-
490-
param_desc.floating_point_range = {range};
491-
492-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
493-
494-
node_->declare_parameter(node_->get_sub_namespace() + "/attitude.kq_yaw", 0.0, param_desc);
495-
}
496-
497-
{
498-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
457+
dynparam_mgr_->register_param("attitude.kq_yaw", &drs_params_.kq_yaw, gains_.kq_yaw, mrs_lib::DynparamMgr::range_t<double>(0.0, 40.0));
499458

500-
rcl_interfaces::msg::FloatingPointRange range;
459+
dynparam_mgr_->register_param("mass.km", &drs_params_.km, gains_.km, mrs_lib::DynparamMgr::range_t<double>(0.0, 2.0));
501460

502-
range.from_value = 0.0;
503-
range.to_value = 2.0;
504-
505-
param_desc.floating_point_range = {range};
506-
507-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
508-
509-
node_->declare_parameter(node_->get_sub_namespace() + "/mass.km", 0.0, param_desc);
510-
}
511-
512-
{
513-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
514-
515-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
516-
517-
node_->declare_parameter(node_->get_sub_namespace() + "/mass.fuse_acceleration", false, param_desc);
518-
}
519-
520-
{
521-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
522-
523-
rcl_interfaces::msg::FloatingPointRange range;
524-
525-
range.from_value = 0.0;
526-
range.to_value = 50.0;
527-
528-
param_desc.floating_point_range = {range};
529-
530-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
531-
532-
node_->declare_parameter(node_->get_sub_namespace() + "/mass.km_lim", 0.0, param_desc);
533-
}
534-
535-
{
536-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
537-
538-
rcl_interfaces::msg::IntegerRange range;
539-
540-
range.from_value = 0;
541-
range.to_value = 3;
542-
543-
param_desc.integer_range = {range};
544-
545-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER;
546-
547-
node_->declare_parameter(node_->get_sub_namespace() + "/preferred_output_mode", 0, param_desc);
548-
}
549-
550-
{
551-
auto param_desc = rcl_interfaces::msg::ParameterDescriptor{};
552-
553-
param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL;
554-
555-
node_->declare_parameter(node_->get_sub_namespace() + "/jerk_feedforward", false, param_desc);
556-
}
461+
dynparam_mgr_->register_param("mass.km_lim", &drs_params_.km_lim, gains_.km_lim, mrs_lib::DynparamMgr::range_t<double>(0.0, 50.0));
557462

558463
drs_params_.kiwxy = gains_.kiwxy;
559464
drs_params_.kibxy = gains_.kibxy;
465+
drs_params_.kiwxy_lim = gains_.kiwxy_lim;
466+
drs_params_.kibxy_lim = gains_.kibxy_lim;
560467
drs_params_.kq_roll_pitch = gains_.kq_roll_pitch;
561468
drs_params_.kq_yaw = gains_.kq_yaw;
562469
drs_params_.km = gains_.km;
563470
drs_params_.km_lim = gains_.km_lim;
564-
drs_params_.kiwxy_lim = gains_.kiwxy_lim;
565-
drs_params_.kibxy_lim = gains_.kibxy_lim;
566471

567-
setParamsToServer(drs_params_);
568-
569-
param_callback_handle_ = node_->add_on_set_parameters_callback(std::bind(&MpcController::callbackParameters, this, std::placeholders::_1));
472+
dynparam_mgr_->update_to_ros();
570473

571474
// | --------------------- service servers -------------------- |
572475

@@ -2121,90 +2024,6 @@ void MpcController::PIDVelocityOutput(const mrs_msgs::msg::UavState &uav_state,
21212024
// | callbacks |
21222025
// --------------------------------------------------------------
21232026

2124-
/* callbackParameters() //{ */
2125-
2126-
rcl_interfaces::msg::SetParametersResult MpcController::callbackParameters(std::vector<rclcpp::Parameter> parameters) {
2127-
2128-
rcl_interfaces::msg::SetParametersResult result;
2129-
2130-
if (params_setting_running_) {
2131-
2132-
result.successful = true;
2133-
result.reason = "not seting, params update triggered from the inside";
2134-
2135-
return result;
2136-
}
2137-
2138-
auto drs_params = mrs_lib::get_mutexed(mutex_drs_params_, drs_params_);
2139-
2140-
// Note that setting a parameter to a nonsensical value (such as setting the `param_namespace.floating_number` parameter to `hello`)
2141-
// doesn't have any effect - it doesn't even call this callback.
2142-
for (auto &param : parameters) {
2143-
2144-
RCLCPP_DEBUG_STREAM(node_->get_logger(), "got parameter: '" << param.get_name() << "' with value '" << param.value_to_string() << "'");
2145-
2146-
if (param.get_name() == node_->get_sub_namespace() + "/horizontal.kiwxy") {
2147-
2148-
drs_params.kiwxy = param.as_double();
2149-
2150-
} else if (param.get_name() == node_->get_sub_namespace() + "/horizontal.kibxy") {
2151-
2152-
drs_params.kibxy = param.as_double();
2153-
2154-
} else if (param.get_name() == node_->get_sub_namespace() + "/horizontal.kiwxy_lim") {
2155-
2156-
drs_params.kiwxy_lim = param.as_double();
2157-
2158-
} else if (param.get_name() == node_->get_sub_namespace() + "/horizontal.kibxy_lim") {
2159-
2160-
drs_params.kibxy_lim = param.as_double();
2161-
2162-
} else if (param.get_name() == node_->get_sub_namespace() + "/attitude.kq_roll_pitch") {
2163-
2164-
drs_params.kq_roll_pitch = param.as_double();
2165-
2166-
} else if (param.get_name() == node_->get_sub_namespace() + "/attitude.kq_yaw") {
2167-
2168-
drs_params.kq_yaw = param.as_double();
2169-
2170-
} else if (param.get_name() == node_->get_sub_namespace() + "/mass.km") {
2171-
2172-
drs_params.km = param.as_double();
2173-
2174-
} else if (param.get_name() == node_->get_sub_namespace() + "/mass.fuse_acceleration") {
2175-
2176-
drs_params.fuse_acceleration = param.as_bool();
2177-
2178-
} else if (param.get_name() == node_->get_sub_namespace() + "/mass.km_lim") {
2179-
2180-
drs_params.km_lim = param.as_double();
2181-
2182-
} else if (param.get_name() == node_->get_sub_namespace() + "/preferred_output_mode") {
2183-
2184-
drs_params.preferred_output_mode = param.as_int();
2185-
2186-
} else if (param.get_name() == node_->get_sub_namespace() + "/jerk_feedforward") {
2187-
2188-
drs_params.jerk_feedforward = param.as_bool();
2189-
2190-
} else {
2191-
2192-
RCLCPP_DEBUG_STREAM(node_->get_logger(), "parameter: '" << param.get_name() << "' is not dynamically reconfigurable!");
2193-
}
2194-
}
2195-
2196-
RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 1000, "[MpcController]: params updated");
2197-
2198-
result.successful = true;
2199-
result.reason = "OK";
2200-
2201-
mrs_lib::set_mutexed(mutex_drs_params_, drs_params, drs_params_);
2202-
2203-
return result;
2204-
}
2205-
2206-
//}
2207-
22082027
/* //{ callbackSetIntegralTerms() */
22092028

22102029
bool MpcController::callbackSetIntegralTerms(const std::shared_ptr<std_srvs::srv::SetBool::Request> &request, const std::shared_ptr<std_srvs::srv::SetBool::Response> &response) {
@@ -2269,19 +2088,7 @@ void MpcController::timerGains() {
22692088
// set the gains back to dynamic reconfigure
22702089
// and only do it when some filtering occurs
22712090
if (updated) {
2272-
2273-
drs_params.kiwxy = gains.kiwxy;
2274-
drs_params.kibxy = gains.kibxy;
2275-
drs_params.kq_roll_pitch = gains.kq_roll_pitch;
2276-
drs_params.kq_yaw = gains.kq_yaw;
2277-
drs_params.km = gains.km;
2278-
drs_params.km_lim = gains.km_lim;
2279-
drs_params.kiwxy_lim = gains.kiwxy_lim;
2280-
drs_params.kibxy_lim = gains.kibxy_lim;
2281-
2282-
setParamsToServer(drs_params);
2283-
2284-
RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 10000, "[%s]: gains have been updated", name_.c_str());
2091+
RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 1000, "[%s]: filtering gains after a dynamic parameter update", name_.c_str());
22852092
}
22862093
}
22872094

@@ -2363,28 +2170,6 @@ double MpcController::getHeadingSafely(const mrs_msgs::msg::UavState &uav_state,
23632170

23642171
//}
23652172

2366-
/* setParamsToServer() //{ */
2367-
2368-
void MpcController::setParamsToServer(const DrsParams_t &drs_params) {
2369-
2370-
mrs_lib::AtomicScopeFlag unset_running(params_setting_running_);
2371-
2372-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/mass.fuse_acceleration", drs_params.fuse_acceleration));
2373-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/jerk_feedforward", drs_params.jerk_feedforward));
2374-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/preferred_output_mode", drs_params.preferred_output_mode));
2375-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/horizontal.kiwxy", drs_params.kiwxy));
2376-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/horizontal.kibxy", drs_params.kibxy));
2377-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/horizontal.kiwxy_lim", drs_params.kiwxy_lim));
2378-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/horizontal.kibxy_lim", drs_params.kibxy_lim));
2379-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/attitude.kq_roll_pitch", drs_params.kq_roll_pitch));
2380-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/attitude.kq_yaw", drs_params.kq_yaw));
2381-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/mass.km", drs_params.km));
2382-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/mass.km_lim", drs_params.km_lim));
2383-
node_->set_parameter(rclcpp::Parameter(node_->get_sub_namespace() + "/mass.fuse_acceleration", drs_params.fuse_acceleration));
2384-
}
2385-
2386-
//}
2387-
23882173
//}
23892174

23902175
} // namespace mpc_controller

0 commit comments

Comments
 (0)