台形速度を書いてみました。

app/trapezoidal_acc/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.8)
+project(trapezoidal_acc)
+
+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(trapezoidal_acc REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(power_map_msg REQUIRED)
+
+ add_executable(Trapezoid
+  src/trapezoid_main.cpp
+  src/trapezoid.cpp
+ )
+
+ ament_target_dependencies(Trapezoid
+   rclcpp
+   std_msgs
+   power_map_msg
+   trapezoidal_acc
+ )
+
+ set_property(TARGET Trapezoid PROPERTY CXX_STANDARD 17)
+
+ target_include_directories(Trapezoid
+   PUBLIC
+     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+     $<INSTALL_INTERFACE:include>
+ )
+ install(TARGETS
+     Trapezoid
+   DESTINATION lib/${PROJECT_NAME}
+ )
+
+ament_package()

app/trapezoidal_acc/include/trapezoidal_acc/trapezoid.hpp

@@ -0,0 +1,32 @@
+#ifndef TRAPEZOIDAL_ACC_TRAPEZOID_HPP
+#define TRAPEZOIDAL_ACC_TRAPEZOID_HPP
+
+#include <memory>
+#include <power_map_msg/msg/detail/normalized_power__struct.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include "std_msgs/msg/string.hpp"
+#include "power_map_msg/msg/normalized_power.hpp"
+
+class Trapezoid : public rclcpp::Node {
+
+public:
+    Trapezoid();
+
+private:
+    rclcpp::Publisher<power_map_msg::msg::NormalizedPower>::SharedPtr Trapezoid_publisher;
+    rclcpp::Subscription<power_map_msg::msg::NormalizedPower>::SharedPtr Trapezoid_subscription;
+
+    rclcpp::TimerBase::SharedPtr _timer;
+    power_map_msg::msg::NormalizedPower NormalizedPower_msg;
+
+    void _loop();
+    void trapezoid_topic_callback(const power_map_msg::msg::NormalizedPower& msg);
+
+    double _current_velocity;
+    double _target_velocity;
+
+    power_map_msg::msg::NormalizedPower _NormalizedPower;
+
+};
+
+#endif // TRAPEZOIDAL_ACC_TRAPEZOID_HPP

app/trapezoidal_acc/package.xml

@@ -0,0 +1,16 @@
+<?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>trapezoidal_acc</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">norikonakano</maintainer>
+  <license>MIT</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>power_map_msg</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

app/trapezoidal_acc/src/trapezoid.cpp

@@ -0,0 +1,37 @@
+#include <chrono>
+#include <functional>
+#include <rclcpp/rclcpp.hpp>
+#include <string>
+#include "trapezoidal_acc/trapezoid.hpp"
+
+Trapezoid::Trapezoid(): 
+  rclcpp::Node("trapezoid_acc"),
+  _current_velocity(0.0), //現在の速度（初期化）
+  _target_velocity(10.0)// 目標速度
+{
+    Trapezoid_publisher = this->create_publisher<power_map_msg::msg::NormalizedPower>("/app/unsafe_power", 10);
+    Trapezoid_subscription = this->create_subscription<power_map_msg::msg::NormalizedPower>(
+        "/app/safe_power", 10, std::bind(&Trapezoid::trapezoid_topic_callback, this, std::placeholders::_1));
+
+    _timer = this->create_wall_timer(
+        std::chrono::milliseconds(100), std::bind(&Trapezoid::_loop, this));
+}
+
+void Trapezoid::trapezoid_topic_callback(const power_map_msg::msg::NormalizedPower& msg)
+{
+  RCLCPP_DEBUG(this->get_logger(), "Received trapezoid_message");
+  _NormalizedPower= msg;
+}
+
+void Trapezoid::_loop() //速度のアルゴリズムがあまり理解できていないです。。
+{
+  double threshold_velocity = 5.0; // 速度の限度（仮に5.0とする。）
+
+  // 速度がしきい値を超えた場合に速度を変更
+  if (_current_velocity > threshold_velocity) {
+    _current_velocity = threshold_velocity;
+  } 
+
+  auto message = power_map_msg::msg::NormalizedPower();
+  Trapezoid_publisher->publish(message);
+}

app/trapezoidal_acc/src/trapezoid_main.cpp

@@ -0,0 +1,11 @@
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+#include "trapezoidal_acc/trapezoid.hpp"
+
+int main(int argc, char * argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<Trapezoid>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}