From f3edf0307c147033aa84dcdaecc8666c28ca18df Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 11 Aug 2024 17:04:00 +0900 Subject: [PATCH 1/7] Add NucleoState.msg --- packet/packet_interfaces/CMakeLists.txt | 1 + packet/packet_interfaces/msg/NucleoState.msg | 5 +++++ 2 files changed, 6 insertions(+) create mode 100644 packet/packet_interfaces/msg/NucleoState.msg 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/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 From f2096cfd2739eed9d0f211e2a8536276bf5886b4 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 11 Aug 2024 17:41:26 +0900 Subject: [PATCH 2/7] Add `SerialPort::receive_state` --- .../nucleo_communicate/nucleo_state.hpp | 18 ++++++++++++++++++ .../include/nucleo_communicate/serial_port.hpp | 3 +++ device/nucleo_communicate/src/serial_port.cpp | 8 ++++++++ 3 files changed, 29 insertions(+) create mode 100644 device/nucleo_communicate/include/nucleo_communicate/nucleo_state.hpp 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..19a35fb 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,6 +27,8 @@ 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(); diff --git a/device/nucleo_communicate/src/serial_port.cpp b/device/nucleo_communicate/src/serial_port.cpp index f10507b..be097a6 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); From 665784c81d50b6edb87added74b4761eda955394 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 11 Aug 2024 17:47:01 +0900 Subject: [PATCH 3/7] Change `SerialPort` apis about nucleo state --- .../include/nucleo_communicate/serial_port.hpp | 5 ++++- device/nucleo_communicate/src/serial_port.cpp | 7 ++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp b/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp index 19a35fb..c14fd08 100644 --- a/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp +++ b/device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp @@ -31,7 +31,10 @@ class SerialPort { auto receive() -> nucleo_com::RecvData; - void quit(); + /// nucleoに初期化命令を送る *SerialPortの初期化ではない + void initialize(); + + void suspend(); }; } diff --git a/device/nucleo_communicate/src/serial_port.cpp b/device/nucleo_communicate/src/serial_port.cpp index be097a6..bdf4e41 100644 --- a/device/nucleo_communicate/src/serial_port.cpp +++ b/device/nucleo_communicate/src/serial_port.cpp @@ -64,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); } From 30d135cd16c6b36b9366c2e7dd92539a25860be4 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 11 Aug 2024 17:53:42 +0900 Subject: [PATCH 4/7] Update nucleo_communicate Channel node --- .../include/nucleo_communicate/channel.hpp | 16 ++++--- device/nucleo_communicate/src/channel.cpp | 44 +++++++++++++++---- 2 files changed, 45 insertions(+), 15 deletions(-) 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/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) ); } From c36454cdc7d2f463d3a3a1dabb948e1459375704 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 11 Aug 2024 17:56:46 +0900 Subject: [PATCH 5/7] Fix documents --- device/nucleo_communicate/README.md | 2 +- packet/packet_interfaces/README.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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/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命令の型 From d91333ceb86922d4cb7e777e72e7fb0048d1af50 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 11 Aug 2024 17:59:34 +0900 Subject: [PATCH 6/7] Update nucleo_communicate channel_launch.py --- device/nucleo_communicate/launch/channel_launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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]) From f2a55abda23120e187cbb1e52324cb8360991bc1 Mon Sep 17 00:00:00 2001 From: H1rono Date: Sun, 11 Aug 2024 19:45:15 +0900 Subject: [PATCH 7/7] Add launch file to order nucleo --- ul/launch/order_nucleo.py | 56 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 ul/launch/order_nucleo.py diff --git a/ul/launch/order_nucleo.py b/ul/launch/order_nucleo.py new file mode 100644 index 0000000..0ab8526 --- /dev/null +++ b/ul/launch/order_nucleo.py @@ -0,0 +1,56 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression + + +def generate_launch_description() -> LaunchDescription: + state_arg = DeclareLaunchArgument( + "state", default_value="running", choices=["initializing", "suspend", "running"] + ) + power_topic_arg = DeclareLaunchArgument("power_topic", default_value="{}") + # launch_ros.actions.Nodeを使っていないためlog_level引数はない + state = LaunchConfiguration("state") + power_topic = LaunchConfiguration("power_topic") + state_is_initializing = PythonExpression(["'", state, "' == 'initializing'"]) + state_is_suspend = PythonExpression(["'", state, "' == 'suspend'"]) + state_is_running = PythonExpression(["'", state, "' == 'running'"]) + order_power = ExecuteProcess( + cmd=[ + "ros2", + "topic", + "pub", + "--once", + "/packet/order/power", + "packet_interfaces/msg/Power", + power_topic, + ], + condition=IfCondition(state_is_running), + ) + order_initializing = ExecuteProcess( + cmd=[ + "ros2", + "topic", + "pub", + "--once", + "/packet/order/initialize", + "std_msgs/msg/Empty", + "{}", + ], + condition=IfCondition(state_is_initializing), + ) + order_suspend = ExecuteProcess( + cmd=[ + "ros2", + "topic", + "pub", + "--once", + "/packet/order/suspend", + "std_msgs/msg/Empty", + "{}", + ], + condition=IfCondition(state_is_suspend), + ) + return LaunchDescription( + [state_arg, power_topic_arg, order_power, order_initializing, order_suspend] + )