Merge branch 'main' into compressed_image

device/camera_reader/camera_reader/camera.py

@@ -11,7 +11,12 @@ def __init__(self):
         super().__init__("camera")
         self.publisher_ = self.create_publisher(CompressedImage, "camera_image", 10)
         self.timer = self.create_timer(0.1, self.timer_callback)
-        self.cap = cv2.VideoCapture(0)
+        param = self.declare_parameter("camera_id", 0)
+        camera_id = (
+            self.get_parameter_or(param.name, param).get_parameter_value().integer_value
+        )
+        assert isinstance(camera_id, int)
+        self.cap = cv2.VideoCapture(camera_id)
         self.bridge = CvBridge()
         if not self.cap.isOpened():
             self.get_logger().error("Failed to open camera")

device/camera_reader/config/default_param.yml

@@ -0,0 +1,3 @@
+/*:
+  ros__parameters:
+    camera_id: 0

device/camera_reader/launch/camera_reader_launch.py

@@ -1,14 +1,23 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory("camera_reader"),
+        "config",
+        "default_param.yml",
+    )
     return LaunchDescription(
         [
             Node(
                 package="camera_reader",
                 executable="camera",
                 namespace="device",
+                parameters=[config],
                 remappings=[("/device/camera_image", "/packet/camera_image")],
             ),
         ],

device/camera_reader/setup.py

@@ -16,6 +16,10 @@
             os.path.join("share", package_name, "launch"),
             glob(os.path.join("launch", "*")),
         ),
+        (
+            os.path.join("share", package_name, "config"),
+            glob(os.path.join("config", "*")),
+        ),
     ],
     install_requires=["setuptools"],
     zip_safe=True,

device/joystick/joystick/joystick.py

@@ -10,7 +10,7 @@
 class Joystick(Node):
     def __init__(self):
         super().__init__("joystick")
-        self._timer = self.create_timer(0.5, self._timer_callback)
+        self._timer = self.create_timer(0.01, self._timer_callback)
         self._joy_publisher = self.create_publisher(Joy, "joystick", 10)
         # TODO: ここparameterで変えられるようにしたい
         self._joystick = pygame.joystick.Joystick(0)

device/nucleo_communicate/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 3.8)
+project(nucleo_communicate)
+
+set(CMAKE_CXX_STANDARD 20)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(packet_interfaces REQUIRED)
+
+add_library(channel
+    SHARED
+    src/recv_data.cpp
+    src/send_data.cpp
+    src/serial_port.cpp
+    src/channel.cpp
+)
+
+ament_target_dependencies(channel
+    rclcpp
+    rclcpp_components
+    std_msgs
+    packet_interfaces
+)
+
+target_include_directories(channel
+    PRIVATE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+)
+
+rclcpp_components_register_node(
+    channel
+    PLUGIN "channel::Channel"
+    EXECUTABLE nucleo-channel
+)
+
+install(
+    TARGETS channel
+    EXPORT channel_component
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+)
+
+install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

device/nucleo_communicate/README.md

@@ -0,0 +1,15 @@
+# nucleo_communicate
+
+Nucleoとの通信周り
+
+## Nodes
+
+- `Channel`: データのやり取り(TX/RX両方)
+
+## Executables
+
+- `nucleo-chanel`: `Channel`Nodeをspin
+
+## Launches
+
+- `channel_launch.py`: `Channel`Nodeを`device`namespaceでspin

device/nucleo_communicate/include/nucleo_communicate/channel.hpp

@@ -0,0 +1,45 @@
+#ifndef NUCLEO_COMMUNICATE_CHANNEL_HPP
+#define NUCLEO_COMMUNICATE_CHANNEL_HPP
+
+#include <mutex>
+
+#include <packet_interfaces/msg/current.hpp>
+#include <packet_interfaces/msg/flex.hpp>
+#include <packet_interfaces/msg/power.hpp>
+#include <packet_interfaces/msg/voltage.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/subscription.hpp>
+#include <std_msgs/msg/empty.hpp>
+
+#include "nucleo_communicate/serial_port.hpp"
+
+namespace channel {
+
+class Channel : public rclcpp::Node {
+private:
+    nucleo_com::SerialPort serial;
+    std::mutex             serial_mutex;
+
+    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr          quit_subscription;
+    rclcpp::Subscription<packet_interfaces::msg::Power>::SharedPtr power_subscription;
+
+    rclcpp::Publisher<packet_interfaces::msg::Flex>::SharedPtr    flex1_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::Flex>::SharedPtr    flex2_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::Current>::SharedPtr current_publisher;
+    rclcpp::Publisher<packet_interfaces::msg::Voltage>::SharedPtr voltage_publisher;
+
+    rclcpp::TimerBase::SharedPtr timer;
+
+    void quit_subscription_callback(const std_msgs::msg::Empty& _msg);
+    void power_subscription_callback(const packet_interfaces::msg::Power& msg);
+
+    void timer_callback();
+
+public:
+    Channel(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+};
+
+}
+
+#endif // NUCLEO_COMMUNICATE_CHANNEL_HPP

device/nucleo_communicate/include/nucleo_communicate/recv_data.hpp

@@ -0,0 +1,25 @@
+#ifndef NUCLEO_COMMUNICATE_RECV_DATA_HPP
+#define NUCLEO_COMMUNICATE_RECV_DATA_HPP
+
+#include <array>
+#include <cstdint>
+#include <std_msgs/msg/detail/header__struct.hpp>
+
+namespace nucleo_com {
+
+struct RecvData {
+    using BufferType = std::array<std::uint8_t, 8>;
+
+    BufferType buffer;
+
+    explicit RecvData(BufferType&& buf);
+
+    auto flex1_value() const -> std::uint16_t;
+    auto flex2_value() const -> std::uint16_t;
+    auto current_value() const -> std::uint16_t;
+    auto voltage_value() const -> std::uint16_t;
+};
+
+}
+
+#endif // NUCLEO_COMMUNICATE_RECV_DATA_HPP

device/nucleo_communicate/include/nucleo_communicate/send_data.hpp

@@ -0,0 +1,25 @@
+#ifndef NUCLEO_COMMUNICATE_SEND_DATA_HPP
+#define NUCLEO_COMMUNICATE_SEND_DATA_HPP
+
+#include <array>
+#include <cstdint>
+
+#include "packet_interfaces/msg/power.hpp"
+
+namespace nucleo_com {
+
+struct SendData {
+    using BufferType = std::array<std::uint8_t, 16>;
+
+    BufferType buffer;
+
+    SendData() = delete;
+
+    explicit SendData(BufferType&& buf);
+
+    static auto from_msg(const packet_interfaces::msg::Power& msg) -> SendData;
+};
+
+}
+
+#endif // NUCLEO_COMMUNICATE_SEND_DATA_HPP

device/nucleo_communicate/include/nucleo_communicate/serial_port.hpp

@@ -0,0 +1,36 @@
+#ifndef NUCLEO_COMMUNICATE_SERIAL_PORT_HPP
+#define NUCLEO_COMMUNICATE_SERIAL_PORT_HPP
+
+#include <stdio.h>
+#include <string>
+#include <termios.h>
+
+#include "nucleo_communicate/recv_data.hpp"
+#include "nucleo_communicate/send_data.hpp"
+
+namespace nucleo_com {
+
+class SerialPort {
+private:
+    FILE* serial;
+
+public:
+    explicit SerialPort(const std::string& path = "/dev/ttyACM0");
+
+    ~SerialPort();
+
+    // same as
+    // https://os.mbed.com/docs/mbed-os/v6.16/apis/serial-uart-apis.html#configuration
+    void setup();
+
+    // https://github.com/rogy-AquaLab/2024_umiusi_nucleo
+    void send(const nucleo_com::SendData& data);
+
+    auto receive() -> nucleo_com::RecvData;
+
+    void quit();
+};
+
+}
+
+#endif // NUCLEO_COMMUNICATE_SERIAL_PORT_HPP

device/nucleo_communicate/launch/channel_launch.py

@@ -0,0 +1,20 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    channel = Node(
+        package="nucleo_communicate",
+        executable="nucleo-channel",
+        namespace="device",
+        remappings=[
+            ("/device/flex_1", "/packet/sensors/flex_1"),
+            ("/device/flex_2", "/packet/sensors/flex_2"),
+            ("/device/current", "/packet/sensors/current"),
+            ("/device/voltage", "/packet/sensors/voltage"),
+            # FIXME: /packet namespaceにremap
+            ("/device/power", "/device/order/power"),
+            ("/device/quit", "/device/order/quit")
+        ],
+    )
+    return LaunchDescription([channel])

device/nucleo_communicate/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nucleo_communicate</name>
+  <version>0.1.0</version>
+  <description>Nucleoとの通信周り</description>
+  <maintainer email="[email protected]">h1rono</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+  <depend>packet_interfaces</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>