Merge pull request #16 from rogy-AquaLab/power-map

Power map

app/power_map/CMakeLists.txt

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

app/power_map/README.md

@@ -0,0 +1,18 @@
+# power_map
+
+`power_map_msg/msg/NormalizedPower`のデータを受け取って`packet_interfaces/msg/Power`に変換するNode
+
+## Nodes
+
+- `power_map`: 上述のとおり
+
+## Executables
+
+- `power-map`: `power_map`Nodeを実行する
+
+## Launches
+
+- `launch.py`:
+ `power_map`Nodeに`config/default_param.yml`でparameterを与えつつ、それを実行する。
+ その際`app`namespaceが適用され、出力の`/app/power`topicは`/device/order/power`topicにremapされる
+ (出力先は`device/nucleo_communicate_py`を参照)。

app/power_map/config/default_param.yml

@@ -0,0 +1,26 @@
+/**:
+ ros__parameters:
+ # 1
+ bldc_center1: 1480
+ bldc_negative_radius1: 250
+ bldc_positive_radius1: 250
+ servo_max1: 2400
+ servo_min1: 500
+ # 2
+ bldc_center2: 1480
+ bldc_negative_radius2: 250
+ bldc_positive_radius2: 250
+ servo_max2: 2400
+ servo_min2: 500
+ # 3
+ bldc_center3: 1480
+ bldc_negative_radius3: 250
+ bldc_positive_radius3: 250
+ servo_max3: 2400
+ servo_min3: 500
+ # 4
+ bldc_center4: 1480
+ bldc_negative_radius4: 250
+ bldc_positive_radius4: 250
+ servo_max4: 2400
+ servo_min4: 500

app/power_map/include/power_map/container.hpp

@@ -0,0 +1,65 @@
+#ifndef POWER_MAP_CONTAINER_HPP
+#define POWER_MAP_CONTAINER_HPP
+
+namespace power_map {
+
+template<class T> class Container {
+private:
+ T _bldc_center;
+ T _bldc_positive_radius;
+ T _bldc_negative_radius;
+ T _servo_min;
+ T _servo_max;
+
+public:
+ Container() = default;
+
+ auto bldc_center() const -> const T& {
+ return this->_bldc_center;
+ }
+
+ auto bldc_center(T&& v) -> Container& {
+ this->_bldc_center = v;
+ return *this;
+ }
+
+ auto bldc_positive_radius() const -> const T& {
+ return this->_bldc_positive_radius;
+ }
+
+ auto bldc_positive_radius(T&& v) -> Container& {
+ this->_bldc_positive_radius = v;
+ return *this;
+ }
+
+ auto bldc_negative_radius() const -> const T& {
+ return this->_bldc_negative_radius;
+ }
+
+ auto bldc_negative_radius(T&& v) -> Container& {
+ this->_bldc_negative_radius = v;
+ return *this;
+ }
+
+ auto servo_min() const -> const T& {
+ return this->_servo_min;
+ }
+
+ auto servo_min(T&& v) -> Container& {
+ this->_servo_min = v;
+ return *this;
+ }
+
+ auto servo_max() const -> const T& {
+ return this->_servo_max;
+ }
+
+ auto servo_max(T&& v) -> Container& {
+ this->_servo_max = v;
+ return *this;
+ }
+};
+
+}
+
+#endif // POWER_MAP_CONTAINER

app/power_map/include/power_map/power_map.hpp

@@ -0,0 +1,57 @@
+#ifndef POWER_MAP_HPP
+#define POWER_MAP_HPP
+
+#include <array>
+
+#include <memory>
+#include <rclcpp/node.hpp>
+#include <rclcpp/node_options.hpp>
+#include <rclcpp/parameter_event_handler.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/subscription.hpp>
+
+#include "packet_interfaces/msg/power.hpp"
+#include "power_map_msg/msg/normalized_power.hpp"
+
+#include "power_map/container.hpp"
+
+namespace power_map {
+
+class PowerMap : public rclcpp::Node {
+private:
+ using Config = Container<int>;
+ using CbHandles = Container<std::shared_ptr<rclcpp::ParameterCallbackHandle>>;
+
+ std::shared_ptr<rclcpp::ParameterEventHandler> param_cb;
+
+ std::array<Config, 4> configs;
+ std::array<CbHandles, 4> cb_handles;
+
+ rclcpp::Publisher<packet_interfaces::msg::Power>::SharedPtr publisher;
+
+ rclcpp::Subscription<power_map_msg::msg::NormalizedPower>::SharedPtr subscription;
+
+ auto create_bldc_center_cb(size_t i
+ ) -> rclcpp::ParameterCallbackHandle::ParameterCallbackType;
+
+ auto create_bldc_positive_radius_cb(size_t i
+ ) -> rclcpp::ParameterCallbackHandle::ParameterCallbackType;
+
+ auto create_bldc_negative_radius_cb(size_t i
+ ) -> rclcpp::ParameterCallbackHandle::ParameterCallbackType;
+
+ auto create_servo_min_cb(size_t i
+ ) -> rclcpp::ParameterCallbackHandle::ParameterCallbackType;
+
+ auto create_servo_max_cb(size_t i
+ ) -> rclcpp::ParameterCallbackHandle::ParameterCallbackType;
+
+ auto subscription_callback(const power_map_msg::msg::NormalizedPower& msg) -> void;
+
+public:
+ PowerMap(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+};
+
+}
+
+#endif // POWER_MAP_HPP

app/power_map/launch/launch.py

@@ -0,0 +1,22 @@
+import os.path
+
+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("power_map"),
+ "config",
+ "default_param.yml"
+ )
+ return LaunchDescription([
+ Node(
+ package="power_map",
+ executable="power-map",
+ namespace="app",
+ parameters=[config],
+ remappings=[("/app/power", "/packet/order/power")]
+ )
+ ])

app/power_map/package.xml

@@ -0,0 +1,21 @@
+<?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>power_map</name>
+ <version>0.1.0</version>
+ <description>
+ `power_map_msg/msg/NormalizedPower`のデータを受け取って`packet_interfaces/msg/Power`に変換するNode
+ </description>
+ <maintainer email="[email protected]">h1rono</maintainer>
+ <license>MIT</license>
+
+ <buildtool_depend>ament_cmake</buildtool_depend>
+ <depend>rclcpp</depend>
+ <depend>packet_interfaces</depend>
+ <depend>power_map_msg</depend>
+ <depend>rclcpp_components</depend>
+
+ <export>
+ <build_type>ament_cmake</build_type>
+ </export>
+</package>