✨ vertical_move_power

rogy-AquaLab · Jun 12, 2024 · 0e0ab77 · 0e0ab77
1 parent f4b7482
commit 0e0ab77
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 1 deletion.
diff --git a/app/simple_joy_app/include/simple_joy_app/app.hpp b/app/simple_joy_app/include/simple_joy_app/app.hpp
@@ -23,6 +23,8 @@ class App : public rclcpp::Node {
 
  auto rotate_power(const double& hstick) -> power_map_msg::msg::NormalizedPower;
 
+ auto vertical_move_power(const double& vstick) -> power_map_msg::msg::NormalizedPower;
+
  auto stop_power() -> power_map_msg::msg::NormalizedPower;
 
 public:

diff --git a/app/simple_joy_app/src/app.cpp b/app/simple_joy_app/src/app.cpp
@@ -39,10 +39,11 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) {
  }
  if (rstick_h_effective) {
  NormalizedPower msg = this->rotate_power(rstick_h);
+ this->power_publisher->publish(msg);
  return;
  }
  // assert(rstick_v_effective); 自明
- // TODO: 垂直方向の移動
+ this->power_publisher->publish(this->vertical_move_power(rstick_v));
 }
 
 auto app::App::para_move_power(const std::pair<double, double>& stick
@@ -78,6 +79,26 @@ auto app::App::para_move_power(const std::pair<double, double>& stick
  return msg;
 }
 
+auto app::App::vertical_move_power(const double& vstick
+) -> power_map_msg::msg::NormalizedPower {
+ const double mag = std::abs(vstick);
+ const double sign = std::signbit(vstick) ? -1 : 1;
+
+ power_map_msg::msg::NormalizedPower msg{};
+ // FIXME: tekito- ni kimeta
+ msg.bldc[0] = mag;
+ msg.bldc[1] = mag;
+ msg.bldc[2] = mag;
+ msg.bldc[3] = mag;
+
+ msg.servo[0] = sign;
+ msg.servo[1] = sign;
+ msg.servo[2] = sign;
+ msg.servo[3] = sign;
+
+ return msg;
+}
+
 auto app::App::rotate_power(const double& hstick) -> power_map_msg::msg::NormalizedPower {
  const double mag = std::abs(hstick);
  const double sign = std::signbit(hstick) ? -1 : 1;