From 905469bb02f2f6ea60866363bf5642c1ae1a9c91 Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 12 Jun 2024 18:38:50 +0900 Subject: [PATCH 1/5] :sparkles: Add placement in power_map --- app/power_map/CMakeLists.txt | 1 + app/power_map/include/power_map/placement.hpp | 20 +++++++++++++++++++ app/power_map/src/placement.cpp | 17 ++++++++++++++++ 3 files changed, 38 insertions(+) create mode 100644 app/power_map/include/power_map/placement.hpp create mode 100644 app/power_map/src/placement.cpp diff --git a/app/power_map/CMakeLists.txt b/app/power_map/CMakeLists.txt index ddd9d90..174f914 100644 --- a/app/power_map/CMakeLists.txt +++ b/app/power_map/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(power_map_msg REQUIRED) add_library(power_map SHARED src/power_map.cpp + src/placement.cpp ) ament_target_dependencies(power_map diff --git a/app/power_map/include/power_map/placement.hpp b/app/power_map/include/power_map/placement.hpp new file mode 100644 index 0000000..898d379 --- /dev/null +++ b/app/power_map/include/power_map/placement.hpp @@ -0,0 +1,20 @@ +#ifndef POWER_MAP_PLACEMENT_HPP +#define POWER_MAP_PLACEMENT_HPP + +#include +#include + +namespace power_map { + +enum class Placement : char { + LeftFront, + RightFront, + LeftBack, + RightBack, +}; + +auto placement_from_str(const std::string& value) -> std::optional; + +} + +#endif // POWER_MAP_PLACEMENT_HPP diff --git a/app/power_map/src/placement.cpp b/app/power_map/src/placement.cpp new file mode 100644 index 0000000..18215be --- /dev/null +++ b/app/power_map/src/placement.cpp @@ -0,0 +1,17 @@ +#include "power_map/placement.hpp" + +auto power_map::placement_from_str(const std::string& value) -> std::optional { + if (value == "left-front") { + return Placement::LeftFront; + } + if (value == "right-front") { + return Placement::RightFront; + } + if (value == "left-back") { + return Placement::LeftBack; + } + if (value == "right-back") { + return Placement::RightBack; + } + return std::nullopt; +} From 6b4a75a9ec9f7b1181d37fa6aedcbbdc732d20cb Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 12 Jun 2024 18:39:35 +0900 Subject: [PATCH 2/5] :sparkles: Add parameters --- app/power_map/include/power_map/power_map.hpp | 14 ++++- app/power_map/src/power_map.cpp | 51 +++++++++++++++++++ 2 files changed, 64 insertions(+), 1 deletion(-) diff --git a/app/power_map/include/power_map/power_map.hpp b/app/power_map/include/power_map/power_map.hpp index 8322bb0..10cbd48 100644 --- a/app/power_map/include/power_map/power_map.hpp +++ b/app/power_map/include/power_map/power_map.hpp @@ -2,8 +2,9 @@ #define POWER_MAP_HPP #include - #include +#include + #include #include #include @@ -14,6 +15,7 @@ #include "power_map_msg/msg/normalized_power.hpp" #include "power_map/container.hpp" +#include "power_map/placement.hpp" namespace power_map { @@ -27,6 +29,12 @@ class PowerMap : public rclcpp::Node { std::array configs; std::array cb_handles; + std::array bldc_placement_config; + std::array servo_placement_config; + + std::shared_ptr bldc_placement_cb_handle; + std::shared_ptr servo_placement_cb_handle; + rclcpp::Publisher::SharedPtr publisher; rclcpp::Subscription::SharedPtr subscription; @@ -46,6 +54,10 @@ class PowerMap : public rclcpp::Node { auto create_servo_max_cb(size_t i ) -> rclcpp::ParameterCallbackHandle::ParameterCallbackType; + auto bldc_placement_param_cb(const rclcpp::Parameter& param) -> void; + + auto servo_placement_param_cb(const rclcpp::Parameter& param) -> void; + auto subscription_callback(const power_map_msg::msg::NormalizedPower& msg) -> void; public: diff --git a/app/power_map/src/power_map.cpp b/app/power_map/src/power_map.cpp index 7027927..bf9ff9d 100644 --- a/app/power_map/src/power_map.cpp +++ b/app/power_map/src/power_map.cpp @@ -86,6 +86,24 @@ auto power_map::PowerMap::subscription_callback( this->publisher->publish(pub_msg); } +auto power_map::PowerMap::bldc_placement_param_cb(const rclcpp::Parameter& param +) -> void { + RCLCPP_INFO(this->get_logger(), "received parameter update of bldc_placement"); + const auto& placement_str = param.as_string_array(); + for (size_t i = 0; i < 4; ++i) { + this->bldc_placement_config[i] = placement_from_str(placement_str[i]).value(); + } +} + +auto power_map::PowerMap::servo_placement_param_cb(const rclcpp::Parameter& param +) -> void { + RCLCPP_INFO(this->get_logger(), "received parameter update of servo_placement"); + const auto& placement_str = param.as_string_array(); + for (size_t i = 0; i < 4; ++i) { + this->servo_placement_config[i] = placement_from_str(placement_str[i]).value(); + } +} + power_map::PowerMap::PowerMap(const rclcpp::NodeOptions& options) : rclcpp::Node("power_map", options) { constexpr int DEFAULT_BLDC_CENTER = 1480; @@ -147,6 +165,39 @@ power_map::PowerMap::PowerMap(const rclcpp::NodeOptions& options) : this->cb_handles[i].servo_max(std::move(cb)); } } + // this->add_on_set_parameters_callback() + { + // bldc placement + const std::string parameter_name = "bldc_placement"; + this->declare_parameter( + parameter_name, + std::vector{ + "left-front", "right-front", "left-back", "right-back" } + ); + this->bldc_placement_param_cb(this->get_parameter(parameter_name)); + this->bldc_placement_cb_handle = this->param_cb->add_parameter_callback( + parameter_name, + [this](const rclcpp::Parameter& param) { + this->bldc_placement_param_cb(param); + } + ); + } + { + // servo placement + const std::string parameter_name = "servo_placement"; + this->declare_parameter( + parameter_name, + std::vector{ + "left-front", "right-front", "left-back", "right-back" } + ); + this->servo_placement_param_cb(this->get_parameter(parameter_name)); + this->servo_placement_cb_handle = this->param_cb->add_parameter_callback( + parameter_name, + [this](const rclcpp::Parameter& param) { + this->servo_placement_param_cb(param); + } + ); + } this->publisher = this->create_publisher("power", 10); this->subscription = this->create_subscription( From d01d727b1f0a96a6345378175e168a2f72477462 Mon Sep 17 00:00:00 2001 From: H1rono Date: Tue, 25 Jun 2024 17:18:45 +0900 Subject: [PATCH 3/5] Add `on_set_parameters_cb` --- app/power_map/include/power_map/power_map.hpp | 6 ++ app/power_map/src/power_map.cpp | 65 ++++++++++++++++++- 2 files changed, 70 insertions(+), 1 deletion(-) diff --git a/app/power_map/include/power_map/power_map.hpp b/app/power_map/include/power_map/power_map.hpp index 10cbd48..ea7bb99 100644 --- a/app/power_map/include/power_map/power_map.hpp +++ b/app/power_map/include/power_map/power_map.hpp @@ -35,6 +35,9 @@ class PowerMap : public rclcpp::Node { std::shared_ptr bldc_placement_cb_handle; std::shared_ptr servo_placement_cb_handle; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr + on_set_parameters_cb_handle; + rclcpp::Publisher::SharedPtr publisher; rclcpp::Subscription::SharedPtr subscription; @@ -58,6 +61,9 @@ class PowerMap : public rclcpp::Node { auto servo_placement_param_cb(const rclcpp::Parameter& param) -> void; + auto on_set_parameters_cb(const std::vector& params + ) -> rcl_interfaces::msg::SetParametersResult; + auto subscription_callback(const power_map_msg::msg::NormalizedPower& msg) -> void; public: diff --git a/app/power_map/src/power_map.cpp b/app/power_map/src/power_map.cpp index bf9ff9d..48ca0f3 100644 --- a/app/power_map/src/power_map.cpp +++ b/app/power_map/src/power_map.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include @@ -104,6 +105,64 @@ auto power_map::PowerMap::servo_placement_param_cb(const rclcpp::Parameter& para } } +auto power_map::PowerMap::on_set_parameters_cb( + const std::vector& params +) -> rcl_interfaces::msg::SetParametersResult { + using rclcpp::ParameterType; + +#define RETURN_RESULT(SUCCESSFUL, REASON) \ + do { \ + rcl_interfaces::msg::SetParametersResult result; \ + result.successful = SUCCESSFUL; \ + result.reason = REASON; \ + return result; \ + } while (false) + + for (const rclcpp::Parameter& param : params) { + const std::string& name = param.get_name(); + const ParameterType& type = param.get_type(); + if (name.find("bldc_center") == 0 || name.find("bldc_positive_radius") == 0 + || name.find("bldc_negative_radius") == 0 || name.find("servo_min") == 0 + || name.find("servo_max") == 0) + { + if (type != ParameterType::PARAMETER_INTEGER) { + RETURN_RESULT(false, "invalid type; expected integer"); + } + } else if (name == "servo_placement" || name == "bldc_placement") { + if (type != ParameterType::PARAMETER_STRING_ARRAY) { + RETURN_RESULT(false, "invalid type; expected array of string"); + } + const std::vector& value = param.as_string_array(); + if (value.size() != 4) { + RETURN_RESULT(false, "invalid value; the length of array must be 4"); + } + std::unordered_set seen_placements; + for (const std::string& v : value) { + const std::optional parsed = placement_from_str(v); + if (!parsed.has_value()) { + RETURN_RESULT( + false, + "invalid value; each element of array must be one of left-front, " + "left-back, right-front, right-back" + ); + } + const power_map::Placement placement = parsed.value(); + if (seen_placements.count(placement) > 0) { + RETURN_RESULT(false, "invalid value; each element must be unique"); + } + seen_placements.insert(placement); + } + } else { + RCLCPP_DEBUG_STREAM( + this->get_logger(), "received unexpected parameter " << name << ", skip" + ); + } + } + RETURN_RESULT(true, ""); + +#undef RETURN_RESULT +} + power_map::PowerMap::PowerMap(const rclcpp::NodeOptions& options) : rclcpp::Node("power_map", options) { constexpr int DEFAULT_BLDC_CENTER = 1480; @@ -165,7 +224,6 @@ power_map::PowerMap::PowerMap(const rclcpp::NodeOptions& options) : this->cb_handles[i].servo_max(std::move(cb)); } } - // this->add_on_set_parameters_callback() { // bldc placement const std::string parameter_name = "bldc_placement"; @@ -198,6 +256,11 @@ power_map::PowerMap::PowerMap(const rclcpp::NodeOptions& options) : } ); } + this->on_set_parameters_cb_handle = this->add_on_set_parameters_callback( + [this](const std::vector& params) { + return this->on_set_parameters_cb(params); + } + ); this->publisher = this->create_publisher("power", 10); this->subscription = this->create_subscription( From fa3802809d6fb3c5306899bbbae64dd05b65714b Mon Sep 17 00:00:00 2001 From: H1rono Date: Sat, 29 Jun 2024 20:38:30 +0900 Subject: [PATCH 4/5] Apply placement configuration --- app/power_map/src/power_map.cpp | 27 +++++++++++++++++++-------- 1 file changed, 19 insertions(+), 8 deletions(-) diff --git a/app/power_map/src/power_map.cpp b/app/power_map/src/power_map.cpp index 48ca0f3..eae7dc2 100644 --- a/app/power_map/src/power_map.cpp +++ b/app/power_map/src/power_map.cpp @@ -69,19 +69,30 @@ auto power_map::PowerMap::subscription_callback( const power_map_msg::msg::NormalizedPower& msg ) -> void { RCLCPP_DEBUG(this->get_logger(), "received normalized power"); + + std::array bldc_placement_map{}, servo_placement_map{}; + for (size_t i = 0; i < 4; ++i) { + bldc_placement_map[static_cast(this->bldc_placement_config[i])] = i; + servo_placement_map[static_cast(this->servo_placement_config[i])] = i; + } + packet_interfaces::msg::Power pub_msg{}; for (size_t i = 0; i < 4; ++i) { + const size_t bldc_index = bldc_placement_map[i]; + const size_t servo_index = servo_placement_map[i]; // FIXME: formatが - const auto& config = this->configs[i]; - const float bldc_radius = msg.bldc[i] < 0 ? config.bldc_negative_radius() - : config.bldc_positive_radius(); - pub_msg.bldc[i] = static_cast( - config.bldc_center() + static_cast(msg.bldc[i] * bldc_radius) + const auto& bldc_config = this->configs[bldc_index]; + const auto& servo_config = this->configs[servo_index]; + const float bldc_radius = msg.bldc[i] < 0 ? bldc_config.bldc_negative_radius() + : bldc_config.bldc_positive_radius(); + pub_msg.bldc[bldc_index] = static_cast( + bldc_config.bldc_center() + static_cast(msg.bldc[i] * bldc_radius) ); const float servo_range - = static_cast(config.servo_max() - config.servo_min()); - pub_msg.servo[i] = static_cast( - config.servo_min() + static_cast((msg.servo[i] + 1) / 2.0 * servo_range) + = static_cast(servo_config.servo_max() - servo_config.servo_min()); + pub_msg.servo[servo_index] = static_cast( + servo_config.servo_min() + + static_cast((msg.servo[i] + 1) / 2.0 * servo_range) ); } this->publisher->publish(pub_msg); From cb258f4a07b89df58f56535535b63ddd86d51a03 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sat, 29 Jun 2024 20:40:29 +0900 Subject: [PATCH 5/5] Modify default parameter file --- app/power_map/config/default_param.yml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/app/power_map/config/default_param.yml b/app/power_map/config/default_param.yml index 1a46e1f..08d6dc9 100644 --- a/app/power_map/config/default_param.yml +++ b/app/power_map/config/default_param.yml @@ -24,3 +24,6 @@ bldc_positive_radius4: 250 servo_max4: 2400 servo_min4: 500 + # placement + bldc_placement: [ "left-front", "right-front", "left-back", "right-back" ] + servo_placement: [ "left-front", "right-front", "left-back", "right-back" ]