diff --git a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt index e7de26d..4ea5b7e 100644 --- a/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt +++ b/humanoid_robot_intelligence_control_system_controller/CMakeLists.txt @@ -37,6 +37,7 @@ if($ENV{ROS_VERSION} EQUAL 1) catkin_install_python(PROGRAMS src/PIDController.py + src/BellandeController.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) @@ -49,6 +50,7 @@ else() install(PROGRAMS src/PIDController.py + src/BellandeController.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/humanoid_robot_intelligence_control_system_controller/setup.py b/humanoid_robot_intelligence_control_system_controller/setup.py index cbd6630..5269c92 100644 --- a/humanoid_robot_intelligence_control_system_controller/setup.py +++ b/humanoid_robot_intelligence_control_system_controller/setup.py @@ -18,7 +18,7 @@ from catkin_pkg.python_setup import generate_distutils_setup # fetch values from package.xml setup_args = generate_distutils_setup( - scripts=['src/PIDController.py'], + scripts=['src/PIDController.py', "src/BellandeController.py"], packages=['humanoid_robot_intelligence_control_system_controller'], package_dir={'': 'src'}, ) diff --git a/humanoid_robot_intelligence_control_system_controller/src/BellandeController.py b/humanoid_robot_intelligence_control_system_controller/src/BellandeController.py new file mode 100644 index 0000000..2496dd5 --- /dev/null +++ b/humanoid_robot_intelligence_control_system_controller/src/BellandeController.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import os +import time +from typing import Tuple + +ros_version = os.getenv("ROS_VERSION") + +if ros_version == "1": + import rospy + from std_msgs.msg import Float64 +elif ros_version == "2": + import rclpy + from rclpy.node import Node + from std_msgs.msg import Float64 +else: + raise ValueError("Invalid ROS version. Set ROS_VERSION environment variable to '1' or '2'.") + +class BellandeController: + def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))): + self.kp, self.ki, self.kd = gains + self.name = name + self.output_limits = output_limits + self.reset() + + if ros_version == "1": + rospy.init_node('bellande_controller', anonymous=True) + self.subscriber = rospy.Subscriber('control_output', Float64, self.callback) + elif ros_version == "2": + rclpy.init() + self.node = Node('bellande_controller') + self.subscriber = self.node.create_subscription(Float64, 'control_output', self.callback, 10) + + def reset(self): + self.last_error = 0 + self.integral = 0 + self.last_time = time.time() + + def update_config(self, gains: Tuple[float, float, float]): + self.kp, self.ki, self.kd = gains + + def callback(self, msg): + print(f"Received control output: {msg.data}") + + def run(self): + if ros_version == "1": + rospy.spin() + elif ros_version == "2": + rclpy.spin(self.node) + + def shutdown(self): + if ros_version == "2": + self.node.destroy_node() + rclpy.shutdown() diff --git a/humanoid_robot_intelligence_control_system_internal_sensors/src/PIDController.py b/humanoid_robot_intelligence_control_system_internal_sensors/src/cpu.py similarity index 100% rename from humanoid_robot_intelligence_control_system_internal_sensors/src/PIDController.py rename to humanoid_robot_intelligence_control_system_internal_sensors/src/cpu.py