diff --git a/app/power_map_msg/msg/NormalizedPower.msg b/app/power_map_msg/msg/NormalizedPower.msg index 0d1598c..9346b16 100644 --- a/app/power_map_msg/msg/NormalizedPower.msg +++ b/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 diff --git a/app/simple_joy_app/CMakeLists.txt b/app/simple_joy_app/CMakeLists.txt index c980264..94d5e6f 100644 --- a/app/simple_joy_app/CMakeLists.txt +++ b/app/simple_joy_app/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(ament_cmake REQUIRED) # find_package( 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 ) diff --git a/app/simple_joy_app/include/simple_joy_app/app.hpp b/app/simple_joy_app/include/simple_joy_app/app.hpp index 26c5895..6d9f068 100644 --- a/app/simple_joy_app/include/simple_joy_app/app.hpp +++ b/app/simple_joy_app/include/simple_joy_app/app.hpp @@ -3,9 +3,10 @@ #include -#include "packet_interfaces/msg/power.hpp" -#include "sensor_msgs/msg/joy.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include + +#include namespace app { @@ -13,14 +14,18 @@ class App : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr subscription; - rclcpp::Publisher::SharedPtr power_publisher; + rclcpp::Publisher::SharedPtr power_publisher; void joy_callback(const sensor_msgs::msg::Joy& msg); auto para_move_power(const std::pair& 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()); diff --git a/app/simple_joy_app/launch/app_launch.py b/app/simple_joy_app/launch/app_launch.py index 08fd462..900495a 100644 --- a/app/simple_joy_app/launch/app_launch.py +++ b/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")] ) ]) diff --git a/app/simple_joy_app/package.xml b/app/simple_joy_app/package.xml index 2a81a70..fd0ca82 100644 --- a/app/simple_joy_app/package.xml +++ b/app/simple_joy_app/package.xml @@ -10,6 +10,7 @@ ament_cmake rclcpp sensor_msgs + power_map_msg rclcpp_components packet_interfaces diff --git a/app/simple_joy_app/src/app.cpp b/app/simple_joy_app/src/app.cpp index c102740..edb0701 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/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& 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( - static_cast(bldc_center) + static_cast(bldc_radius) * bldc1 - ); - const uint16_t bldc2_msg = static_cast( - static_cast(bldc_center) + static_cast(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( - "/device/joystick", 10, callback - ); - this->power_publisher = this->create_publisher( - "/device/order/power", 10 - ); + auto callback = std::bind(&app::App::joy_callback, this, _1); + this->subscription + = this->create_subscription("joystick", 10, callback); + this->power_publisher + = this->create_publisher("normalized_power", 10); } RCLCPP_COMPONENTS_REGISTER_NODE(app::App)