Merge pull request #21 from rogy-AquaLab/joy-map

simple_joy_appでpower_mapを使うように

app/power_map_msg/msg/NormalizedPower.msg

@@ -1,5 +1,7 @@
 # 正規化されたBLDC, サーボの値
 
+std_msgs/Header header
+
 # BLDC4つ -1 ~ 1
 float32[4] bldc
 # サーボ4つ -pi ~ pi

app/simple_joy_app/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED)
 # find_package(<dependency> REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(power_map_msg REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(packet_interfaces REQUIRED)
 
@@ -23,6 +24,7 @@ add_library(simple_joy_app
 ament_target_dependencies(simple_joy_app
  rclcpp
  sensor_msgs
+ power_map_msg
  rclcpp_components
  packet_interfaces
 )

app/simple_joy_app/include/simple_joy_app/app.hpp

@@ -3,24 +3,29 @@
 
 #include <utility>
 
-#include "packet_interfaces/msg/power.hpp"
-#include "sensor_msgs/msg/joy.hpp"
-#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+
+#include <power_map_msg/msg/normalized_power.hpp>
 
 namespace app {
 
 class App : public rclcpp::Node {
 private:
  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription;
 
- rclcpp::Publisher<packet_interfaces::msg::Power>::SharedPtr power_publisher;
+ rclcpp::Publisher<power_map_msg::msg::NormalizedPower>::SharedPtr power_publisher;
 
  void joy_callback(const sensor_msgs::msg::Joy& msg);
 
  auto para_move_power(const std::pair<double, double>& stick
- ) -> packet_interfaces::msg::Power;
+ ) -> power_map_msg::msg::NormalizedPower;
+
+ 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() -> packet_interfaces::msg::Power;
+ auto stop_power() -> power_map_msg::msg::NormalizedPower;
 
 public:
  App(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

app/simple_joy_app/launch/app_launch.py

@@ -7,6 +7,7 @@ def generate_launch_description():
  Node(
  package="simple_joy_app",
  executable="app",
- namespace="app"
+ namespace="app",
+ remappings=[("/app/joystick", "/device/joystick")]
  )
  ])

app/simple_joy_app/package.xml

@@ -10,6 +10,7 @@
  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>sensor_msgs</depend>
+ <depend>power_map_msg</depend>
  <depend>rclcpp_components</depend>
  <depend>packet_interfaces</depend>
 

app/simple_joy_app/src/app.cpp

@@ -5,6 +5,8 @@
 
 #include "simple_joy_app/app.hpp"
 
+using power_map_msg::msg::NormalizedPower;
+
 void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) {
  RCLCPP_INFO_STREAM(this->get_logger(), "received joystick msg");
  // https://www.pygame.org/docs/ref/joystick.html#playstation-4-controller-pygame-2-x
@@ -26,89 +28,108 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) {
  const bool rstick_effective = rstick_h_effective || rstick_v_effective;
 
  if (lstick_effective) {
- packet_interfaces::msg::Power msg = this->para_move_power({ lstick_v, lstick_h });
+ NormalizedPower msg = this->para_move_power({ lstick_v, lstick_h });
  this->power_publisher->publish(msg);
  return;
  }
  if (!rstick_effective) {
- packet_interfaces::msg::Power msg = this->stop_power();
+ NormalizedPower msg = this->stop_power();
  this->power_publisher->publish(msg);
  return;
  }
  if (rstick_h_effective) {
- // TODO: 水平回転
+ 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
-) -> packet_interfaces::msg::Power {
+) -> NormalizedPower {
  // FIXME: C++20...?
  constexpr double pi = 3.141592653589793;
- // FIXME: use parameter
- constexpr uint16_t bldc_center = 1480;
- constexpr uint16_t bldc_radius = 250; // TODO: check this
- constexpr uint16_t servo_min = 500;
- constexpr uint16_t servo_max = 2400;
  // TODO: provide explanation
  constexpr double theta = pi / 4;
  // Left-Up, Right-Down
- double bldc1 = stick.first * cos(theta) + stick.second * sin(theta);
+ float bldc1 = stick.first * cos(theta) + stick.second * sin(theta);
  // Right-Up, Left-Down
- double bldc2 = stick.first * cos(theta) - stick.second * sin(theta);
+ float bldc2 = stick.first * cos(theta) - stick.second * sin(theta);
+
  const double bldc_abc_max = std::max(std::abs(bldc1), std::abs(bldc2));
- // constrain
  if (bldc_abc_max > 1) {
  const double scale = 1 / bldc_abc_max;
  bldc1 *= scale;
  bldc2 *= scale;
  }
 
- packet_interfaces::msg::Power msg{};
+ NormalizedPower msg{};
+
+ msg.bldc[0] = bldc1;
+ msg.bldc[1] = bldc2;
+ msg.bldc[2] = bldc2;
+ msg.bldc[3] = bldc1;
 
- const uint16_t bldc1_msg = static_cast<uint16_t>(
- static_cast<double>(bldc_center) + static_cast<double>(bldc_radius) * bldc1
- );
- const uint16_t bldc2_msg = static_cast<uint16_t>(
- static_cast<double>(bldc_center) + static_cast<double>(bldc_radius) * bldc2
- );
+ msg.servo[0] = 0.0;
+ msg.servo[1] = 0.0;
+ msg.servo[2] = 0.0;
+ msg.servo[3] = 0.0;
+
+ return msg;
+}
 
- // TODO: Fix order with param
- msg.bldc[0] = bldc1_msg;
- msg.bldc[1] = bldc2_msg;
- msg.bldc[2] = bldc2_msg;
- msg.bldc[3] = bldc1_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;
 
- constexpr uint16_t servo_center = (servo_min + servo_max) / 2;
+ 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] = servo_center;
- msg.servo[1] = servo_center;
- msg.servo[2] = servo_center;
- msg.servo[3] = servo_center;
+ msg.servo[0] = sign;
+ msg.servo[1] = sign;
+ msg.servo[2] = sign;
+ msg.servo[3] = sign;
 
  return msg;
 }
 
-auto app::App::stop_power() -> packet_interfaces::msg::Power {
- // FIXME: use parameter
- constexpr uint16_t bldc_center = 1480;
- constexpr uint16_t servo_min = 500;
- constexpr uint16_t servo_max = 2400;
- constexpr uint16_t servo_center = (servo_min + servo_max) / 2;
+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;
+
+ NormalizedPower msg{};
+
+ msg.bldc[0] = sign * mag;
+ msg.bldc[1] = -sign * mag;
+ msg.bldc[2] = sign * mag;
+ msg.bldc[3] = -sign * mag;
+
+ msg.servo[0] = 0.0;
+ msg.servo[1] = 0.0;
+ msg.servo[2] = 0.0;
+ msg.servo[3] = 0.0;
+
+ return msg;
+}
 
- packet_interfaces::msg::Power msg{};
+auto app::App::stop_power() -> NormalizedPower {
+ NormalizedPower msg{};
 
- msg.bldc[0] = bldc_center;
- msg.bldc[1] = bldc_center;
- msg.bldc[2] = bldc_center;
- msg.bldc[3] = bldc_center;
+ msg.bldc[0] = 0.0;
+ msg.bldc[1] = 0.0;
+ msg.bldc[2] = 0.0;
+ msg.bldc[3] = 0.0;
 
- msg.servo[0] = servo_center;
- msg.servo[1] = servo_center;
- msg.servo[2] = servo_center;
- msg.servo[3] = servo_center;
+ msg.servo[0] = 0.0;
+ msg.servo[1] = 0.0;
+ msg.servo[2] = 0.0;
+ msg.servo[3] = 0.0;
 
  return msg;
 }
@@ -118,13 +139,11 @@ app::App::App(const rclcpp::NodeOptions& options) :
  subscription(),
  power_publisher() {
  using std::placeholders::_1;
- auto callback = std::bind(&app::App::joy_callback, this, _1);
- this->subscription = this->create_subscription<sensor_msgs::msg::Joy>(
- "/device/joystick", 10, callback
- );
- this->power_publisher = this->create_publisher<packet_interfaces::msg::Power>(
- "/device/order/power", 10
- );
+ auto callback = std::bind(&app::App::joy_callback, this, _1);
+ this->subscription
+ = this->create_subscription<sensor_msgs::msg::Joy>("joystick", 10, callback);
+ this->power_publisher
+ = this->create_publisher<NormalizedPower>("normalized_power", 10);
 }
 
 RCLCPP_COMPONENTS_REGISTER_NODE(app::App)