From 717edc9aa303f7f8cc7c716c725def0a3d191811 Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 21 Aug 2024 11:08:07 +0900 Subject: [PATCH 1/7] =?UTF-8?q?=E5=9E=82=E7=9B=B4=E7=A7=BB=E5=8B=95?= =?UTF-8?q?=E3=81=A7=E3=81=99=E3=81=90=E3=81=AB=E3=81=AF=E5=8B=95=E3=81=8D?= =?UTF-8?q?=E5=87=BA=E3=81=95=E3=81=AA=E3=81=84=E3=82=88=E3=81=86=E3=81=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/simple_joy_app/app.hpp | 3 +++ app/simple_joy_app/src/app.cpp | 23 +++++++++++++++---- 2 files changed, 21 insertions(+), 5 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 1d574ed..811f3bd 100644 --- a/app/simple_joy_app/include/simple_joy_app/app.hpp +++ b/app/simple_joy_app/include/simple_joy_app/app.hpp @@ -1,6 +1,7 @@ #ifndef SIMPLE_JOY_APP_APP_HPP #define SIMPLE_JOY_APP_APP_HPP +#include #include #include @@ -27,6 +28,8 @@ class App : public rclcpp::Node { app::Status status; + std::optional vertical_move_start_at; + auto generate_header() -> std_msgs::msg::Header; // headerを修飾してpublish diff --git a/app/simple_joy_app/src/app.cpp b/app/simple_joy_app/src/app.cpp index 0a2a790..b5b5a1e 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/app/simple_joy_app/src/app.cpp @@ -8,6 +8,7 @@ #include "simple_joy_app/app.hpp" using power_map_msg::msg::NormalizedPower; +using namespace std::chrono_literals; auto app::App::generate_header() -> std_msgs::msg::Header { std_msgs::msg::Header header; @@ -52,13 +53,19 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) { NormalizedPower pub_msg; if (lstick_effective) { - pub_msg = this->para_move_power({ lstick_v, lstick_h }); + this->vertical_move_start_at = std::nullopt; + pub_msg = this->para_move_power({ lstick_v, lstick_h }); } else if (!rstick_effective) { - pub_msg = this->stop_power(); + this->vertical_move_start_at = std::nullopt; + pub_msg = this->stop_power(); } else if (rstick_h_effective) { - pub_msg = this->rotate_power(rstick_h); + this->vertical_move_start_at = std::nullopt; + pub_msg = this->rotate_power(rstick_h); } else { // assert(rstick_v_effective); 自明 + if (!this->vertical_move_start_at.has_value()) { + this->vertical_move_start_at = this->get_clock()->now(); + } pub_msg = this->vertical_move_power(rstick_v); } this->status = power_is_zero(pub_msg) ? Status::Stopped : Status::Moving; @@ -98,7 +105,12 @@ auto app::App::para_move_power(const std::pair& stick auto app::App::vertical_move_power(const double& vstick ) -> power_map_msg::msg::NormalizedPower { - const double mag = std::abs(vstick); + const auto duration_rclcpp = this->get_clock()->now() + - this->vertical_move_start_at.value(); + const auto duration = duration_rclcpp.to_chrono(); + const bool affect_bldc = duration > 500ms; + + const double mag = std::abs(vstick) * (affect_bldc ? 1.0 : 0.0); const double sign = std::signbit(vstick) ? -1 : 1; power_map_msg::msg::NormalizedPower msg{}; @@ -194,7 +206,8 @@ app::App::App(const rclcpp::NodeOptions& options) : led_left_publisher(), led_right_publisher(), healthcheck_timer(), - status(app::Status::NoInput) { + status(app::Status::NoInput), + vertical_move_start_at(std::nullopt) { using namespace std::chrono_literals; using std::placeholders::_1; auto joy_callback = std::bind(&app::App::joy_callback, this, _1); From 36d5825c5175498ea8e7083d1c18c00a407cf4a1 Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 21 Aug 2024 11:23:14 +0900 Subject: [PATCH 2/7] =?UTF-8?q?NucleoState=E3=82=92LED=E5=87=BA=E5=8A=9B?= =?UTF-8?q?=E3=81=AB=E5=8F=8D=E6=98=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/simple_joy_app/app.hpp | 10 ++++- .../include/simple_joy_app/status.hpp | 6 +++ app/simple_joy_app/src/app.cpp | 38 +++++++++++-------- 3 files changed, 37 insertions(+), 17 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 811f3bd..29203a2 100644 --- a/app/simple_joy_app/include/simple_joy_app/app.hpp +++ b/app/simple_joy_app/include/simple_joy_app/app.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include "simple_joy_app/status.hpp" @@ -17,7 +18,9 @@ namespace app { class App : public rclcpp::Node { private: - rclcpp::Subscription::SharedPtr subscription; + rclcpp::Subscription::SharedPtr joy_subscription; + rclcpp::Subscription::SharedPtr + nucleo_state_subscription; rclcpp::Publisher::SharedPtr power_publisher; @@ -26,7 +29,8 @@ class App : public rclcpp::Node { rclcpp::TimerBase::SharedPtr healthcheck_timer; - app::Status status; + app::Status status; + app::NucleoState nucleo_state; std::optional vertical_move_start_at; @@ -37,6 +41,8 @@ class App : public rclcpp::Node { void joy_callback(const sensor_msgs::msg::Joy& msg); + void nucleo_state_callback(const packet_interfaces::msg::NucleoState& msg); + auto para_move_power(const std::pair& stick ) -> power_map_msg::msg::NormalizedPower; diff --git a/app/simple_joy_app/include/simple_joy_app/status.hpp b/app/simple_joy_app/include/simple_joy_app/status.hpp index f0fd342..bc63135 100644 --- a/app/simple_joy_app/include/simple_joy_app/status.hpp +++ b/app/simple_joy_app/include/simple_joy_app/status.hpp @@ -22,6 +22,12 @@ std::string status_str(const Status& status) { : STATUS_NO_INPUT_STR; } +enum class NucleoState : std::uint8_t { + Initializing = 0, + Suspend = 1, + Running = 2, +}; + } #endif // SIMPLE_JOY_APP_STATUS_HPP diff --git a/app/simple_joy_app/src/app.cpp b/app/simple_joy_app/src/app.cpp index b5b5a1e..3a4c96f 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/app/simple_joy_app/src/app.cpp @@ -72,6 +72,11 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) { this->publish_power(pub_msg); } +void app::App::nucleo_state_callback(const packet_interfaces::msg::NucleoState& msg) { + RCLCPP_DEBUG_STREAM(this->get_logger(), "received nucleo_state msg"); + this->nucleo_state = static_cast(msg.state); +} + auto app::App::para_move_power(const std::pair& stick ) -> NormalizedPower { // TODO: provide explanation @@ -178,21 +183,17 @@ void app::App::healthcheck() { this->get_logger(), "simple_joy_app status: " << status_str(this->status) ); LedColor left_msg, right_msg; - left_msg.header = this->generate_header(); + left_msg.header = this->generate_header(); + switch (this->nucleo_state) { + case NucleoState::Suspend: color_red(left_msg); break; + case NucleoState::Initializing: color_blue(left_msg); break; + case NucleoState::Running: color_green(left_msg); break; + } right_msg.header = this->generate_header(); switch (this->status) { - case Status::NoInput: - color_blue(left_msg); - color_red(right_msg); - break; - case Status::Moving: - color_green(left_msg); - color_blue(right_msg); - break; - case Status::Stopped: - color_green(left_msg); - color_green(right_msg); - break; + case Status::NoInput: color_red(right_msg); break; + case Status::Stopped: color_blue(right_msg); break; + case Status::Moving: color_green(right_msg); break; } this->led_left_publisher->publish(left_msg); this->led_right_publisher->publish(right_msg); @@ -201,18 +202,25 @@ void app::App::healthcheck() { app::App::App(const rclcpp::NodeOptions& options) : rclcpp::Node("simple_joy_app", options), - subscription(), + joy_subscription(), + nucleo_state_subscription(), power_publisher(), led_left_publisher(), led_right_publisher(), healthcheck_timer(), status(app::Status::NoInput), + nucleo_state(app::NucleoState::Suspend), vertical_move_start_at(std::nullopt) { using namespace std::chrono_literals; using std::placeholders::_1; auto joy_callback = std::bind(&app::App::joy_callback, this, _1); - this->subscription + this->joy_subscription = this->create_subscription("joystick", 10, joy_callback); + auto ns_callback = std::bind(&app::App::nucleo_state_callback, this, _1); + this->nucleo_state_subscription + = this->create_subscription( + "nucleo_state", 10, ns_callback + ); this->power_publisher = this->create_publisher("normalized_power", 10); this->led_left_publisher From 89c8315bf47a2baf04b7692db76bd4202afc623b Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 21 Aug 2024 11:24:46 +0900 Subject: [PATCH 3/7] =?UTF-8?q?rotate=E3=81=AE=E3=83=91=E3=83=AF=E3=83=BC?= =?UTF-8?q?=E5=BC=B1=E3=82=81=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/simple_joy_app/src/app.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/app/simple_joy_app/src/app.cpp b/app/simple_joy_app/src/app.cpp index 3a4c96f..426758e 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/app/simple_joy_app/src/app.cpp @@ -127,7 +127,7 @@ auto app::App::vertical_move_power(const double& vstick } auto app::App::rotate_power(const double& hstick) -> power_map_msg::msg::NormalizedPower { - const double mag = std::abs(hstick); + const double mag = std::abs(hstick) * 0.7; const double sign = std::signbit(hstick) ? -1 : 1; NormalizedPower msg{}; From a479270f55d9e239ba11a7bbbe065d7a02fd3c11 Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 21 Aug 2024 11:25:51 +0900 Subject: [PATCH 4/7] Add remapping --- app/simple_joy_app/launch/app_launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/app/simple_joy_app/launch/app_launch.py b/app/simple_joy_app/launch/app_launch.py index 6622bb4..928d613 100644 --- a/app/simple_joy_app/launch/app_launch.py +++ b/app/simple_joy_app/launch/app_launch.py @@ -17,6 +17,7 @@ def generate_launch_description(): namespace="app", remappings=[ ("/app/joystick", "/packet/joystick"), + ("/app/nucleo_state", "/packet/nucleo_state"), ("/app/led_color_left", "/packet/order/led_color_left"), ("/app/led_color_right", "/packet/order/led_color_right"), ], From d8fa37d412a4efee15fd351668e701571c6dd68c Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 21 Aug 2024 11:41:42 +0900 Subject: [PATCH 5/7] =?UTF-8?q?=E3=83=9C=E3=82=BF=E3=83=B3=E3=81=A7order?= =?UTF-8?q?=E5=87=BA=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/simple_joy_app/app.hpp | 4 ++++ app/simple_joy_app/src/app.cpp | 16 ++++++++++++++++ 2 files changed, 20 insertions(+) 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 29203a2..83b9694 100644 --- a/app/simple_joy_app/include/simple_joy_app/app.hpp +++ b/app/simple_joy_app/include/simple_joy_app/app.hpp @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -27,6 +28,9 @@ class App : public rclcpp::Node { rclcpp::Publisher::SharedPtr led_left_publisher; rclcpp::Publisher::SharedPtr led_right_publisher; + rclcpp::Publisher::SharedPtr initialize_publisher; + rclcpp::Publisher::SharedPtr suspend_publisher; + rclcpp::TimerBase::SharedPtr healthcheck_timer; app::Status status; diff --git a/app/simple_joy_app/src/app.cpp b/app/simple_joy_app/src/app.cpp index 426758e..0851375 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/app/simple_joy_app/src/app.cpp @@ -44,6 +44,8 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) { const double rstick_h = msg.axes[2]; const double rstick_v = -msg.axes[3]; + const bool rbutton_left = msg.buttons[0]; + const bool rbutton_down = msg.buttons[1]; const bool lstick_h_effective = std::abs(lstick_h) > stick_threshold; const bool lstick_v_effective = std::abs(lstick_v) > stick_threshold; const bool lstick_effective = lstick_h_effective || lstick_v_effective; @@ -51,6 +53,14 @@ void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) { const bool rstick_v_effective = std::abs(rstick_v) > stick_threshold; const bool rstick_effective = rstick_h_effective || rstick_v_effective; + if (rbutton_left) { + std_msgs::msg::Empty pub_msg; + this->initialize_publisher->publish(pub_msg); + } else if (rbutton_down) { + std_msgs::msg::Empty pub_msg; + this->suspend_publisher->publish(pub_msg); + } + NormalizedPower pub_msg; if (lstick_effective) { this->vertical_move_start_at = std::nullopt; @@ -207,6 +217,8 @@ app::App::App(const rclcpp::NodeOptions& options) : power_publisher(), led_left_publisher(), led_right_publisher(), + initialize_publisher(), + suspend_publisher(), healthcheck_timer(), status(app::Status::NoInput), nucleo_state(app::NucleoState::Suspend), @@ -227,6 +239,10 @@ app::App::App(const rclcpp::NodeOptions& options) : = this->create_publisher("led_color_left", 10); this->led_right_publisher = this->create_publisher("led_color_right", 10); + this->initialize_publisher + = this->create_publisher("order/initialize", 10); + this->suspend_publisher + = this->create_publisher("order/suspend", 10); auto healthcheck_callback = std::bind(&app::App::healthcheck, this); this->healthcheck_timer = this->create_wall_timer(100ms, healthcheck_callback); } From c7631b455c52f00c672570793411be5d8d4fa358 Mon Sep 17 00:00:00 2001 From: H1rono Date: Wed, 21 Aug 2024 11:42:49 +0900 Subject: [PATCH 6/7] Fix launch --- app/simple_joy_app/launch/app_launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/app/simple_joy_app/launch/app_launch.py b/app/simple_joy_app/launch/app_launch.py index 928d613..ecea293 100644 --- a/app/simple_joy_app/launch/app_launch.py +++ b/app/simple_joy_app/launch/app_launch.py @@ -20,6 +20,8 @@ def generate_launch_description(): ("/app/nucleo_state", "/packet/nucleo_state"), ("/app/led_color_left", "/packet/order/led_color_left"), ("/app/led_color_right", "/packet/order/led_color_right"), + ("/app/order/initialize", "/packet/order/initialize"), + ("/app/order/suspend", "/packet/order/suspend"), ], ros_arguments=["--log-level", log_level], ) From f84fe042c80f26a7188b3224305a15f456b23c88 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sat, 24 Aug 2024 09:21:13 +0900 Subject: [PATCH 7/7] Add comment --- app/simple_joy_app/src/app.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/app/simple_joy_app/src/app.cpp b/app/simple_joy_app/src/app.cpp index 0851375..cc1aee4 100644 --- a/app/simple_joy_app/src/app.cpp +++ b/app/simple_joy_app/src/app.cpp @@ -123,6 +123,7 @@ auto app::App::vertical_move_power(const double& vstick const auto duration_rclcpp = this->get_clock()->now() - this->vertical_move_start_at.value(); const auto duration = duration_rclcpp.to_chrono(); + // TODO: 500msをパラメータにする const bool affect_bldc = duration > 500ms; const double mag = std::abs(vstick) * (affect_bldc ? 1.0 : 0.0);