Merge pull request #7 from rogy-AquaLab/simple-joy-app

WIP package simple_joy_app

.clang-format

@@ -36,7 +36,7 @@ AlignConsecutiveShortCaseStatements:
  Enabled: true
  AcrossEmptyLines: false
  AcrossComments: false
- AlignCaseArrows: false
+ # AlignCaseArrows: false
  AlignCaseColons: false
 
 AlignEscapedNewlines: Left
@@ -138,11 +138,11 @@ InsertBraces: true
 InsertNewlineAtEOF: true
 InsertTrailingCommas: Wrapped
 IntegerLiteralSeparator:
- Binary: 4
+ Binary: 0
  BinaryMinDigits: 0
- Decimal: 3
+ Decimal: 0
  DecimalMinDigits: 0
- Hex: 4
+ Hex: 0
  HexMinDigits: 0
 #JavaScriptQuotes: Leave
 #JavaScriptWrapImports: true

app/simple_joy_app/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.8)
+project(simple_joy_app)
+
+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(sensor_msgs REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(packet_interfaces REQUIRED)
+
+add_library(simple_joy_app
+ SHARED
+ src/app.cpp
+)
+
+ament_target_dependencies(simple_joy_app
+ rclcpp
+ sensor_msgs
+ rclcpp_components
+ packet_interfaces
+)
+
+target_include_directories(simple_joy_app
+ PRIVATE
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>
+)
+
+rclcpp_components_register_node(
+ simple_joy_app
+ PLUGIN "app::App"
+ EXECUTABLE app
+)
+
+install(
+ TARGETS simple_joy_app
+ EXPORT export_simple_joy_app_component
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION bin
+)
+
+install(
+ DIRECTORY launch
+ DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

app/simple_joy_app/README.md

@@ -0,0 +1,3 @@
+# Simple Joy App
+
+joystickからモーター出力

app/simple_joy_app/include/simple_joy_app/app.hpp

@@ -0,0 +1,31 @@
+#ifndef SIMPLE_JOY_APP_APP_HPP
+#define SIMPLE_JOY_APP_APP_HPP
+
+#include <utility>
+
+#include "packet_interfaces/msg/power.hpp"
+#include "sensor_msgs/msg/joy.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace app {
+
+class App : public rclcpp::Node {
+private:
+ rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription;
+
+ rclcpp::Publisher<packet_interfaces::msg::Power>::SharedPtr power_publisher;
+
+ void joy_callback(const sensor_msgs::msg::Joy& msg);
+
+ auto para_move_power(const std::pair<double, double>& stick
+ ) -> packet_interfaces::msg::Power;
+
+ auto stop_power() -> packet_interfaces::msg::Power;
+
+public:
+ App(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+};
+
+}
+
+#endif // SIMPLE_JOY_APP_APP_HPP

app/simple_joy_app/launch/app_launch.py

@@ -0,0 +1,12 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package="simple_joy_app",
+ executable="app",
+ namespace="app"
+ )
+ ])

app/simple_joy_app/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>simple_joy_app</name>
+ <version>0.1.0</version>
+ <description>joystickからモーター出力</description>
+ <maintainer email="[email protected]">h1rono</maintainer>
+ <license>MIT</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <depend>rclcpp</depend>
+ <depend>sensor_msgs</depend>
+ <depend>rclcpp_components</depend>
+ <depend>packet_interfaces</depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>

app/simple_joy_app/src/app.cpp

@@ -0,0 +1,130 @@
+#include <cmath>
+#include <functional>
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "simple_joy_app/app.hpp"
+
+void app::App::joy_callback(const sensor_msgs::msg::Joy& msg) {
+ RCLCPP_INFO_STREAM(this->get_logger(), "received joystick msg");
+ // https://www.pygame.org/docs/ref/joystick.html#playstation-4-controller-pygame-2-x
+
+ // スティックの値を見る最小範囲
+ constexpr double stick_threshold = 0.1;
+ // 左スティック h:水平 v:垂直
+ const double lstick_h = msg.axes[0];
+ const double lstick_v = -msg.axes[1]; // 上方向が負のため逆転
+ // 右スティック h:水平 v:垂直
+ const double rstick_h = msg.axes[2];
+ const double rstick_v = -msg.axes[3];
+
+ 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 (lstick_effective) {
+ packet_interfaces::msg::Power msg = this->para_move_power({ lstick_v, lstick_h });
+ this->power_publisher->publish(msg);
+ return;
+ }
+ if (!rstick_effective) {
+ packet_interfaces::msg::Power msg = this->stop_power();
+ this->power_publisher->publish(msg);
+ return;
+ }
+ if (rstick_h_effective) {
+ // TODO: 水平回転
+ return;
+ }
+ // assert(rstick_v_effective); 自明
+ // TODO: 垂直方向の移動
+}
+
+auto app::App::para_move_power(const std::pair<double, double>& stick
+) -> packet_interfaces::msg::Power {
+ // FIXME: C++20...?
+ constexpr double pi = 3.141592653589793;
+ // FIXME: use parameter
+ constexpr uint16_t bldc_center = 1480;
+ constexpr uint16_t bldc_radius = 250; // TODO: check this
+ constexpr uint16_t servo_min = 500;
+ constexpr uint16_t servo_max = 2400;
+ // TODO: provide explanation
+ constexpr double theta = pi / 4;
+ // Left-Up, Right-Down
+ double bldc1 = stick.first * cos(theta) + stick.second * sin(theta);
+ // Right-Up, Left-Down
+ double bldc2 = stick.first * cos(theta) - stick.second * sin(theta);
+ const double bldc_abc_max = std::max(std::abs(bldc1), std::abs(bldc2));
+ // constrain
+ if (bldc_abc_max > 1) {
+ const double scale = 1 / bldc_abc_max;
+ bldc1 *= scale;
+ bldc2 *= scale;
+ }
+
+ packet_interfaces::msg::Power msg{};
+
+ const uint16_t bldc1_msg = static_cast<uint16_t>(
+ static_cast<double>(bldc_center) + static_cast<double>(bldc_radius) * bldc1
+ );
+ const uint16_t bldc2_msg = static_cast<uint16_t>(
+ static_cast<double>(bldc_center) + static_cast<double>(bldc_radius) * bldc2
+ );
+
+ // TODO: Fix order with param
+ msg.bldc[0] = bldc1_msg;
+ msg.bldc[1] = bldc2_msg;
+ msg.bldc[2] = bldc2_msg;
+ msg.bldc[3] = bldc1_msg;
+
+ constexpr uint16_t servo_center = (servo_min + servo_max) / 2;
+
+ msg.servo[0] = servo_center;
+ msg.servo[1] = servo_center;
+ msg.servo[2] = servo_center;
+ msg.servo[3] = servo_center;
+
+ return msg;
+}
+
+auto app::App::stop_power() -> packet_interfaces::msg::Power {
+ // FIXME: use parameter
+ constexpr uint16_t bldc_center = 1480;
+ constexpr uint16_t servo_min = 500;
+ constexpr uint16_t servo_max = 2400;
+ constexpr uint16_t servo_center = (servo_min + servo_max) / 2;
+
+ packet_interfaces::msg::Power msg{};
+
+ msg.bldc[0] = bldc_center;
+ msg.bldc[1] = bldc_center;
+ msg.bldc[2] = bldc_center;
+ msg.bldc[3] = bldc_center;
+
+ msg.servo[0] = servo_center;
+ msg.servo[1] = servo_center;
+ msg.servo[2] = servo_center;
+ msg.servo[3] = servo_center;
+
+ return msg;
+}
+
+app::App::App(const rclcpp::NodeOptions& options) :
+ rclcpp::Node("simple_joy_app", options),
+ subscription(),
+ power_publisher() {
+ using std::placeholders::_1;
+ auto callback = std::bind(&app::App::joy_callback, this, _1);
+ this->subscription = this->create_subscription<sensor_msgs::msg::Joy>(
+ "/device/joystick", 10, callback
+ );
+ this->power_publisher = this->create_publisher<packet_interfaces::msg::Power>(
+ "/device/order/power", 10
+ );
+}
+
+RCLCPP_COMPONENTS_REGISTER_NODE(app::App)