Merge pull request #74 from rogy-AquaLab/appprogress

app/simple_joy_app/include/simple_joy_app/app.hpp

@@ -1,13 +1,16 @@
 #ifndef SIMPLE_JOY_APP_APP_HPP
 #define SIMPLE_JOY_APP_APP_HPP
 
+#include <optional>
 #include <utility>
 
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/joy.hpp>
+#include <std_msgs/msg/empty.hpp>
 #include <std_msgs/msg/header.hpp>
 
 #include <packet_interfaces/msg/led_color.hpp>
+#include <packet_interfaces/msg/nucleo_state.hpp>
 #include <power_map_msg/msg/normalized_power.hpp>
 
 #include "simple_joy_app/status.hpp"
@@ -16,16 +19,24 @@ namespace app {
 
 class App : public rclcpp::Node {
 private:
-    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription;
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscription;
+    rclcpp::Subscription<packet_interfaces::msg::NucleoState>::SharedPtr
+        nucleo_state_subscription;
 
     rclcpp::Publisher<power_map_msg::msg::NormalizedPower>::SharedPtr power_publisher;
 
     rclcpp::Publisher<packet_interfaces::msg::LedColor>::SharedPtr led_left_publisher;
     rclcpp::Publisher<packet_interfaces::msg::LedColor>::SharedPtr led_right_publisher;
 
+    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr initialize_publisher;
+    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr suspend_publisher;
+
     rclcpp::TimerBase::SharedPtr healthcheck_timer;
 
-    app::Status status;
+    app::Status      status;
+    app::NucleoState nucleo_state;
+
+    std::optional<rclcpp::Time> vertical_move_start_at;
 
     auto generate_header() -> std_msgs::msg::Header;
 
@@ -34,6 +45,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<double, double>& stick
     ) -> power_map_msg::msg::NormalizedPower;
 

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

app/simple_joy_app/launch/app_launch.py

@@ -17,8 +17,11 @@ 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"),
+            ("/app/order/initialize", "/packet/order/initialize"),
+            ("/app/order/suspend", "/packet/order/suspend"),
         ],
         ros_arguments=["--log-level", log_level],
     )

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;
@@ -43,28 +44,49 @@ 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;
     const bool rstick_h_effective = std::abs(rstick_h) > stick_threshold;
     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) {
-        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;
     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<NucleoState>(msg.state);
+}
+
 auto app::App::para_move_power(const std::pair<double, double>& stick
 ) -> NormalizedPower {
     // TODO: provide explanation
@@ -98,7 +120,13 @@ auto app::App::para_move_power(const std::pair<double, double>& 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<std::chrono::milliseconds>();
+    // TODO: 500msをパラメータにする
+    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{};
@@ -110,7 +138,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{};
@@ -166,21 +194,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);
@@ -189,23 +213,37 @@ 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(),
+    initialize_publisher(),
+    suspend_publisher(),
     healthcheck_timer(),
-    status(app::Status::NoInput) {
+    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<sensor_msgs::msg::Joy>("joystick", 10, joy_callback);
+    auto ns_callback = std::bind(&app::App::nucleo_state_callback, this, _1);
+    this->nucleo_state_subscription
+        = this->create_subscription<packet_interfaces::msg::NucleoState>(
+            "nucleo_state", 10, ns_callback
+        );
     this->power_publisher
         = this->create_publisher<NormalizedPower>("normalized_power", 10);
     this->led_left_publisher
         = this->create_publisher<packet_interfaces::msg::LedColor>("led_color_left", 10);
     this->led_right_publisher
         = this->create_publisher<packet_interfaces::msg::LedColor>("led_color_right", 10);
+    this->initialize_publisher
+        = this->create_publisher<std_msgs::msg::Empty>("order/initialize", 10);
+    this->suspend_publisher
+        = this->create_publisher<std_msgs::msg::Empty>("order/suspend", 10);
     auto healthcheck_callback = std::bind(&app::App::healthcheck, this);
     this->healthcheck_timer   = this->create_wall_timer(100ms, healthcheck_callback);
 }