diff --git a/device/nucleo_communicate/README.md b/device/nucleo_communicate/README.md index 728b4aa..0dd549b 100644 --- a/device/nucleo_communicate/README.md +++ b/device/nucleo_communicate/README.md @@ -8,7 +8,7 @@ Nucleoとの通信周り ## Executables -- `nucleo-chanel`: `Channel`Nodeをspin +- `nucleo-channel`: `Channel`Nodeをspin ## Launches diff --git a/device/nucleo_communicate/include/nucleo_communicate/channel.hpp b/device/nucleo_communicate/include/nucleo_communicate/channel.hpp index e98cc16..301ffd3 100644 --- a/device/nucleo_communicate/include/nucleo_communicate/channel.hpp +++ b/device/nucleo_communicate/include/nucleo_communicate/channel.hpp @@ -5,6 +5,7 @@ #include #include +#include #include #include #include @@ -21,17 +22,20 @@ class Channel : public rclcpp::Node { nucleo_com::SerialPort serial; std::mutex serial_mutex; - rclcpp::Subscription::SharedPtr quit_subscription; + rclcpp::Subscription::SharedPtr initialize_subscription; + rclcpp::Subscription::SharedPtr suspend_subscription; rclcpp::Subscription::SharedPtr power_subscription; - rclcpp::Publisher::SharedPtr flex1_publisher; - rclcpp::Publisher::SharedPtr flex2_publisher; - rclcpp::Publisher::SharedPtr current_publisher; - rclcpp::Publisher::SharedPtr voltage_publisher; + rclcpp::Publisher::SharedPtr state_publisher; + rclcpp::Publisher::SharedPtr flex1_publisher; + rclcpp::Publisher::SharedPtr flex2_publisher; + rclcpp::Publisher::SharedPtr current_publisher; + rclcpp::Publisher::SharedPtr voltage_publisher; rclcpp::TimerBase::SharedPtr timer; - void quit_subscription_callback(const std_msgs::msg::Empty& _msg); + void initialize_subscription_callback(const std_msgs::msg::Empty& _msg); + void suspend_subscription_callback(const std_msgs::msg::Empty& _msg); void power_subscription_callback(const packet_interfaces::msg::Power& msg); void timer_callback(); diff --git a/device/nucleo_communicate/include/nucleo_communicate/nucleo_state.hpp b/device/nucleo_communicate/include/nucleo_communicate/nucleo_state.hpp new file mode 100644 index 0000000..9cbc2bb --- /dev/null +++ b/device/nucleo_communicate/include/nucleo_communicate/nucleo_state.hpp @@ -0,0 +1,18 @@ +#ifndef NUCLEO_COMMUNICATE_NUCLEO_STATE_HPP +#define NUCLEO_COMMUNICATE_NUCLEO_STATE_HPP + +#include + +namespace nucleo_com { + +// https://github.com/rogy-AquaLab/2024_umiusi_nucleo/blob/4ccdc0d/include/umiusi/state.hpp + +enum class NucleoState: std::uint8_t { + INITIALIZING = 0, + SUSPEND = 1, + RUNNING = 2, +}; + +} + +#endif // NUCLEO_COMMUNICATE_NUCLEO_STATE_HPP diff --git a/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp b/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp index 9f3a839..c14fd08 100644 --- a/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp +++ b/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp @@ -5,6 +5,7 @@ #include #include +#include "nucleo_communicate/nucleo_state.hpp" #include "nucleo_communicate/recv_data.hpp" #include "nucleo_communicate/send_data.hpp" @@ -26,9 +27,14 @@ class SerialPort { // https://github.com/rogy-AquaLab/2024_umiusi_nucleo void send(const nucleo_com::SendData& data); + auto receive_state() -> nucleo_com::NucleoState; + auto receive() -> nucleo_com::RecvData; - void quit(); + /// nucleoに初期化命令を送る *SerialPortの初期化ではない + void initialize(); + + void suspend(); }; } diff --git a/device/nucleo_communicate/launch/channel_launch.py b/device/nucleo_communicate/launch/channel_launch.py index 64b0f54..29fb054 100644 --- a/device/nucleo_communicate/launch/channel_launch.py +++ b/device/nucleo_communicate/launch/channel_launch.py @@ -8,12 +8,14 @@ def generate_launch_description(): executable="nucleo-channel", namespace="device", remappings=[ + ("/device/nucleo_state", "/packet/nucleo_state"), ("/device/flex_1", "/packet/sensors/flex_1"), ("/device/flex_2", "/packet/sensors/flex_2"), ("/device/current", "/packet/sensors/current"), ("/device/voltage", "/packet/sensors/voltage"), ("/device/power", "/packet/order/power"), - ("/device/quit", "/packet/order/quit"), + ("/device/initialize", "/packet/order/initialize"), + ("/device/suspend", "/packet/order/suspend"), ], ) return LaunchDescription([channel]) diff --git a/device/nucleo_communicate/src/channel.cpp b/device/nucleo_communicate/src/channel.cpp index b1b0795..61d88eb 100644 --- a/device/nucleo_communicate/src/channel.cpp +++ b/device/nucleo_communicate/src/channel.cpp @@ -12,12 +12,20 @@ using packet_interfaces::msg::Current; using packet_interfaces::msg::Flex; +using packet_interfaces::msg::NucleoState; using packet_interfaces::msg::Power; using packet_interfaces::msg::Voltage; -void channel::Channel::quit_subscription_callback(const std_msgs::msg::Empty&) { - this->serial.quit(); - RCLCPP_INFO(this->get_logger(), "Sent quit order"); +void channel::Channel::initialize_subscription_callback(const std_msgs::msg::Empty&) { + const std::lock_guard _guard(this->serial_mutex); + this->serial.initialize(); + RCLCPP_INFO(this->get_logger(), "Sent initialize order"); +} + +void channel::Channel::suspend_subscription_callback(const std_msgs::msg::Empty&) { + const std::lock_guard _guard(this->serial_mutex); + this->serial.suspend(); + RCLCPP_INFO(this->get_logger(), "Sent suspend order"); } void channel::Channel::power_subscription_callback(const Power& msg) { @@ -32,8 +40,16 @@ void channel::Channel::timer_callback() { const std::lock_guard _guard(this->serial_mutex); RCLCPP_DEBUG(this->get_logger(), "tick"); - const rclcpp::Time now = this->get_clock()->now(); - const nucleo_com::RecvData data = this->serial.receive(); + const rclcpp::Time now = this->get_clock()->now(); + const nucleo_com::NucleoState state_data = this->serial.receive_state(); + const nucleo_com::RecvData data = this->serial.receive(); + { + NucleoState state_msg; + state_msg.header.frame_id = "nucleo_state"; + state_msg.header.stamp = now; + state_msg.state = static_cast(state_data); + this->state_publisher->publish(state_msg); + } { Flex flex1_msg; flex1_msg.header.frame_id = "nucleo_flex_1"; @@ -68,8 +84,10 @@ channel::Channel::Channel(const rclcpp::NodeOptions& options) : rclcpp::Node("channel", options), serial("/dev/ttyACM0"), serial_mutex(), - quit_subscription(nullptr), + initialize_subscription(nullptr), + suspend_subscription(nullptr), power_subscription(nullptr), + state_publisher(nullptr), flex1_publisher(nullptr), flex2_publisher(nullptr), current_publisher(nullptr), @@ -80,20 +98,28 @@ channel::Channel::Channel(const rclcpp::NodeOptions& options) : this->serial.setup(); - this->quit_subscription = this->create_subscription( - "quit", 10, std::bind(&channel::Channel::quit_subscription_callback, this, _1) + this->initialize_subscription = this->create_subscription( + "initialize", + 10, + std::bind(&channel::Channel::initialize_subscription_callback, this, _1) + ); + this->suspend_subscription = this->create_subscription( + "suspend", + 10, + std::bind(&channel::Channel::suspend_subscription_callback, this, _1) ); this->power_subscription = this->create_subscription( "power", 10, std::bind(&channel::Channel::power_subscription_callback, this, _1) ); + this->state_publisher = this->create_publisher("nucleo_state", 10); this->flex1_publisher = this->create_publisher("flex_1", 10); this->flex2_publisher = this->create_publisher("flex_2", 10); this->current_publisher = this->create_publisher("current", 10); this->voltage_publisher = this->create_publisher("voltage", 10); this->timer = this->create_wall_timer( - 40ms, std::bind(&channel::Channel::timer_callback, this) + 70ms, std::bind(&channel::Channel::timer_callback, this) ); } diff --git a/device/nucleo_communicate/src/serial_port.cpp b/device/nucleo_communicate/src/serial_port.cpp index f10507b..bdf4e41 100644 --- a/device/nucleo_communicate/src/serial_port.cpp +++ b/device/nucleo_communicate/src/serial_port.cpp @@ -43,6 +43,14 @@ void SerialPort::send(const SendData& data) { ); } +auto SerialPort::receive_state() -> NucleoState { + std::uint8_t header = 0x02; + fwrite(&header, sizeof(std::uint8_t), 1, this->serial); + std::uint8_t recv_data = 0; + fread(&recv_data, sizeof(std::uint8_t), 1, this->serial); + return static_cast(recv_data); +} + auto SerialPort::receive() -> RecvData { std::uint8_t header = 0x01; fwrite(&header, sizeof(std::uint8_t), 1, this->serial); @@ -56,7 +64,12 @@ auto SerialPort::receive() -> RecvData { return RecvData(std::move(buffer)); } -void SerialPort::quit() { +void SerialPort::initialize() { + std::uint8_t header = 0xFE; + fwrite(&header, sizeof(std::uint8_t), 1, this->serial); +} + +void SerialPort::suspend() { std::uint8_t header = 0xFF; fwrite(&header, sizeof(std::uint8_t), 1, this->serial); } diff --git a/packet/packet_interfaces/CMakeLists.txt b/packet/packet_interfaces/CMakeLists.txt index 7883992..8d72d65 100644 --- a/packet/packet_interfaces/CMakeLists.txt +++ b/packet/packet_interfaces/CMakeLists.txt @@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/Depth.msg msg/Flex.msg msg/LedColor.msg + msg/NucleoState.msg msg/Power.msg msg/Voltage.msg DEPENDENCIES std_msgs sensor_msgs diff --git a/packet/packet_interfaces/README.md b/packet/packet_interfaces/README.md index 31494ee..17a878e 100644 --- a/packet/packet_interfaces/README.md +++ b/packet/packet_interfaces/README.md @@ -15,4 +15,4 @@ device-app間で受け渡すメッセージ型 (see also) - sensor_msgs/msg/Image: カメライメージ (**これはComposedに含めない**) -- std_msgs/msg/Empty: quit命令の型 +- std_msgs/msg/Empty: nucleoのsuspend, initialize命令の型 diff --git a/packet/packet_interfaces/msg/NucleoState.msg b/packet/packet_interfaces/msg/NucleoState.msg new file mode 100644 index 0000000..1551654 --- /dev/null +++ b/packet/packet_interfaces/msg/NucleoState.msg @@ -0,0 +1,5 @@ +std_msgs/Header header + +# nucleoの状態 +# https://github.com/rogy-AquaLab/2024_umiusi_nucleo/tree/4ccdc0d?tab=readme-ov-file#nucleo-status-0x02 +byte state