From b0d3b3a5f7400fb09fcd89143354de50d8525e92 Mon Sep 17 00:00:00 2001 From: Space concordia rover Date: Wed, 21 Feb 2024 16:51:49 -0500 Subject: [PATCH] move the control system to inside the absenc node to hopefully reduce latency --- .../src/absenc_interface/src/absenc_node.cpp | 92 +++++++++--- .../src/arm_controller/CMakeLists.txt | 34 +++++ .../src/arm_ik/arm_ik/AbsencControlSystem.py | 135 ------------------ .../src/arm_ik/arm_ik/AbsencNode.py | 95 ------------ robot/rospackages/src/arm_ik/arm_ik/IKNode.py | 5 +- robot/rospackages/src/arm_ik/setup.py | 1 - robot/util/rosRoverStart/robot_ik.py | 12 +- 7 files changed, 117 insertions(+), 257 deletions(-) create mode 100644 robot/rospackages/src/arm_controller/CMakeLists.txt delete mode 100644 robot/rospackages/src/arm_ik/arm_ik/AbsencControlSystem.py delete mode 100644 robot/rospackages/src/arm_ik/arm_ik/AbsencNode.py diff --git a/robot/rospackages/src/absenc_interface/src/absenc_node.cpp b/robot/rospackages/src/absenc_interface/src/absenc_node.cpp index 76a1b60d3..7bafa4c98 100644 --- a/robot/rospackages/src/absenc_interface/src/absenc_node.cpp +++ b/robot/rospackages/src/absenc_interface/src/absenc_node.cpp @@ -1,10 +1,15 @@ #include "absenc.h" +#include "math.h" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include #include +#include +#include +#include /*Include autogenerated message header*/ #include "absenc_interface/msg/encoder_values.hpp" +#include using namespace std::chrono_literals; class AbsEnc : public rclcpp::Node { @@ -21,10 +26,9 @@ class AbsEnc : public rclcpp::Node this->declare_parameter("absenc_path", "/dev/ttyUSB0"); this->declare_parameter("absenc_polling_rate", 100); - subscription = this->create_subscription( - "absenc_values", 10, std::bind(&AbsEnc::encoderValuesTopicCallback, this, std::placeholders::_1)); - - publisher = this->create_publisher("absenc_values", 10); + angles_publisher = this->create_publisher("absenc_values", 10); + + arm_publisher = this->create_publisher("arm_command", 10); timer = this->create_wall_timer( std::chrono::milliseconds(this->get_parameter("absenc_polling_rate").as_int()), @@ -37,6 +41,10 @@ class AbsEnc : public rclcpp::Node if(err.error != 0){ std::cout << "fuic you"; } + + subscription = this->create_subscription( + "joint_states", 10, std::bind(&AbsEnc::ikValuesCallback, this, std::placeholders::_1)); + } private: @@ -51,24 +59,76 @@ class AbsEnc : public rclcpp::Node ABSENC::PollSlave(2,&absenc_meas_2); ABSENC::PollSlave(3,&absenc_meas_3); - message.angle_1 = absenc_meas_1.angval + 180.f; + message.angle_1 = absenc_meas_1.angval < 0 ? absenc_meas_1.angval + 180.f : absenc_meas_1.angval - 180; message.angle_2 = absenc_meas_2.angval; - message.angle_3 = absenc_meas_3.angval - 180.f; - + message.angle_3 = absenc_meas_3.angval < 0 ? 180 + absenc_meas_3.angval : absenc_meas_3.angval - 180.f; + + // hold onto value for control system + abs_angles[1] = message.angle_1; + abs_angles[2] = message.angle_2; + abs_angles[3] = message.angle_3; + //TODO : Can simplify this a bit. - publisher->publish(message); + angles_publisher->publish(message); } - void encoderValuesTopicCallback(const absenc_interface::msg::EncoderValues::SharedPtr msg) const - { - - std::cout << "Angle 1 : " << msg->angle_1 << std::endl; - std::cout << "Angle 2 : " << msg->angle_2 << std::endl; - std::cout << "Angle 3 : " << msg->angle_3 << std::endl; + void ikValuesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { + // Convert ik angles to degrees + for (int i = 0; i < 4; i++) { + ik_angles[i] = msg->position[i] * 180.0 / M_PI; + } + std::string arm_command = "set_motor_speeds "; + + std::cout << "Angles: "; + for (int i = 0; i < std::size(ik_angles); i++) { + std::cout << "ik " << ik_angles[i] << " abs " << abs_angles[i] << " "; + } + std::cout << "\n"; + + for (int i = 0; i < std::size(ik_angles); i++) { + float absenc_angle = abs_angles[i]; + float ik_angle = ik_angles[i]; + float motor_sign = motor_signs[i]; + + float absolute_difference = std::abs(absenc_angle - ik_angle); + int difference_sign = absenc_angle - ik_angle >= 0 ? 1 : -1; + + if (absolute_difference <= 1) { + arm_command += "0 "; + } else { + // Get motor value from difference from desired position, with gain 2000 + float value = ((absolute_difference) / 180.0) * 2000.0; + // Clamp + value = value > 255.0 ? 255.0 : value; + // Account for sign differences + value *= difference_sign; + value *= motor_sign; + // Add to msg + int intVal = (int)value; + arm_command += std::to_string(intVal); + arm_command += " "; + } + } + + // Add 0 values for two last values + arm_command += "0 0"; + + auto arm_msg = std_msgs::msg::String(); + arm_msg.data = arm_command; + + arm_publisher->publish(arm_msg); } + rclcpp::TimerBase::SharedPtr timer; - rclcpp::Subscription::SharedPtr subscription; - rclcpp::Publisher::SharedPtr publisher; + rclcpp::Subscription::SharedPtr subscription; + rclcpp::Publisher::SharedPtr angles_publisher; + rclcpp::Publisher::SharedPtr arm_publisher; + + // Both these store 4 angles to be ready if a 4th encoder is added + std::array ik_angles = {0.0, 0.0, 0.0, 0.0}; + std::array abs_angles = {0.0, 0.0, 0.0, 0.0}; + // Controls if motor sign is aligned with encoder direction + std::array motor_signs = {1, -1, -1, -1}; }; int main(int argc, char * argv[]) diff --git a/robot/rospackages/src/arm_controller/CMakeLists.txt b/robot/rospackages/src/arm_controller/CMakeLists.txt new file mode 100644 index 000000000..e8a8b3f84 --- /dev/null +++ b/robot/rospackages/src/arm_controller/CMakeLists.txt @@ -0,0 +1,34 @@ + +cmake_minimum_required(VERSION 3.5) +project(arm_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(JetsonGPIO) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ArmMotorValues.msg" +) +include_directories(include) + +add_executable(arm_controller_node src/arm_controller_node.cpp) +rosidl_target_interfaces(arm_controller_node ${PROJECT_NAME} "rosidl_typesupport_cpp") + +ament_target_dependencies(arm_controller_node rclcpp sensor_msgs std_msgs) +target_link_libraries(arm_controller_node JetsonGPIO::JetsonGPIO) + +install(TARGETS + arm_controller_node + DESTINATION lib/${PROJECT_NAME}) +ament_package() diff --git a/robot/rospackages/src/arm_ik/arm_ik/AbsencControlSystem.py b/robot/rospackages/src/arm_ik/arm_ik/AbsencControlSystem.py deleted file mode 100644 index 047422351..000000000 --- a/robot/rospackages/src/arm_ik/arm_ik/AbsencControlSystem.py +++ /dev/null @@ -1,135 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from sensor_msgs.msg import Joy -from rclpy.node import Node -import math -from rclpy.node import Node -from rclpy.qos import QoSProfile -from geometry_msgs.msg import Quaternion -from std_msgs.msg import String, Header, Float32 -from sensor_msgs.msg import JointState -from absenc_interface.msg import EncoderValues -from arm_controller.msg import ArmMotorValues -import threading - - - -class AbsencNode(Node): - - def __init__(self): - node_name = 'absenc_node' - super().__init__(node_name) - self.get_logger().info('Initialized "'+node_name+'" node for functionality') - - arm_topic = '/arm_command' - self.arm_publisher = self.create_publisher(String, arm_topic, 10) - self.get_logger().info('Created publisher for topic "'+arm_topic) - - arm_2_topic = '/arm_values' - self.arm_2_publisher = self.create_publisher(ArmMotorValues, arm_2_topic, 10) - #self.get_logger().info('Created publisher for topic "'+arm_topic) - - ik_topic = '/joint_states' - self.ik_sub = self.create_subscription(JointState, ik_topic, self.ik_callback, 10) - self.get_logger().info('Created subscriber for topic "'+ik_topic) - - absenc_topic = '/absenc_values' - self.absenc_sub = self.create_subscription(EncoderValues, absenc_topic, self.absenc_callback, 10) - self.get_logger().info('Created subscriber for topic "' + absenc_topic) - - # Angles reported from absenc - # Angles reported from IK - self.abs_angles = None - self.ik_angles = None - # self.ik_angles = [0.0, -30.0, 20.0, 10.0] - - # Controls if motor sign is aligned with encoder direction - self.angle_signs = [1, -1, -1, -1] - - - def ik_callback(self, message:JointState): - # Receive values and convert to degrees - self.ik_angles = [math.degrees(x) for x in list(message.position)] - - def absenc_callback(self, message: EncoderValues): - angle_1 = message.angle_1 if message.angle_1 < 180 else message.angle_1 - 360 - angle_2 = message.angle_2 - angle_3 = 360 + message.angle_3 if message.angle_3 < -180 else message.angle_3 - self.abs_angles = [0, angle_1, angle_2, angle_3] - self.get_logger().info(f"Angles Fixed: {self.abs_angles}") - - def publish_arm_command(self): - if self.ik_angles == None or self.abs_angles == None: - return - - arm_command = "set_motor_speeds " - values_2 = [] - - for absenc_angle, ik_angle, angle_sign in zip(self.abs_angles, self.ik_angles, self.angle_signs): - # Here, the difference between the actual and desired angle is split into abs value and sign - # This is done to make mapping the range easier (it has a minimum value of 55, it can simply - # be added before the proper sign is applied) - absolute_difference = abs(absenc_angle - ik_angle) - difference_sign = 1 if absenc_angle - ik_angle >= 0 else -1 - - if absolute_difference <= 1: - arm_command += "0 " - values_2.append(0) - else: - # Map the angle difference (at most 180 deg) onto 0-255 range with a min of 150 - # This implements a basic P controller, further from correct angle, faster it moves - value = ((absolute_difference) / 180.0) * 2000 - # Clamp value - value = 255 if value >= 255 else value - value *= difference_sign - value = int(value) * angle_sign - values_2.append(float(value)) - arm_command += f"{value} " - # self.get_logger().info(f"Angle difference {}") - print(arm_command) - print(values_2) - # Add two for last two pwm motors this isn't controlling yet - arm_command += "0 0" - msg = String() - msg.data = arm_command - - msg2 = ArmMotorValues() - msg2.val[0] = values_2[0] - msg2.val[1] = values_2[1] - msg2.val[2] = values_2[2] - msg2.val[3] = values_2[3] - msg2.val[4] = 0 - msg2.val[5] = 0 - - # msg2.val[0] = values_2[0] - # msg2.val[1] = values_2[1] - # msg2.val[2] = values_2[2] - # msg2.val[3] = values_2[3] - # msg2.val[4] = values_2[4] - # msg2.val[5] = values_2[5] - - - self.arm_publisher.publish(msg) - self.arm_2_publisher.publish(msg2) - - -def main(args=None): - rclpy.init(args=args) - - absenc_node = AbsencNode() - - # Spin in a separate thread - thread = threading.Thread(target=rclpy.spin, args=(absenc_node, ), daemon=True) - thread.start() - - loop_rate = absenc_node.create_rate(30) - while rclpy.ok(): - try: - absenc_node.publish_arm_command() - loop_rate.sleep() - except KeyboardInterrupt: - print("Node shutting down due to shutting down node.") - break - - rclpy.shutdown() diff --git a/robot/rospackages/src/arm_ik/arm_ik/AbsencNode.py b/robot/rospackages/src/arm_ik/arm_ik/AbsencNode.py deleted file mode 100644 index 4ef1ffd59..000000000 --- a/robot/rospackages/src/arm_ik/arm_ik/AbsencNode.py +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env python3 - -import rclpy -from sensor_msgs.msg import Joy -from rclpy.node import Node -import math -from rclpy.node import Node -from rclpy.qos import QoSProfile -from geometry_msgs.msg import Quaternion -from std_msgs.msg import String, Header, Float32 -from sensor_msgs.msg import JointState -import threading - - - -class AbsencNode(Node): - - def __init__(self): - node_name = 'absenc_node' - super().__init__(node_name) - self.get_logger().info('Initialized "'+node_name+'" node for functionality') - - arm_topic = '/arm_command' - self.arm_publisher = self.create_publisher(String, arm_topic, 10) - self.get_logger().info('Created publisher for topic "'+arm_topic) - - ik_topic = '/joint_states' - self.ik_sub = self.create_subscription(JointState, ik_topic, self.ik_callback, 10) - self.get_logger().info('Created subscriber for topic "'+ik_topic) - - absenc_topic = '/absenc_states' - self.absenc_sub = self.create_subscription(JointState, absenc_topic, self.absenc_callback, 10) - self.get_logger().info('Created subscriber for topic "' + absenc_topic) - - # Angles reported from absenc - self.abs_angles = None - # Angles reported from IK - self.ik_angles = None - - - def ik_callback(self, message:JointState): - # Receive values and convert to degrees - self.ik_angles = [math.degrees(x) for x in list(message.position)] - - def absenc_callback(self, message): - pass - - def publish_arm_command(self): - if self.ik_angles == None or self.abs_angles == None: - return - - arm_command = "set_motor_speeds " - - for absenc_angle, ik_angle in zip(self.abs_angles, self.ik_angles): - # If very far move more quickly - if abs(absenc_angle - ik_angle) > 15: - if absenc_angle > ik_angle: - arm_command += "-255 " - else: - arm_command += "255 " - # if close move slowly - elif abs(absenc_angle - ik_angle) > 5: - if absenc_angle > ik_angle: - arm_command += "-100 " - else: - arm_command += "100 " - # if very close don't move - else: - arm_command += "0 " - - # Add two for last two pwm motors this isn't controlling yet - arm_command += "0 0" - - self.arm_publisher.publish(arm_command) - - - -def main(args=None): - rclpy.init(args=args) - - absenc_node = AbsencNode() - - # Spin in a separate thread - thread = threading.Thread(target=rclpy.spin, args=(absenc_node, ), daemon=True) - thread.start() - - loop_rate = absenc_node.create_rate(30) - while rclpy.ok(): - try: - absenc_node.publish_arm_command() - loop_rate.sleep() - except KeyboardInterrupt: - print("Node shutting down due to shutting down node.") - - rclpy.shutdown() diff --git a/robot/rospackages/src/arm_ik/arm_ik/IKNode.py b/robot/rospackages/src/arm_ik/arm_ik/IKNode.py index bf37c8630..8d3728dba 100644 --- a/robot/rospackages/src/arm_ik/arm_ik/IKNode.py +++ b/robot/rospackages/src/arm_ik/arm_ik/IKNode.py @@ -115,10 +115,7 @@ def initialize_angles_coords(self): def absenc_callback(self, message): # self.get_logger().info(f"Callback for absenc value") - angle_1 = 360 - message.angle_1 if message.angle_1 > 180 else message.angle_1 - angle_2 = message.angle_2 - angle_3 = 360 + message.angle_3 if message.angle_3 < -180 else message.angle_3 - self.abs_angles = [0, math.radians(angle_1), math.radians(angle_2), math.radians(angle_3)] + self.abs_angles = [0, math.radians(message.angle_1), math.radians(message.angle_2), math.radians(message.angle_3)] def coords_from_flex(self, angles): diff --git a/robot/rospackages/src/arm_ik/setup.py b/robot/rospackages/src/arm_ik/setup.py index 450e9564e..f75f62eaa 100644 --- a/robot/rospackages/src/arm_ik/setup.py +++ b/robot/rospackages/src/arm_ik/setup.py @@ -30,7 +30,6 @@ entry_points={ 'console_scripts': [ 'IKNode = arm_ik.IKNode:main', - 'AbsencControlSystem = arm_ik.AbsencControlSystem:main', 'CadMouseJoyNode = arm_ik.CadMouseJoyNode:main', 'state_publisher = arm_ik.state_publisher:main' ], diff --git a/robot/util/rosRoverStart/robot_ik.py b/robot/util/rosRoverStart/robot_ik.py index 2b44a33f1..5e2fdb4fd 100644 --- a/robot/util/rosRoverStart/robot_ik.py +++ b/robot/util/rosRoverStart/robot_ik.py @@ -23,12 +23,12 @@ def generate_launch_description(): name='absenc_node', output='screen' ), - Node( - package='arm_ik', - executable='AbsencControlSystem', - name='absenc_control_system', - output='screen' - ), + # Node( + # package='arm_ik', + # executable='AbsencControlSystem', + # name='absenc_control_system', + # output='screen' + # ), Node( package='arm_ik', executable='IKNode',