From 541f7049f3ecaf1bacc1a596be926b90b60e3d27 Mon Sep 17 00:00:00 2001 From: H1rono Date: Tue, 11 Jun 2024 17:13:21 +0900 Subject: [PATCH 1/6] Add header --- app/power_map_msg/msg/NormalizedPower.msg | 2 ++ 1 file changed, 2 insertions(+) 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 From 90b15fed6895d8a53c194c85b9040e673c78d4f7 Mon Sep 17 00:00:00 2001 From: H1rono Date: Tue, 11 Jun 2024 17:27:30 +0900 Subject: [PATCH 2/6] Add power_map_msg in simple_joy_app --- app/simple_joy_app/CMakeLists.txt | 2 ++ app/simple_joy_app/package.xml | 1 + 2 files changed, 3 insertions(+) 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/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 From 945ce7bf1d3794271b7f5cca6f0279cd40c9f105 Mon Sep 17 00:00:00 2001 From: H1rono Date: Tue, 11 Jun 2024 17:28:50 +0900 Subject: [PATCH 3/6] Publish `NormalizedPower` from simple_joy_app --- .../include/simple_joy_app/app.hpp | 13 ++-- app/simple_joy_app/src/app.cpp | 78 +++++++------------ 2 files changed, 36 insertions(+), 55 deletions(-) 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..9b134c8 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,14 @@ 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 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/src/app.cpp b/app/simple_joy_app/src/app.cpp index c102740..dcbf413 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,12 +28,12 @@ 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; } @@ -44,71 +46,50 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) { } 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{}; - - 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 - ); - - // 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; + NormalizedPower msg{}; - constexpr uint16_t servo_center = (servo_min + servo_max) / 2; + msg.bldc[0] = bldc1; + msg.bldc[1] = bldc2; + msg.bldc[2] = bldc2; + msg.bldc[3] = bldc1; - 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; } -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::stop_power() -> NormalizedPower { + NormalizedPower msg{}; - packet_interfaces::msg::Power msg{}; + msg.bldc[0] = 0.0; + msg.bldc[1] = 0.0; + msg.bldc[2] = 0.0; + msg.bldc[3] = 0.0; - msg.bldc[0] = bldc_center; - msg.bldc[1] = bldc_center; - msg.bldc[2] = bldc_center; - msg.bldc[3] = bldc_center; - - 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; } @@ -122,9 +103,8 @@ app::App::App(const rclcpp::NodeOptions& options) : this->subscription = this->create_subscription( "/device/joystick", 10, callback ); - this->power_publisher = this->create_publisher( - "/device/order/power", 10 - ); + this->power_publisher + = this->create_publisher("normalized_power", 10); } RCLCPP_COMPONENTS_REGISTER_NODE(app::App) From 33e4f0b1f4f4ce8cb67c32ee1aef44f8a37bbec6 Mon Sep 17 00:00:00 2001 From: H1rono Date: Tue, 11 Jun 2024 17:30:00 +0900 Subject: [PATCH 4/6] Remap input --- app/simple_joy_app/launch/app_launch.py | 3 ++- app/simple_joy_app/src/app.cpp | 7 +++---- 2 files changed, 5 insertions(+), 5 deletions(-) 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/src/app.cpp b/app/simple_joy_app/src/app.cpp index dcbf413..599cce5 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/app/simple_joy_app/src/app.cpp @@ -99,10 +99,9 @@ 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 - ); + 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); } From f4b74820641411aa51fc7ae8dd6bf042d7fb78a3 Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 12 Jun 2024 14:30:04 +0900 Subject: [PATCH 5/6] :sparkles: Impl rotate_move --- .../include/simple_joy_app/app.hpp | 2 ++ app/simple_joy_app/src/app.cpp | 21 ++++++++++++++++++- 2 files changed, 22 insertions(+), 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 index 9b134c8..0f6e40f 100644 --- a/app/simple_joy_app/include/simple_joy_app/app.hpp +++ b/app/simple_joy_app/include/simple_joy_app/app.hpp @@ -21,6 +21,8 @@ class App : public rclcpp::Node { auto para_move_power(const std::pair& stick ) -> power_map_msg::msg::NormalizedPower; + auto rotate_power(const double& hstick) -> 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 index 599cce5..82dad3c 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/app/simple_joy_app/src/app.cpp @@ -38,7 +38,7 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) { return; } if (rstick_h_effective) { - // TODO: 水平回転 + NormalizedPower msg = this->rotate_power(rstick_h); return; } // assert(rstick_v_effective); 自明 @@ -78,6 +78,25 @@ auto app::App::para_move_power(const std::pair& stick 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; + + 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; +} + auto app::App::stop_power() -> NormalizedPower { NormalizedPower msg{}; From 0e0ab77118a009ba9d6030d0fd046127c3ae932d Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 12 Jun 2024 14:38:28 +0900 Subject: [PATCH 6/6] :sparkles: vertical_move_power --- .../include/simple_joy_app/app.hpp | 2 ++ app/simple_joy_app/src/app.cpp | 23 ++++++++++++++++++- 2 files changed, 24 insertions(+), 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 index 0f6e40f..6d9f068 100644 --- 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 index 82dad3c..edb0701 100644 --- 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& stick @@ -78,6 +79,26 @@ auto app::App::para_move_power(const std::pair& 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;