add auto_driver_interface

auto_driver_interface/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.5)
+project(auto_driver_interface)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+
+# yaw_node --------------------------------------------------
+set(TARGET pid_node)
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/${TARGET}.cpp)
+
+rclcpp_components_register_node(
+  ${PROJECT_NAME}
+  PLUGIN "${PROJECT_NAME}::PidNode"
+  EXECUTABLE ${PROJECT_NAME}_exec)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)

auto_driver_interface/README.md

@@ -0,0 +1,3 @@
+```bash
+ros2 launch auto_driver_interface examle.launch.py
+```

auto_driver_interface/launch/examle.launch.py

@@ -0,0 +1,48 @@
+import launch
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    return launch.LaunchDescription([
+        ComposableNodeContainer(
+            name='auto_driver_interface',
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='auto_driver_interface',
+                    plugin='auto_driver_interface::PidNode',
+                    name='pitch_motor',
+                    namespace='can_node/gm6020_0',
+                    parameters=[
+                        {'p': 100},
+                        {'i': 30},
+                        {'d': 5},
+                        {'max_spd': 8000},
+                        {'init_deg': 120},
+                        {'min_limit': 20},
+                        {'max_limit': 135}
+                    ]
+                ),
+
+                ComposableNode(
+                    package='auto_driver_interface',
+                    plugin='auto_driver_interface::PidNode',
+                    name='yaw_motor',
+                    namespace='can_node/gm6020_1',
+                    parameters=[
+                        {'p': 100},
+                        {'i': 30},
+                        {'d': 5},
+                        {'max_spd': 8000},
+                        {'init_deg': 180},
+                        {'min_limit': 25},
+                        {'max_limit': 315}
+                    ]
+                )
+            ],
+        )
+    ])

auto_driver_interface/package.xml

@@ -0,0 +1,23 @@
+<?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>auto_driver_interface</name>
+  <version>0.0.0</version>
+  <description>The auto_driver_interface package</description>
+  <maintainer email="[email protected]">Ar-Ray-code</maintainer>
+  <license>Apache-2.0 License</license>
+  <author email="[email protected]">Ar-Ray-code</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

auto_driver_interface/src/pid.hpp

@@ -0,0 +1,65 @@
+#pragma once
+
+class PID
+{
+
+public:
+    PID(float p, float i, float d, float max, float min, float dt)
+    {
+        this->p = p;
+        this->i = i;
+        this->d = d;
+        this->max = max;
+        this->min = min;
+        this->integral = 0;
+        this->previous_error = 0;
+        this->dt = dt;
+    }
+
+    float calculate(float setpoint, float pv)
+    {
+        float error = setpoint - pv;
+        float p_term = p * error;
+        integral += error * dt;
+        float i_term = i * integral;
+        float d_term = d * ((error - previous_error) / dt);
+        previous_error = error;
+        float output = p_term + i_term + d_term;
+        if (output > max)
+        {
+            output = max;
+        }
+        else if (output < min)
+        {
+            output = min;
+        }
+        return output;
+    }
+
+    void reconfigure(float p, float i, float d, float max, float min)
+    {
+        this->p = p;
+        this->i = i;
+        this->d = d;
+        this->max = max;
+        this->min = min;
+
+        reset();
+    }
+
+    void reset()
+    {
+        integral = 0;
+        previous_error = 0;
+    }
+
+private:
+    float p;
+    float i;
+    float d;
+    float max;
+    float min;
+    float integral;
+    float previous_error;
+    float dt;
+};

auto_driver_interface/src/pid_node.cpp

@@ -0,0 +1,115 @@
+// Copyright 2024 StrayedCats.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/int64.hpp>
+#include <std_msgs/msg/bool.hpp>
+
+#include "pid.hpp"
+
+namespace auto_driver_interface
+{
+
+class PidNode : public rclcpp::Node
+{
+
+public:
+  PidNode(rclcpp::NodeOptions options)
+  : Node("node", options)
+  {
+    this->declare_parameter<int>("init_deg", 180);
+    this->declare_parameter<int>("p", 100);
+    this->declare_parameter<int>("i", 60);
+    this->declare_parameter<int>("d", 10);
+    this->declare_parameter<int>("max_spd", 5000);
+    this->declare_parameter<int>("min_limit", 25);
+    this->declare_parameter<int>("max_limit", 315);
+
+    this->get_parameter("init_deg", target_deg);
+    this->get_parameter("p", p);
+    this->get_parameter("i", i);
+    this->get_parameter("d", d);
+    this->get_parameter("max_spd", max_spd);
+    this->get_parameter("min_limit", min_limit);
+    this->get_parameter("max_limit", max_limit);
+
+    publisher_ = this->create_publisher<std_msgs::msg::Int64>("target_volt", 1);
+
+    current_deg_sub = this->create_subscription<std_msgs::msg::Float64>("degree", 1, [&](const std_msgs::msg::Float64::SharedPtr msg) {
+        current_deg = msg->data;
+    });
+    target_deg_sub = this->create_subscription<std_msgs::msg::Int64>("target_deg", 1, [&](const std_msgs::msg::Int64::SharedPtr msg) {
+      if (msg->data > max_limit)
+      {
+        RCLCPP_WARN(this->get_logger(), "Target degree is over max limit");
+        target_deg = max_limit;
+      }
+      else if (msg->data < min_limit)
+      {
+        RCLCPP_WARN(this->get_logger(), "Target degree is under min limit");
+        target_deg = min_limit;
+      }
+      else
+      {
+        target_deg = msg->data;
+      }
+    });
+
+    timer_ = this->create_wall_timer(std::chrono::milliseconds(5), std::bind(&PidNode::timer_callback, this));
+    pid = std::make_shared<PID>(p, i, d, max_spd, -max_spd, 0.005);
+
+  }
+
+private:
+  void timer_callback()
+  {
+      auto message = std_msgs::msg::Int64();
+      message.data = pid->calculate(target_deg, current_deg);
+      publisher_->publish(message);
+  }
+
+  rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr publisher_;
+  rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr pitch_publisher_;
+
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr minus_stop_trigger_sub;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr plus_stop_trigger_sub;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr pitch_down_stop_trigger_sub;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr pitch_up_stop_trigger_sub;
+  rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr target_deg_sub;
+
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr current_deg_sub;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr pitch_current_deg_sub;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  std::shared_ptr<PID> pid;
+
+  float current_deg = 180.0;
+
+  int target_deg = 180.0;
+  float pitch_target_deg = 90.0;
+
+  int p = 50;
+  int i = 3;
+  int d = 0;
+  int max_spd = 5000;
+  int min_limit = 25;
+  int max_limit = 315;
+};
+
+}  // namespace auto_driver_interface
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(auto_driver_interface::PidNode)