Merge pull request #34 from rogy-AquaLab/placement

Placement

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

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" ]

app/power_map/include/power_map/placement.hpp

@@ -0,0 +1,20 @@
+#ifndef POWER_MAP_PLACEMENT_HPP
+#define POWER_MAP_PLACEMENT_HPP
+
+#include <optional>
+#include <string>
+
+namespace power_map {
+
+enum class Placement : char {
+ LeftFront,
+ RightFront,
+ LeftBack,
+ RightBack,
+};
+
+auto placement_from_str(const std::string& value) -> std::optional<Placement>;
+
+}
+
+#endif // POWER_MAP_PLACEMENT_HPP

app/power_map/include/power_map/power_map.hpp

@@ -2,8 +2,9 @@
 #define POWER_MAP_HPP
 
 #include <array>
-
 #include <memory>
+#include <utility>
+
 #include <rclcpp/node.hpp>
 #include <rclcpp/node_options.hpp>
 #include <rclcpp/parameter_event_handler.hpp>
@@ -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,15 @@ class PowerMap : public rclcpp::Node {
  std::array<Config, 4> configs;
  std::array<CbHandles, 4> cb_handles;
 
+ std::array<Placement, 4> bldc_placement_config;
+ std::array<Placement, 4> servo_placement_config;
+
+ std::shared_ptr<rclcpp::ParameterCallbackHandle> bldc_placement_cb_handle;
+ std::shared_ptr<rclcpp::ParameterCallbackHandle> servo_placement_cb_handle;
+
+ rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
+ on_set_parameters_cb_handle;
+
  rclcpp::Publisher<packet_interfaces::msg::Power>::SharedPtr publisher;
 
  rclcpp::Subscription<power_map_msg::msg::NormalizedPower>::SharedPtr subscription;
@@ -46,6 +57,13 @@ 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 on_set_parameters_cb(const std::vector<rclcpp::Parameter>& params
+ ) -> rcl_interfaces::msg::SetParametersResult;
+
  auto subscription_callback(const power_map_msg::msg::NormalizedPower& msg) -> void;
 
 public:

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<Placement> {
+ 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;
+}

app/power_map/src/power_map.cpp

@@ -2,6 +2,7 @@
 
 #include <cstdint>
 #include <string>
+#include <unordered_set>
 
 #include <rclcpp/parameter.hpp>
 #include <rclcpp/parameter_event_handler.hpp>
@@ -68,24 +69,111 @@ auto power_map::PowerMap::subscription_callback(
  const power_map_msg::msg::NormalizedPower& msg
 ) -> void {
  RCLCPP_DEBUG(this->get_logger(), "received normalized power");
+
+ std::array<size_t, 4> bldc_placement_map{}, servo_placement_map{};
+ for (size_t i = 0; i < 4; ++i) {
+ bldc_placement_map[static_cast<size_t>(this->bldc_placement_config[i])] = i;
+ servo_placement_map[static_cast<size_t>(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<std::uint16_t>(
- config.bldc_center() + static_cast<int>(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<std::uint16_t>(
+ bldc_config.bldc_center() + static_cast<int>(msg.bldc[i] * bldc_radius)
  );
  const float servo_range
- = static_cast<float>(config.servo_max() - config.servo_min());
- pub_msg.servo[i] = static_cast<std::uint16_t>(
- config.servo_min() + static_cast<int>((msg.servo[i] + 1) / 2.0 * servo_range)
+ = static_cast<float>(servo_config.servo_max() - servo_config.servo_min());
+ pub_msg.servo[servo_index] = static_cast<std::uint16_t>(
+ servo_config.servo_min()
+ + static_cast<int>((msg.servo[i] + 1) / 2.0 * servo_range)
  );
  }
  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();
+ }
+}
+
+auto power_map::PowerMap::on_set_parameters_cb(
+ const std::vector<rclcpp::Parameter>& 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<std::string>& value = param.as_string_array();
+ if (value.size() != 4) {
+ RETURN_RESULT(false, "invalid value; the length of array must be 4");
+ }
+ std::unordered_set<power_map::Placement> seen_placements;
+ for (const std::string& v : value) {
+ const std::optional<power_map::Placement> 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;
@@ -147,6 +235,43 @@ power_map::PowerMap::PowerMap(const rclcpp::NodeOptions& options) :
  this->cb_handles[i].servo_max(std::move(cb));
  }
  }
+ {
+ // bldc placement
+ const std::string parameter_name = "bldc_placement";
+ this->declare_parameter(
+ parameter_name,
+ std::vector<std::string>{
+ "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<std::string>{
+ "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->on_set_parameters_cb_handle = this->add_on_set_parameters_callback(
+ [this](const std::vector<rclcpp::Parameter>& params) {
+ return this->on_set_parameters_cb(params);
+ }
+ );
 
  this->publisher = this->create_publisher<packet_interfaces::msg::Power>("power", 10);
  this->subscription = this->create_subscription<power_map_msg::msg::NormalizedPower>(