diff --git a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro index 7d09e83b..9754c9d1 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -1,36 +1,32 @@ - - rmd_ros2_control/RMDHardwareInterface + 10 + 5 + 0 + can0 - 330 - 586 + 0x14A 100 1.0 - - -1.0 - + 150 + - 324 - 580 + 0x144 100 1.0 - - -2.0 - + 150 + - - \ No newline at end of file diff --git a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro index 9deb797e..7f3638ec 100644 --- a/src/description/ros2_control/arm/arm.servo.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.servo.ros2_control.xacro @@ -1,22 +1,27 @@ - - - servo_ros2_control/SERVOHardwareInterface + 10 + 5 + 0 + can0 + 0x80 + 0x1 + 1 + 180 + 0.00016666666 + standard + prismatic - - - diff --git a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro index 30ce0a9e..1dc8b9b4 100644 --- a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro @@ -1,58 +1,44 @@ - - - smc_ros2_control/SMCHardwareInterface + 10 + 5 + 0 + can0 - 325 + 0x145 40 - - ${-2*pi} - ${2*pi} - + 300 + - - 0.0 - + - 326 + 0x146 40 - - ${-2*pi} - ${2*pi} - + 1000 + - - 0.0 - + - 327 + 0x147 40 - - ${-pi} - ${pi} - + 1000 + - - 0.0 - + - - - diff --git a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro index 954ba7cf..620a27c8 100644 --- a/src/description/ros2_control/arm/arm.talon.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.talon.ros2_control.xacro @@ -1,29 +1,24 @@ - - - talon_ros2_control/TALONHardwareInterface + 10 + 5 + 0 + can0 2 - - 0 - 1 - + 0.06 + prismatic + - - 0.0 - + - - - diff --git a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro index d21547a8..112d0805 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -1,67 +1,58 @@ - - rmd_ros2_control/RMDHardwareInterface + 10 + 5 + 0 + can0 - 323 - 579 + 0x143 1 -1.0 + 150 - - 0.0 - + - 322 - 578 + 0x142 1 1.0 + 150 - - 0.0 - + - 321 - 577 + 0x141 1 -1.0 + 150 - - 0.0 - + - 329 - 585 + 0x149 1 1.0 + 150 - - 0.0 - + - - - \ No newline at end of file diff --git a/src/description/ros2_control/science/science.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index abc0f49e..c8529a65 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -1,12 +1,14 @@ - - - servo_ros2_control/SERVOHardwareInterface + 10 + 5 + 0 + can0 + 0x80 @@ -24,30 +26,51 @@ + 0x2 + 1 + 178 + standard + revolute + + 0x3 + 1 + 178 + standard + revolute + 0x4 + 1 + 90 + 0.00016666666 + standard + prismatic + + 0x5 + 1 + 270 + 0.00016666666 + standard + prismatic - - - diff --git a/src/description/ros2_control/science/science.talon.ros2_control.xacro b/src/description/ros2_control/science/science.talon.ros2_control.xacro index 12bcc36c..00cbc8fe 100644 --- a/src/description/ros2_control/science/science.talon.ros2_control.xacro +++ b/src/description/ros2_control/science/science.talon.ros2_control.xacro @@ -1,42 +1,32 @@ - - - talon_ros2_control/TALONHardwareInterface + 10 + 5 + 0 + can0 1 - - 0 - 1 - + revolute + - - 0.0 - + 2 - - 0 - 1 - + revolute + - - 0.0 - + - - - \ No newline at end of file diff --git a/src/description/urdf/athena_arm.urdf.xacro b/src/description/urdf/athena_arm.urdf.xacro index ddaa25b0..0a5a8f7d 100644 --- a/src/description/urdf/athena_arm.urdf.xacro +++ b/src/description/urdf/athena_arm.urdf.xacro @@ -51,6 +51,4 @@ - - \ No newline at end of file diff --git a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp index 0e9fc460..e97c7b06 100644 --- a/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp +++ b/src/hardware_interfaces/rmd_ros2_control/include/rmd_ros2_control/rmd_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #ifndef RMD_HARDWARE_INTERACE_HPP_ #define RMD_HARDWARE_INTERACE_HPP_ @@ -44,22 +26,21 @@ namespace rmd_ros2_control { -class RMDHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface +class RMDHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(RMDHardwareInterface) - // Initialization, so reading parameters, initializing variables, checking if all the joint state and command interfaces are correct + // Initialization: Reading parameters, initializing variables, checking if all the joint state and command interfaces are correct hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo & info) override; // Exports/exposes Interfaces that are available so that the controllers // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - // Lifecycle + // -- Lifecycle Functions -- hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -86,22 +67,27 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; - void onCanMessage(const CANLib::CanFrame& frame); - // Helper Functions + // -- Helper Functions -- + void send_command(int can_id, int cmd_id); + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); - int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); private: - + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + + // Keeps track of amount of joints int num_joints; - int current_joint; - - // Mutex - std::mutex state_mutex_; // Store the state for the simulated robot std::vector joint_state_position_; @@ -111,19 +97,26 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher std::vector joint_command_position_; std::vector joint_command_velocity_; - std::vector encoder_position; + // Place holders for data from the canBus, will be accessed in read() std::vector motor_velocity; std::vector motor_position; - + + // Velocity at which **joint** rotates to reach position in 1 dps + uint16_t operating_velocity; + + // CAN Library Setup CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; - - std::vector joint_node_write_ids; - std::vector joint_node_read_ids; + std::string can_interface; + + // Joint specific parameters + std::vector joint_node_write_ids; + std::vector joint_node_read_ids; std::vector joint_gear_ratios; std::vector joint_orientation; + // Modes for control mode enum integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -134,6 +127,15 @@ class RMDHardwareInterface : public hardware_interface::SystemInterface // Inher // Active control mode for each actuator std::vector control_level_; + // CAN Commands + static constexpr uint8_t BRAKE_RELEASE_CMD = 0X77; + static constexpr uint8_t BRAKE_LOCK_CMD = 0x78; + static constexpr uint8_t MOTOR_SHUTDOWN_CMD = 0X80; + static constexpr uint8_t MOTOR_STOP_CMD = 0x81; + static constexpr uint8_t MOTOR_STATUS_2_CMD = 0X9C; + static constexpr uint8_t SPEED_CONTROL_CMD = 0xA2; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0xA4; + }; } // namespace rmd_hardware_interface diff --git a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp index 0f287fd0..fcb765db 100644 --- a/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp +++ b/src/hardware_interfaces/rmd_ros2_control/src/rmd_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #include "rmd_ros2_control/rmd_hardware_interface.hpp" #include @@ -30,41 +12,132 @@ #include #include #include +#include #include "hardware_interface/system_interface.hpp" #include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 0 // 0 for off 1 for on using std::placeholders::_1; namespace rmd_ros2_control { + +double RMDHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ + // Converts from 0.01 deg to deg to radians/s + return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; +} + +double RMDHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s + return (motor_velocity * (M_PI/180.0))/gear_ratio; +} + +int32_t RMDHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ + // radians -> deg -> 0.01 deg + return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); +} + +int32_t RMDHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s -> 0.01 deg/s + return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); +} + +void RMDHardwareInterface::send_command(int can_id, int cmd_id){ + CANLib::CanFrame frame; + frame.id = can_id; + frame.dlc = 8; + frame.data.fill(0); + frame.data[0] = cmd_id; + canBus.send(frame); +} + +void RMDHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nRMD Logger"; + std::string control_mode = ""; + + // HWI Specific + std::ostringstream oss; + + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + if(static_cast(control_level_[i]) == 1) { + control_mode = "POSITION"; + } + else if(static_cast(control_level_[i]) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Write CAN ID: 0x" << std::hex << std::uppercase << joint_node_write_ids[i] + << " | Read CAN ID: 0x" << std::hex << std::uppercase << joint_node_read_ids[i] + << " | Gear Ratio: " << joint_gear_ratios[i] + << " | Orientation: " << joint_orientation[i] << "\n" + << "-- Commands --\n" + << "Control Mode: " << control_mode << "\n" + << "Motor Position: " << motor_position[i] + << " | Joint Command Position: " << joint_command_position_[i] << "\n" + << "Motor Velocity: " << motor_velocity[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), log_msg.c_str()); +} + + hardware_interface::CallbackReturn RMDHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - - // This stores the can ids for each joint aka motor + // Setting Parameters for (auto& joint : info_.joints) { - joint_node_write_ids.push_back(std::stoi(joint.parameters.at("node_write_id"))); - joint_node_read_ids.push_back(std::stoi(joint.parameters.at("node_read_id"))); - joint_gear_ratios.push_back(std::stoi(joint.parameters.at("gear_ratio"))); - joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation"))); + int write_id = std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160); + int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); + joint_node_write_ids.push_back(write_id); + joint_node_read_ids.push_back(write_id+0x100); + + joint_gear_ratios.push_back(gear_ratio); + joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation")) == -1 ? -1 : 1); + operating_velocity = std::clamp( + std::stoi(joint.parameters.at("operating_velocity")), + 0, + static_cast(3.5 * gear_ratio) + ); + } num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); + can_interface = info_.hardware_parameters.at("can_interface"); + + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; // Initializes command and state interface values joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -73,18 +146,11 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_init( joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_command_velocity_.assign(num_joints, 0); - encoder_position.assign(num_joints, 0.0); motor_position.assign(num_joints, 0.0); motor_velocity.assign(num_joints, 0.0); control_level_.resize(num_joints, integration_level_t::POSITION); - current_joint = 0; - - for (size_t i = 0; i < joint_command_velocity_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); - } - return hardware_interface::CallbackReturn::SUCCESS; } @@ -134,7 +200,7 @@ RMDHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn RMDHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!canBus.open("can0", std::bind(&RMDHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + if (!canBus.open(can_interface, std::bind(&RMDHardwareInterface::on_can_message, this, std::placeholders::_1))) { RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -142,18 +208,23 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { +void RMDHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { can_rx_frame_ = frame; // Store the received frame for processing in read() std::string result; - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + std::vector data(8, 0x00); + double raw_motor_velocity = 0.0; + double raw_motor_position = 0.0; for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == joint_node_read_ids[i] && can_rx_frame_.data[0] == 0x9C){ + if(can_rx_frame_.id == joint_node_read_ids[i] && + (can_rx_frame_.data[0] == SPEED_CONTROL_CMD || + can_rx_frame_.data[0] == ABSOLUTE_POS_CONTROL_CMD || + can_rx_frame_.data[0] == MOTOR_STATUS_2_CMD) && + can_rx_frame_.data[1] != 0x00) { - // DECODING CAN MESSAGE FOR VELOCITY - data[0] = 0x9C; + // DECODING CAN MESSAGE (A2, A4, and 9C all share the same reply) data[1] = can_rx_frame_.data[1]; // Motor Temperature data[2] = can_rx_frame_.data[2]; // Torque low byte data[3] = can_rx_frame_.data[3]; // Torque high byte @@ -162,69 +233,31 @@ void RMDHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { data[6] = can_rx_frame_.data[6]; // encoder position low byte data[7] = can_rx_frame_.data[7]; // encoder position high byte - // ENCODER POSITION + // POSITION // uint16 -> int16 -> double (for calcs) - encoder_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); + motor_position[i] = static_cast(static_cast((data[7] << 8) | data[6])); - // SPEED + // VELOCITY // uint16 -> int16 -> double (for calcs) motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); - - // CALCULATING JOINT STATE - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - } - else if(can_rx_frame_.id == joint_node_read_ids[i] && can_rx_frame_.data[0] == 0x92){ - - // DECODING CAN MESSAGE FOR POSITION - data[0] = 0x92; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = can_rx_frame_.data[4]; // Multi-turn low byte - data[5] = can_rx_frame_.data[5]; - data[6] = can_rx_frame_.data[6]; - data[7] = can_rx_frame_.data[7]; // Multi-turn high byte - - // POSITION - // uint32 -> int32 -> double (for calcs) - motor_position[i] = static_cast(static_cast((data[7] << 24) | (data[6] << 16) | (data[5] << 8) | data[4])); - - if(DEBUG_MODE == 1) { - for(int j = 0; j < 8; j++){ - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Received at index %d: %d", j, can_rx_frame_.data[j]); - } - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Motor Position: %f", motor_position[i]); - } - - // CALCULATING JOINT STATE - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - } - else{ - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reply not heard."); + else { + if(logger_state == 1) { + // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reply not heard."); } } } - } hardware_interface::CallbackReturn RMDHardwareInterface::on_cleanup( const rclcpp_lifecycle::State & /*previous_state*/) { // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here - for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - // Motor Shutdown Command - can_tx_frame_.data = {0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + std::vector data(8, 0x00); - // Brake Lock Command (don't think this works) - can_tx_frame_.data = {0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + for(int i = 0; i < num_joints; i++){ + send_command(joint_node_write_ids[i], MOTOR_SHUTDOWN_CMD); // Motor Shutdown Command + send_command(joint_node_write_ids[i], BRAKE_LOCK_CMD); // Brake Lock Command (don't think this works) } canBus.close(); @@ -238,21 +271,12 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_activate( RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Activating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - // Brake Release command (pretty sure brakes don't work) - can_tx_frame_.data = {0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_write_ids[i], BRAKE_RELEASE_CMD); } // Sets initial command to joint state joint_command_position_ = joint_state_position_; - - for (size_t i = 0; i < joint_command_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); - } RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Successfully activated!"); @@ -266,17 +290,8 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Deactivating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - // Motor Stop Command - can_tx_frame_.data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - - // Brake Lock Command (don't think this works) - can_tx_frame_.data = {0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_write_ids[i], MOTOR_STOP_CMD); // Motor Stop Command + send_command(joint_node_write_ids[i], BRAKE_LOCK_CMD); // Brake Lock Command (don't think this works) } RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Successfully deactivated all RMD motors!"); @@ -284,156 +299,97 @@ hardware_interface::CallbackReturn RMDHardwareInterface::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -double RMDHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from 0.01 deg to deg to radians/s - return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; -} - -double RMDHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s - return (motor_velocity * (M_PI/180.0))/gear_ratio; -} - -int32_t RMDHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg -> 0.01 deg - return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); -} - -int32_t RMDHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s -> 0.01 deg/s - return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); -} - - hardware_interface::return_type RMDHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - std::lock_guard lock(state_mutex_); - - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - current_joint+=1; - current_joint = current_joint % num_joints; +{ + // CALCULATING JOINT STATE for(int i = 0; i < num_joints; i++) { - if(current_joint == i){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - // Command to read multi-turn angle - can_tx_frame_.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - - // Command to read motor status 2 - can_tx_frame_.data = {0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - - // // CALCULATING JOINT STATE - // joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - // joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - motor_position[i], - joint_state_position_[i], - joint_state_velocity_[i]); - } - } + joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); + joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); } - // // Values retrieved - // for(int i = 0; i < num_joints; i++){ - // if(current_joint == i){ - // // CALCULATING JOINT STATE - // joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - // joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - - // if(DEBUG_MODE == 1) { - // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \n", - // info_.joints[i].name.c_str(), - // motor_position[i], - // joint_state_position_[i], - // joint_state_velocity_[i]); - // } - // } - // } - return hardware_interface::return_type::OK; } hardware_interface::return_type rmd_ros2_control::RMDHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & time, const rclcpp::Duration & period) { - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; + int data[8] = {0x00}; int32_t joint_angle = 0; - int16_t operating_velocity = 200; int32_t joint_velocity = 0; + + elapsed_time+=period.seconds(); - for(int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated - can_tx_frame_.id = joint_node_write_ids[i]; - can_tx_frame_.dlc = 8; - - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - - // CALCULATE DESIRED JOINT ANGLE - joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d Joint command: %f", - info_.joints[i].name.c_str(), - joint_angle, - joint_command_position_[i]); - } - - // ENCODING CAN MESSAGE - data[0] = 0xA4; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; - + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + } + + + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(elapsed_update_time > update_period){ + elapsed_update_time = 0.0; + for(int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated + can_tx_frame_.id = joint_node_write_ids[i]; + can_tx_frame_.dlc = 8; - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); + // CALCULATE DESIRED JOINT ANGLE + joint_angle = joint_orientation[i]*calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + + // ENCODING CAN MESSAGE + data[0] = ABSOLUTE_POS_CONTROL_CMD; + data[1] = 0x00; + data[2] = operating_velocity & 0xFF; + data[3] = (operating_velocity >> 8) & 0xFF; + data[4] = joint_angle & 0xFF; + data[5] = (joint_angle >> 8) & 0xFF; + data[6] = (joint_angle >> 16) & 0xFF; + data[7] = (joint_angle >> 24) & 0xFF; + } + else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + + // CALCULATE DESIRED JOINT VELOCITY + joint_velocity = joint_orientation[i]*calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + + // ENCODING CAN MESSAGE + data[0] = SPEED_CONTROL_CMD; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = joint_velocity & 0xFF; + data[5] = (joint_velocity >> 8) & 0xFF; + data[6] = (joint_velocity >> 16) & 0xFF; + data[7] = (joint_velocity >> 24) & 0xFF; + } + else{ + // RCLCPP_WARN(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); + // ENCODING CAN MESSAGE + data[0] = MOTOR_STATUS_2_CMD; } - - // ENCODING CAN MESSAGE - data[0] = 0xA2; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - - } - else{ - // RCLCPP_INFO(rclcpp::get_logger("RMDHardwareInterface"), "Joint command value not found or undefined command state"); - } - // Cast data to uint8_t - for(int j = 0; j < 8; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; + // Cast data to uint8_t + for(int j = 0; j < 8; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; + } + canBus.send(can_tx_frame_); } - - canBus.send(can_tx_frame_); } - + return hardware_interface::return_type::OK; } diff --git a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp index a44628d6..d6e10772 100644 --- a/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp +++ b/src/hardware_interfaces/servo_ros2_control/include/servo_ros2_control/servo_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #ifndef SERVO_HARDWARE_INTERACE_HPP_ #define SERVO_HARDWARE_INTERACE_HPP_ @@ -38,7 +20,9 @@ #include #include #include -#include "msgs/msg/cana.hpp" + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" namespace servo_ros2_control { @@ -54,14 +38,15 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Exports/exposes Interfaces that are available so that the controllers // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - // Establish communications and set initial values for state and command interfaces - // hardware_interface::CallbackReturn on_configure( - // const rclcpp_lifecycle::State & previous_state) override; - // Lifecycle + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; @@ -82,9 +67,36 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; -private: + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); + + double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); + double calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg); + double calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); + double calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg); + + int16_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); + int16_t calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double meters_per_deg); + int16_t calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio); + int16_t calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg); + +private: + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + + // Keeps track of amount of joints int num_joints; + + // Stores Arbitration IDs + int can_command_id; + uint32_t can_response_id; // Store the state for the simulated robot std::vector joint_state_position_; @@ -93,33 +105,65 @@ class SERVOHardwareInterface : public hardware_interface::SystemInterface // Inh // Store the command for the simulated robot std::vector joint_command_position_; std::vector joint_command_velocity_; - - double encoder_position; - double motor_speed; - - rclcpp::Publisher::SharedPtr actuator_can_publisher_; - rclcpp::Subscription::SharedPtr actuator_can_subscriber_; - rclcpp::Node::SharedPtr node_; - - msgs::msg::CANA received_joint_data_; - + + // Place holders for data from the canBus, will be accessed in read() + std::vector motor_velocity; + std::vector motor_position; + std::vector rated_max; + std::vector meters_per_deg; + std::vector device_status; + + // CAN Library Setup + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + CANLib::CanFrame can_rx_frame_; + std::string can_interface; + + // Joint specific parameters std::vector joint_node_ids; std::vector joint_gear_ratios; - - enum integration_level_t : std::uint8_t + // Modes for control mode + enum class integration_level_t : std::uint8_t { UNDEFINED = 0, POSITION = 1, VELOCITY = 2, }; - // Active control mode for each servok + // Active control mode for each actuator std::vector control_level_; + enum class servo_type_t : std::uint8_t + { + STANDARD = 0, + CONTINUOUS = 1, + }; + + // Type of servo for each actuator + std::vector servo_type_; + + // Types of joints + enum class joint_type_t : std::uint8_t + { + REVOLUTE = 0, + PRISMATIC = 1, + }; + + // Type of joint for each actuator + std::vector joint_type_; + + // CAN Commands + static constexpr uint8_t PCB_HEARTBEAT_CMD = 0X10; + static constexpr uint8_t LED_STATUS_CMD = 0x11; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0x20; + static constexpr uint8_t VELOCITY_CONTROL_CMD = 0x30; + static constexpr uint8_t MOTOR_STATE_CMD = 0x4; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x5; + static constexpr uint8_t MAINTENANCE_CMD = 0x6; + static constexpr uint8_t SERVO_SPECS_CMD = 0x7; }; } // namespace servo_hardware_interface #endif // SERVO_HARDWARE_INTERACE_HPP_ - diff --git a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp index 1afbecd8..4a69414e 100644 --- a/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp +++ b/src/hardware_interfaces/servo_ros2_control/src/servo_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #include "servo_ros2_control/servo_hardware_interface.hpp" #include @@ -36,63 +18,210 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" +using std::placeholders::_1; + namespace servo_ros2_control { + +double SERVOHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ + // Converts from deg to radians with gear ratio + return (motor_position * (M_PI/180.0))/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_position, int gear_ratio, double meters_per_deg){ + // Converts from deg to meters with gear ratio + return (motor_position * meters_per_deg)/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s with gear ratio + return (motor_velocity * (M_PI/180.0))/gear_ratio; +} + +double SERVOHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_velocity, int gear_ratio, double meters_per_deg){ + // Converts from dps to meters/s with gear ratio + return (motor_velocity * meters_per_deg)/gear_ratio; +} + +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ + // radians -> deg with gear ratio + return static_cast(std::round((joint_position*(180/M_PI))*gear_ratio)); +} + +int16_t SERVOHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double meters_per_deg){ + // m -> deg with gear ratio + return static_cast(std::round((joint_position*meters_per_deg*gear_ratio))); +} + +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity*(180/M_PI))*gear_ratio)); +} + +int16_t SERVOHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double meters_per_deg){ + // m/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity*meters_per_deg*gear_ratio))); +} + +void SERVOHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nSERVO Logger"; + std::ostringstream oss; + std::string status; + + // HWI Specific + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | Command CAN ID: 0x" << std::hex << std::uppercase << can_command_id + << " | Response CAN ID: 0x" << std::hex << std::uppercase << can_response_id + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + switch (device_status[i]) { + case 0x00: + status = "IDLE (ready)"; + break; + case 0x01: + status = "ACTIVE (busy)"; + break; + case 0x02: + status = "DOES NOT EXIST"; + break; + case 0x03: + status = "ERROR"; + break; + default: + status = "UNDEFINED"; + break; + } + + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] + << " | Gear Ratio: " << joint_gear_ratios[i] + << " | Device Status: " << std::hex << std::uppercase << device_status[i] + << " - " << status << "\n" + << "-- Commands --\n" + << "Control Mode: " << static_cast(control_level_[i]) << "\n" + << "Motor Position: " << motor_position[i] + << " | Joint Command Position: " << joint_command_position_[i] << "\n" + << "Motor Velocity: " << motor_velocity[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), log_msg.c_str()); +} + + hardware_interface::CallbackReturn SERVOHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS){ return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - - // This stores the can ids for each joint aka motor - /* for (auto& joint : info_.joints) { - joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); - } */ + // -- Parameters -- + for (auto& joint : info_.joints) { + joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); + joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); + + std::string servo_type = joint.parameters.at("servo_type"); + std::string joint_type = joint.parameters.at("joint_type"); + + // Standard or Continuous + if (servo_type == "standard" && joint.parameters.count("max_pos")) { + servo_type_.push_back(servo_type_t::STANDARD); + } else if (servo_type == "continuous" && joint.parameters.count("max_vel")) { + servo_type_.push_back(servo_type_t::CONTINUOUS); + } else { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid servo_type parameter for joint %s. Must be 'standard' with 'max_pos' or 'continuous' with 'max_vel'.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + // Revolute or Prismatic + if (joint_type == "revolute") { + joint_type_.push_back(joint_type_t::REVOLUTE); + meters_per_deg.push_back(std::nan("")); // Not used for revolute joints + } else if (joint_type == "prismatic" && joint.parameters.count("meters_per_deg")) { + joint_type_.push_back(joint_type_t::PRISMATIC); + meters_per_deg.push_back(std::abs(std::stod(joint.parameters.at("meters_per_deg")))); + } else { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'meters_per_deg' parameter.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + // Determining rated max based on joint and servo type + if (joint_type_.back() == joint_type_t::REVOLUTE && servo_type_.back() == servo_type_t::STANDARD) { + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_pos")))*(M_PI/180.0)); // Convert from degrees to radians + } else if(joint_type_.back() == joint_type_t::REVOLUTE && servo_type_.back() == servo_type_t::CONTINUOUS){ + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_vel")))*(M_PI/180.0)); // Convert from dps to radians/s + } else if(joint_type_.back() == joint_type_t::PRISMATIC && servo_type_.back() == servo_type_t::STANDARD){ + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_pos")) * std::stod(joint.parameters.at("meters_per_deg")))); // Convert from degrees to meters + } else if(joint_type_.back() == joint_type_t::PRISMATIC && servo_type_.back() == servo_type_t::CONTINUOUS){ + rated_max.push_back(std::abs(std::stod(joint.parameters.at("max_vel")) * std::stod(joint.parameters.at("meters_per_deg")))); // Convert from dps to meters/s + } + else { + RCLCPP_ERROR(rclcpp::get_logger("SERVOHardwareInterface"), "Invalid combination of joint_type and servo_type for joint %s.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + } num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); + can_interface = info_.hardware_parameters.at("can_interface"); + can_command_id = std::stoi(info_.hardware_parameters.at("can_id"), nullptr, 0); + can_response_id = can_command_id+0x01; + + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; - // Initializes command and state interface values - joint_state_position_.assign(num_joints, 0); + // -- Command and State Interface initialization -- + joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_state_velocity_.assign(num_joints, 0); - joint_command_position_.assign(num_joints, 0); + joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); joint_command_velocity_.assign(num_joints, 0); - encoder_position = 0; - motor_speed = 0; - - control_level_.resize(num_joints, integration_level_t::UNDEFINED); - - node_ = rclcpp::Node::make_shared("servo_hardware_node"); - actuator_can_publisher_ = node_->create_publisher("can_tx", 10); - - // Lambda function that takes the message as a shared pointer, dereferences it, - // and stores it in received_joint_data_ to be used - actuator_can_subscriber_ = node_->create_subscription( - "can_rx", - 10, - [this](const msgs::msg::CANA::SharedPtr received_message) - { - received_joint_data_ = *received_message; - }); + motor_position.assign(num_joints, 0.0); + motor_velocity.assign(num_joints, 0.0); + device_status.assign(num_joints, 0); + + // Set initial control type based on servo type + control_level_.resize(num_joints, integration_level_t::POSITION); + for (int i = 0; i < num_joints; i++) { + if(servo_type_[i] == servo_type_t::STANDARD){ + control_level_[i] = integration_level_t::POSITION; + } else if(servo_type_[i] == servo_type_t::CONTINUOUS){ + control_level_[i] = integration_level_t::VELOCITY; + } else{ + control_level_[i] = integration_level_t::UNDEFINED; + } + } return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn SERVOHardwareInterface::on_shutdown( - const rclcpp_lifecycle::State & /*previous_state*/) + const rclcpp_lifecycle::State & previous_state) { - // may not need this either - return hardware_interface::CallbackReturn::SUCCESS; + // Reuse cleanup logic which shuts down the motor and then deinitializes shared pointers. + // Need this in case on_cleanup never gets called + return this->on_cleanup(previous_state); } @@ -129,12 +258,111 @@ SERVOHardwareInterface::export_command_interfaces() return command_interfaces; } +hardware_interface::CallbackReturn SERVOHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + if (!canBus.open(can_interface, std::bind(&SERVOHardwareInterface::on_can_message, this, std::placeholders::_1))) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Failed to open CAN interface"); + return hardware_interface::CallbackReturn::ERROR; + } + + return hardware_interface::CallbackReturn::SUCCESS; +} + +void SERVOHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { + can_rx_frame_ = frame; // Store the received frame for processing in read() + + std::string result; + + int data[7] = {0x00}; + int8_t command_nibble = ((can_rx_frame_.data[0]>>4) & 0x0F); + int8_t device_id_nibble = (can_rx_frame_.data[0] & 0x0F); + double raw_motor_position = 0.0; + double raw_motor_velocity = 0.0; + + for(int i = 0; i < num_joints; i++){ + if(can_rx_frame_.id == can_response_id && command_nibble == MOTOR_STATE_CMD && device_id_nibble == joint_node_ids[i]){ + + // DECODING CAN MESSAGE FOR VELOCITY + data[1] = can_rx_frame_.data[1]; // Position low byte + data[2] = can_rx_frame_.data[2]; // Position high byte + data[3] = can_rx_frame_.data[3]; // Velocity low byte + data[4] = can_rx_frame_.data[4]; // Velocity high byte + + // POSITION + // uint32 -> int16 -> double (for calcs) + raw_motor_position = static_cast(static_cast((data[2] << 8) | data[1])); + + // VELOCITY + // uint16 -> int16 -> double (for calcs) + raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); + + // CALCULATING JOINT STATE + if(joint_type_[i] == joint_type_t::REVOLUTE){ + motor_position[i] = calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); // radians + motor_velocity[i] = calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); // radians/s + } + else if(joint_type_[i] == joint_type_t::PRISMATIC){ + motor_position[i] = calculate_joint_displacement_from_motor_position(raw_motor_position, joint_gear_ratios[i], meters_per_deg[i]); // meters + motor_velocity[i] = calculate_joint_linear_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i], meters_per_deg[i]); // meters/s + } + else{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + } + else if(can_rx_frame_.id == can_response_id && command_nibble == MOTOR_STATUS_CMD && device_id_nibble == joint_node_ids[i]){ + + // Populate device status + device_status[i] = can_rx_frame_.data[1]; + } + else{ + if(logger_state == 1) { + // RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Reply not heard."); + } + } + } + +} + +hardware_interface::CallbackReturn SERVOHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up ...please wait..."); + + // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here + int8_t command_nibble = MAINTENANCE_CMD; + int8_t maintenance_command = 2; // Motor Shutdown Command + int8_t device_id_nibble; + for(int i = 0; i < num_joints; i++){ + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((command_nibble << 4) | device_id_nibble); + can_tx_frame_.data[1] = maintenance_command; + canBus.send(can_tx_frame_); + } + + // Close CAN bus + canBus.close(); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Cleaning up successful!"); + + return hardware_interface::CallbackReturn::SUCCESS; +} hardware_interface::CallbackReturn SERVOHardwareInterface::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Activating ...please wait..."); + // Sets initial command to joint state + joint_command_position_ = joint_state_position_; + + for (size_t i = 0; i < joint_command_position_.size(); ++i) { + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); + } + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; @@ -144,8 +372,22 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_activate( hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - auto joint_tx = msgs::msg::CANA(); RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Deactivating ...please wait..."); + int8_t command_nibble = MAINTENANCE_CMD; + int8_t maintenance_command = 1; // Motor Stop Command + int8_t device_id_nibble; + + for(int i = 0; i < num_joints; i++){ + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + + // Motor Stop Command + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((command_nibble << 4) | device_id_nibble); + can_tx_frame_.data[1] = maintenance_command; + canBus.send(can_tx_frame_); + } RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "Successfully deactivated all SERVO motors!"); @@ -156,13 +398,135 @@ hardware_interface::CallbackReturn SERVOHardwareInterface::on_deactivate( hardware_interface::return_type SERVOHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + for(int i = 0; i < num_joints; i++) { + // CALCULATING JOINT STATE + joint_state_velocity_[i] = motor_velocity[i]; + joint_state_position_[i] = motor_position[i]; + + // ACCOUNTING FOR DEVICE STATUS + if(device_status[i] != 0x00 && device_status[i] != 0x01 && device_status[i] != -1){ + return hardware_interface::return_type::ERROR; + } + } + return hardware_interface::return_type::OK; } hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; + int data[3] = {0x00}; + int8_t command_nibble; + int8_t device_id_nibble; + int16_t joint_angle = 0; + int16_t joint_velocity = 0; + + elapsed_time+=period.seconds(); + + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } + + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(elapsed_update_time > update_period){ + elapsed_update_time = 0.0; + + // Motor Command for each joint at given frequency + for(int i = 0; i < num_joints; i++) { + // Motor Command for each joint at given frequency + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 3; + + if(control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::STANDARD && std::isfinite(joint_command_position_[i])) { + // CALCULATE DESIRED JOINT ANGLE + joint_angle = std::clamp(joint_command_position_[i], 0.0, rated_max[i]); // Input must be within bounds of rated max (positive and either rad or m) + if(joint_type_[i] == joint_type_t::REVOLUTE){ + joint_angle = calculate_motor_position_from_desired_joint_position(joint_angle, joint_gear_ratios[i]); + } + else if(joint_type_[i] == joint_type_t::PRISMATIC){ + joint_angle = calculate_motor_position_from_desired_joint_displacement(joint_angle, joint_gear_ratios[i], meters_per_deg[i]); + } + else{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + + // ENCODING CAN MESSAGE + data[0] = ABSOLUTE_POS_CONTROL_CMD + joint_node_ids[i]; + data[1] = joint_angle & 0xFF; + data[2] = (joint_angle >> 8) & 0xFF; + + // Cast data to uint8_t + for(int j = 0; j < 3; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; + } + + } + else if(control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::CONTINUOUS && std::isfinite(joint_command_velocity_[i])) { + + // CALCULATE DESIRED JOINT VELOCITY + joint_velocity = std::clamp(joint_command_velocity_[i], -rated_max[i], rated_max[i]); // Input must be within bounds of rated max (all real values and either rad or m) + if(joint_type_[i] == joint_type_t::REVOLUTE){ + joint_velocity = calculate_motor_velocity_from_desired_joint_angular_velocity(joint_velocity, joint_gear_ratios[i]); + } + else if(joint_type_[i] == joint_type_t::PRISMATIC){ + joint_velocity = calculate_motor_velocity_from_desired_joint_linear_velocity(joint_velocity, joint_gear_ratios[i], meters_per_deg[i]); + } + else{ + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + + // ENCODING CAN MESSAGE + data[0] = VELOCITY_CONTROL_CMD + joint_node_ids[i]; + data[1] = joint_velocity & 0xFF; + data[2] = (joint_velocity >> 8) & 0xFF; + + // Cast data to uint8_t + for(int j = 0; j < 3; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; + } + + } + else { + command_nibble = MOTOR_STATE_CMD; // Motor State Command for each joint at given frequency + can_tx_frame_.dlc = 1; + device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data = { static_cast((command_nibble << 4) | device_id_nibble) }; + + if (control_level_[i] == integration_level_t::POSITION && servo_type_[i] == servo_type_t::CONTINUOUS) { + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is continuous type and cannot be position controlled.", info_.joints[i].name.c_str()); + } + } + else if (control_level_[i] == integration_level_t::VELOCITY && servo_type_[i] == servo_type_t::STANDARD) { + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); + } + } + else{ + if (logger_state == 1) { + RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state"); + } + } + } + + canBus.send(can_tx_frame_); + } + } + return hardware_interface::return_type::OK; } @@ -177,29 +541,28 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi ss << "] stop_interfaces: ["; for (auto &s : stop_interfaces) ss << s << ","; ss << "]"; - RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), "%s", ss.str().c_str()); + RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), ss.str().c_str()); // For each joint, decide its new control mode based on start/stop interfaces. + // We allow partial starts/stops: only affected joints are switched. std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); - // -------- STOP INTERFACES -------- + // Process stop interfaces first: mark those joints as UNDEFINED for (const auto &ifname : stop_interfaces) { for (int i = 0; i < num_joints; ++i) { const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); - if (ifname == pos_if || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { requested_modes[i] = integration_level_t::UNDEFINED; } } } - // -------- START INTERFACES -------- + // Process start interfaces: set POSITION or VELOCITY per joint for (const auto &ifname : start_interfaces) { for (int i = 0; i < num_joints; ++i) { const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); - if (ifname == pos_if) { requested_modes[i] = integration_level_t::POSITION; } else if (ifname == vel_if) { @@ -208,10 +571,12 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi } } - // -------- APPLY MODES -------- + // Now apply the requested_modes to control_level_. + // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. for (int i = 0; i < num_joints; ++i) { - if (requested_modes[i] == integration_level_t::UNDEFINED) { + // if stop requested, set to UNDEFINED; otherwise leave existing mode + // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) bool was_stopped = false; for (const auto &ifname : stop_interfaces) { if (ifname.find(info_.joints[i].name) != std::string::npos) { @@ -219,29 +584,29 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi break; } } - if (was_stopped) { control_level_[i] = integration_level_t::UNDEFINED; joint_command_velocity_[i] = 0; + // optional: reset position cmd to current state to be safe joint_command_position_[i] = joint_state_position_[i]; RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: stopped -> UNDEFINED", - info_.joints[i].name.c_str()); + "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); } - + // else, leave control_level_ as-is } else { - + // Set the mode requested control_level_[i] = requested_modes[i]; - + // If switching to velocity, optionally set command velocity to current state to avoid jumps if (requested_modes[i] == integration_level_t::VELOCITY) { - joint_command_velocity_[i] = 0.0; + // joint_command_velocity_[i] = joint_state_velocity_[i]; + joint_command_velocity_[i] = 0; RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: switched to VELOCITY", - info_.joints[i].name.c_str()); + "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", + info_.joints[i].name.c_str(), joint_command_velocity_[i]); } else if (requested_modes[i] == integration_level_t::POSITION) { joint_command_position_[i] = joint_state_position_[i]; RCLCPP_INFO(rclcpp::get_logger("SERVOHardwareInterface"), - "Joint %s: switched to POSITION (init pos=%f)", + "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", info_.joints[i].name.c_str(), joint_command_position_[i]); } } @@ -256,4 +621,4 @@ hardware_interface::return_type SERVOHardwareInterface::perform_command_mode_swi #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - servo_ros2_control::SERVOHardwareInterface, hardware_interface::SystemInterface) + servo_ros2_control::SERVOHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file diff --git a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp index 70920c85..57247583 100644 --- a/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp +++ b/src/hardware_interfaces/smc_ros2_control/include/smc_ros2_control/smc_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #ifndef SMC_HARDWARE_INTERACE_HPP_ #define SMC_HARDWARE_INTERACE_HPP_ @@ -38,6 +20,7 @@ #include #include #include + #include "umdloop_can_library/SocketCanBus.hpp" #include "umdloop_can_library/CanFrame.hpp" @@ -55,10 +38,9 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Exports/exposes Interfaces that are available so that the controllers // know what to read and write to std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; - // Lifecycle + // -- Lifecycle Functions -- hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -85,17 +67,26 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; - void onCanMessage(const CANLib::CanFrame& frame); - - // Helper Functions + + // -- Helper Functions -- + void send_command(int can_id, int cmd_id); + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); double calculate_joint_position_from_motor_position(double motor_position, int gear_ratio); double calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio); - int32_t calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio); int32_t calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio); private: - + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + + // Keeps track of amount of joints int num_joints; // Store the state for the simulated robot @@ -105,21 +96,27 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Store the command for the simulated robot std::vector joint_command_position_; std::vector joint_command_velocity_; - + + // Place holders for data from the canBus, will be accessed in read() std::vector encoder_position; std::vector motor_velocity; std::vector motor_position; + + // Velocity at which **joint** rotates to reach position in 1 dps + uint16_t operating_velocity; - std::vector joint_initialization_; - + // CAN Library Setup CANLib::SocketCanBus canBus; CANLib::CanFrame can_tx_frame_; CANLib::CanFrame can_rx_frame_; + std::string can_interface; + // Joint specific parameters std::vector joint_node_ids; std::vector joint_gear_ratios; - - + std::vector joint_initialization_; + + // Modes for control mode enum integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -130,6 +127,13 @@ class SMCHardwareInterface : public hardware_interface::SystemInterface // Inher // Active control mode for each actuator std::vector control_level_; + // CAN Commands + static constexpr uint8_t MOTOR_SHUTDOWN_CMD = 0X80; + static constexpr uint8_t MOTOR_STOP_CMD = 0x81; + static constexpr uint8_t MOTOR_RUNNING_CMD = 0x88; + static constexpr uint8_t MOTOR_STATUS_2_CMD = 0X9C; + static constexpr uint8_t SPEED_CONTROL_CMD = 0xA2; + static constexpr uint8_t ABSOLUTE_POS_CONTROL_CMD = 0xA4; }; } // namespace smc_hardware_interface diff --git a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp index 5e204761..1021bd37 100644 --- a/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp +++ b/src/hardware_interfaces/smc_ros2_control/src/smc_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #include "smc_ros2_control/smc_hardware_interface.hpp" #include @@ -30,37 +12,118 @@ #include #include #include +#include #include "hardware_interface/system_interface.hpp" #include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" -#define DEBUG_MODE 0 // 0 for off 1 for on - namespace smc_ros2_control { + +double SMCHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ + // Converts from 0.01 deg to deg to radians/s + return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; +} + +double SMCHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ + // Converts from dps to radians/s + return (motor_velocity * (M_PI/180.0))/gear_ratio; +} + +int32_t SMCHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ + // radians -> deg -> 0.01 deg + return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); +} + +int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ + // radians/s -> deg/s -> 0.01 deg/s + return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); +} + +void SMCHardwareInterface::send_command(int can_id, int cmd_id){ + CANLib::CanFrame frame; + frame.id = can_id; + frame.dlc = 8; + frame.data.fill(0); + frame.data[0] = cmd_id; + canBus.send(frame); +} + +void SMCHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nSMC Logger"; + std::string control_mode = ""; + + // HWI Specific + std::ostringstream oss; + + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + if(static_cast(control_level_[i]) == 1) { + control_mode = "POSITION"; + } + else if(static_cast(control_level_[i]) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: CAN ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] + << " | Gear Ratio: " << joint_gear_ratios[i] << "\n" + << "-- Commands --\n" + << "Control Mode: " << control_mode << "\n" + << "Motor Position: " << motor_position[i] + << " | Joint Command Position: " << joint_command_position_[i] << "\n" + << "Motor Velocity: " << motor_velocity[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), log_msg.c_str()); +} + + hardware_interface::CallbackReturn SMCHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) - { + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! - */ - - // This stores the can ids for each joint aka motor + // Setting Parameters for (auto& joint : info_.joints) { - joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); - joint_gear_ratios.push_back(std::stoi(joint.parameters.at("gear_ratio"))); + int gear_ratio = std::abs(std::stoi(joint.parameters.at("gear_ratio"))); + joint_node_ids.push_back(std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x141, 0x160)); + joint_gear_ratios.push_back(gear_ratio); + operating_velocity = std::clamp(std::stoi(joint.parameters.at("operating_velocity")), 0, 65*gear_ratio); } num_joints = static_cast(info_.joints.size()); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); + can_interface = info_.hardware_parameters.at("can_interface"); + + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; // Initializes command and state interface values joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); @@ -124,7 +187,7 @@ SMCHardwareInterface::export_command_interfaces() hardware_interface::CallbackReturn SMCHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - if (!canBus.open("can0", std::bind(&SMCHardwareInterface::onCanMessage, this, std::placeholders::_1))) { + if (!canBus.open(can_interface, std::bind(&SMCHardwareInterface::on_can_message, this, std::placeholders::_1))) { RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Failed to open CAN interface"); return hardware_interface::CallbackReturn::ERROR; } @@ -132,18 +195,21 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_configure( return hardware_interface::CallbackReturn::SUCCESS; } -void SMCHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { +void SMCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) { can_rx_frame_ = frame; // Store the received frame for processing in read() std::string result; - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + int data[8] = {0x00}; -for(int i = 0; i < num_joints; i++){ - if(can_rx_frame_.id == joint_node_ids[i] && can_rx_frame_.data[0] == 0x9C){ + for(int i = 0; i < num_joints; i++){ + if(can_rx_frame_.id == joint_node_ids[i] && + (can_rx_frame_.data[0] == SPEED_CONTROL_CMD || + can_rx_frame_.data[0] == ABSOLUTE_POS_CONTROL_CMD || + can_rx_frame_.data[0] == MOTOR_STATUS_2_CMD) && + can_rx_frame_.data[1] != 0x00){ // DECODING CAN MESSAGE FOR VELOCITY - data[0] = 0x9C; data[1] = can_rx_frame_.data[1]; // Motor Temperature data[2] = can_rx_frame_.data[2]; // Torque low byte data[3] = can_rx_frame_.data[3]; // Torque high byte @@ -159,10 +225,6 @@ for(int i = 0; i < num_joints; i++){ // SPEED // uint16 -> int16 -> double (for calcs) motor_velocity[i] = static_cast(static_cast((data[5] << 8) | data[4])); - - //CALCULATING JOINT STATE - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); - } else if(can_rx_frame_.id == joint_node_ids[i] && can_rx_frame_.data[0] == 0x92){ data[0] = 0x92; @@ -175,7 +237,6 @@ for(int i = 0; i < num_joints; i++){ data[7] = can_rx_frame_.data[7]; // Multi-turn high byte // POSITION - // TODO: This sign extension may only work when the value is negative, look into it // In this case, we have 56 bits to store in 64, so we must have a sign extension // uint64 -> int64 (to do sign extension) -> sign extension (left shift 8, then arithmetic right shift 8) --> double (for calcs) uint64_t motor_position_raw = (static_cast(data[7]) << 48) | @@ -186,32 +247,13 @@ for(int i = 0; i < num_joints; i++){ (static_cast(data[2]) << 8) | static_cast(data[1]); - motor_position[i] = static_cast((static_cast(motor_position_raw) << 8) >> 8); - - if(DEBUG_MODE == 1) { - for(int j = 0; j < 8; j++){ - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Received at index %d: %d", j, can_rx_frame_.data[j]); - } - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Motor Position: %f", motor_position[i]); - } - - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - + motor_position[i] = static_cast((static_cast(motor_position_raw) << 8) >> 8); } else{ - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reply not heard."); + if(logger_state == 1) { + // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reply not heard."); } } - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reading for joint: %s Joint position: %f Joint velocity: %f \nJoint Initialization State: %d \n", - info_.joints[i].name.c_str(), - joint_state_position_[i], - joint_state_velocity_[i], - static_cast(joint_initialization_[i])); - } - } } @@ -221,13 +263,8 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_cleanup( { // If cleanup occurs before shutdown, this is the last opportunity to shutdown motor since pointers must be deleted here for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - // Motor Off (Shutdown) Command - can_tx_frame_.data = {0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_ids[i], MOTOR_SHUTDOWN_CMD); } canBus.close(); @@ -242,22 +279,11 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_activate( RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Activating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - - can_tx_frame_.data = {0x88, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - control_level_[i] = integration_level_t::UNDEFINED; + send_command(joint_node_ids[i], MOTOR_RUNNING_CMD); } // Sets initial command to joint state - // joint_command_position_ = joint_state_position_; - for (size_t i = 0; i < joint_state_velocity_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_state_velocity_[i]); - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint %zu joint command in on_activate: %f", i, joint_command_position_[i]); - } + joint_command_position_ = joint_state_position_; RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Successfully activated!"); @@ -271,13 +297,7 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Deactivating ...please wait..."); for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - // Motor Stop Command - can_tx_frame_.data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); + send_command(joint_node_ids[i], MOTOR_STOP_CMD); } RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Successfully deactivated all SMC motors!"); @@ -285,132 +305,96 @@ hardware_interface::CallbackReturn SMCHardwareInterface::on_deactivate( return hardware_interface::CallbackReturn::SUCCESS; } -double SMCHardwareInterface::calculate_joint_position_from_motor_position(double motor_position, int gear_ratio){ - // Converts from 0.01 deg to deg to radians/s - return (motor_position * 0.01 * (M_PI/180.0))/gear_ratio; -} - -double SMCHardwareInterface::calculate_joint_velocity_from_motor_velocity(double motor_velocity, int gear_ratio){ - // Converts from dps to radians/s - return (motor_velocity * (M_PI/180.0))/gear_ratio; -} - -int32_t SMCHardwareInterface::calculate_motor_position_from_desired_joint_position(double joint_position, int gear_ratio){ - // radians -> deg -> 0.01 deg - return static_cast(std::round((joint_position*(180/M_PI)*100)*gear_ratio)); -} - -int32_t SMCHardwareInterface::calculate_motor_velocity_from_desired_joint_velocity(double joint_velocity, int gear_ratio){ - // radians/s -> deg/s -> 0.01 deg/s - return static_cast(std::round((joint_velocity*(180/M_PI)*100)*gear_ratio)); -} hardware_interface::return_type SMCHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - - // Since you can't publish two messages in one sweep without a delay, every other iteration of read - // will call angle or velocity messages - for(int i = 0; i < num_joints; i++){ - can_tx_frame_ = CANLib::CanFrame(); - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - // Command to read multi-turn angle - can_tx_frame_.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - - // Command to read motor status 2 - can_tx_frame_.data = {0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - canBus.send(can_tx_frame_); - } - - // Values retrieved + //CALCULATING JOINT STATE for(int i = 0; i < num_joints; i++){ - - //CALCULATING JOINT STATE joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity[i], joint_gear_ratios[i]); joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Reading for joint: %s Joint position: %f Joint velocity: %f \n", - info_.joints[i].name.c_str(), - joint_state_position_[i], - joint_state_velocity_[i]); - } } + return hardware_interface::return_type::OK; } hardware_interface::return_type smc_ros2_control::SMCHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; + int data[8] = {0x00}; int32_t joint_angle = 0; int16_t operating_velocity = 200; int32_t joint_velocity = 0; - - for(int i = 0; i < num_joints; i++) { - can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated - can_tx_frame_.id = joint_node_ids[i]; - can_tx_frame_.dlc = 8; - - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { - - // CALCULATE DESIRED JOINT ANGLE - joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing positions for: %s Joint angle of motor (0.01 deg): %d", - info_.joints[i].name.c_str(), - joint_angle); - } - - // ENCODING CAN MESSAGE - data[0] = 0xA4; - data[1] = 0x00; - data[2] = operating_velocity & 0xFF; - data[3] = (operating_velocity >> 8) & 0xFF; - data[4] = joint_angle & 0xFF; - data[5] = (joint_angle >> 8) & 0xFF; - data[6] = (joint_angle >> 16) & 0xFF; - data[7] = (joint_angle >> 24) & 0xFF; + + elapsed_time+=period.seconds(); + + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); } - else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + } - // CALCULATE DESIRED JOINT VELOCITY - joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d", - info_.joints[i].name.c_str(), - joint_velocity); + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(elapsed_update_time > update_period){ + elapsed_update_time = 0.0; + for(int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); // Must reinstantiate else data from past iteration gets repeated + can_tx_frame_.id = joint_node_ids[i]; + can_tx_frame_.dlc = 8; + + if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { + + // CALCULATE DESIRED JOINT ANGLE + joint_angle = calculate_motor_position_from_desired_joint_position(joint_command_position_[i], joint_gear_ratios[i]); + + // ENCODING CAN MESSAGE + data[0] = ABSOLUTE_POS_CONTROL_CMD; + data[1] = 0x00; + data[2] = operating_velocity & 0xFF; + data[3] = (operating_velocity >> 8) & 0xFF; + data[4] = joint_angle & 0xFF; + data[5] = (joint_angle >> 8) & 0xFF; + data[6] = (joint_angle >> 16) & 0xFF; + data[7] = (joint_angle >> 24) & 0xFF; } - - // ENCODING CAN MESSAGE - data[0] = 0xA2; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = joint_velocity & 0xFF; - data[5] = (joint_velocity >> 8) & 0xFF; - data[6] = (joint_velocity >> 16) & 0xFF; - data[7] = (joint_velocity >> 24) & 0xFF; - - } - else{ - // RCLCPP_INFO(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state"); - } + else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { - // Cast data to uint8_t - for(int j = 0; j < 8; j++){ - data[j] = static_cast(data[j]); - can_tx_frame_.data[j] = data[j]; + // CALCULATE DESIRED JOINT VELOCITY + joint_velocity = calculate_motor_velocity_from_desired_joint_velocity(joint_command_velocity_[i], joint_gear_ratios[i]); + + // ENCODING CAN MESSAGE + data[0] = SPEED_CONTROL_CMD; + data[1] = 0x00; + data[2] = 0x00; + data[3] = 0x00; + data[4] = joint_velocity & 0xFF; + data[5] = (joint_velocity >> 8) & 0xFF; + data[6] = (joint_velocity >> 16) & 0xFF; + data[7] = (joint_velocity >> 24) & 0xFF; + } + else{ + // RCLCPP_WARN(rclcpp::get_logger("SMCHardwareInterface"), "Joint command value not found or undefined command state. Sending Motor Status 3 commands for now."); + // ENCODING CAN MESSAGE + data[0] = MOTOR_STATUS_2_CMD; + } + + // Cast data to uint8_t + for(int j = 0; j < 8; j++){ + data[j] = static_cast(data[j]); + can_tx_frame_.data[j] = data[j]; + } + canBus.send(can_tx_frame_); } - - canBus.send(can_tx_frame_); } return hardware_interface::return_type::OK; diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp index 13031734..188bfd52 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.cpp @@ -101,18 +101,16 @@ float getClawVelocity(TalonSRX *motor) { } void setDutyCycle(TalonSRX *motor, double dutyCycle, int ms) { - // printf("Spin command received...\n\r"); motor->Set(ctre::phoenix::motorcontrol::ControlMode::PercentOutput, dutyCycle); ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds } -float setVelocity(TalonSRX *motor, double claw_vel, int ms) { +float setVelocityFromLinearVelocity(TalonSRX *motor, double claw_vel, int ms) { // float desired_rpm = joy_input * MAX_RPM; // float desired_rev_per_100ms = desired_rpm / 600; // 600 100ms to 1 minute //TO DO: why does the 5 fix this. I dont have a clue rn float desired_talon_units_per_100ms = convertRevtoTalonUnits(convertDistanceToRev((claw_vel*5)/10)); // m/s -> m/100ms -> rev/100ms -> talon/100ms - // printf("Spin command received...\n\r"); motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, desired_talon_units_per_100ms); ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds @@ -120,12 +118,29 @@ float setVelocity(TalonSRX *motor, double claw_vel, int ms) { return desired_talon_units_per_100ms; } -float setPosition(TalonSRX *motor, double claw_dist, int ms) { +float setVelocityFromAngularVelocity(TalonSRX *motor, double joint_velocity, int ms) { + float desired_talon_units_per_100ms = convertRevtoTalonUnits(joint_velocity / (2*M_PI*10)); // rad/s -> rad/100ms -> rev/100ms -> talon/100ms + + motor->Set(ctre::phoenix::motorcontrol::ControlMode::Velocity, desired_talon_units_per_100ms); + ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds + + return desired_talon_units_per_100ms; +} + +float setPositionFromDisplacement(TalonSRX *motor, double claw_displacement, int ms) { // float length_range = LENGTH_RANGE * 2.54; // in cm // float desired_pos = (joy_input * length_range) / DISTANCE_PER_REV; // in rev // float desired_talon_units = convertRevtoTalonUnits(desired_pos); - float desired_talon_units = convertRevtoTalonUnits(convertDistanceToRev(claw_dist)); // m -> rev -> talon units + float desired_talon_units = convertRevtoTalonUnits(convertDistanceToRev(claw_displacement)); // m -> rev -> talon units + motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, desired_talon_units); + ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds + + return desired_talon_units; +} + +float setPositionFromJointCommand(TalonSRX *motor, double joint_position, int ms) { + float desired_talon_units = convertRevtoTalonUnits(joint_position / (2*M_PI)); // rad -> rev -> talon units motor->Set(ctre::phoenix::motorcontrol::ControlMode::Position, desired_talon_units); ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds @@ -139,14 +154,14 @@ void stopMotor(TalonSRX *motor, int ms){ } // float runMotor(TalonSRX *motor, double speed, int ms) { -// float velocity = setVelocity(motor, speed); // motor receives talon units/100ms, function returns rev/s +// float velocity = setVelocityFromLinearVelocity(motor, speed); // motor receives talon units/100ms, function returns rev/s // // setDutyCycle(motor, speed); // set percent output and direction // ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds // return velocity; // } // float setMotorPosition(TalonSRX *motor, double joy_input, int ms) { -// float position = setPosition(motor, joy_input); // in Talon Units +// float position = setPositionFromDisplacement(motor, joy_input); // in Talon Units // ctre::phoenix::unmanaged::Unmanaged::FeedEnable(ms); // activate motor for (ms) milliseconds // return position; // } diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h index e6a48403..6bc28e8f 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_control.h @@ -25,8 +25,10 @@ float getClawVelocity(TalonSRX *); // Retrieves Velocity of claw in m/s // Setting States void setDutyCycle(TalonSRX *, double, int); // Sets duty cycle, aka percent of max speed basically -float setVelocity(TalonSRX *, double, int); // Sets velocity (units per 100ms) based on input velocity (mm/s), returns this as well -float setPosition(TalonSRX *, double, int); // Sets position (Talon units) based on input position (cm), returns this as well +float setVelocityFromLinearVelocity(TalonSRX *, double, int); // Sets velocity (units per 100ms) based on input velocity (mm/s), returns this as well +float setVelocityFromAngularVelocity(TalonSRX *, double, int); // Sets velocity (units per 100ms) based on input velocity (radians/s), returns this as well +float setPositionFromDisplacement(TalonSRX *, double, int); // Sets position (Talon units) based on input displacement (cm), returns this as well +float setPositionFromJointCommand(TalonSRX *, double, int); // Sets position (Talon units) based on input joint command (radians), returns this as well void stopMotor(TalonSRX *, int); // Stops the motor #endif \ No newline at end of file diff --git a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp index a11a9e44..a68327b3 100644 --- a/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp +++ b/src/hardware_interfaces/talon_ros2_control/include/talon_ros2_control/talon_hardware_interface.hpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #ifndef TALON_HARDWARE_INTERACE_HPP_ #define TALON_HARDWARE_INTERACE_HPP_ @@ -54,8 +36,6 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector export_command_interfaces() override; - void enable_system_thread(); - // Lifecycle hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -83,13 +63,24 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh hardware_interface::return_type write( const rclcpp::Time & time, const rclcpp::Duration & period) override; -private: + // Helper Functions + void logger_function(); + void enable_system_thread(); +private: + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; // Time since last hardware interface update + double elapsed_time; // Time since first hardware interface update + double elapsed_logger_time; // Time since last logger update + int logger_rate; // Logger update rate + int logger_state; // Logger on/off state + + // Keeps track of amount of joints int num_joints; - - // EXPERIMENTING - std::vector initial_position_; + // Maximum displacement for prismatic joints + std::vector max_disp; // Store the state for the simulated robot std::vector joint_state_position_; @@ -99,8 +90,14 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh std::vector joint_command_position_; std::vector joint_command_velocity_; + // Talon specific information std::vector joint_node_ids; + std::string can_interface; + std::vector talon_motors; + std::thread worker; + std::atomic is_running = false; + // Modes for control mode enum integration_level_t : std::uint8_t { UNDEFINED = 0, @@ -111,9 +108,15 @@ class TALONHardwareInterface : public hardware_interface::SystemInterface // Inh // Active control mode std::vector control_level_; - std::vector talon_motors; - std::thread worker; - std::atomic is_running = false; + // Types of joints + enum class joint_type_t : std::uint8_t + { + REVOLUTE = 0, + PRISMATIC = 1, + }; + + // Type of joint for each actuator + std::vector joint_type_; }; } // namespace talon_hardware_interface diff --git a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp index 694767a7..b06ea01b 100644 --- a/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp +++ b/src/hardware_interfaces/talon_ros2_control/src/talon_hardware_interface.cpp @@ -1,21 +1,3 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) -// -// 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. - -// -// Authors: Denis Stogl -// - #include "talon_ros2_control/talon_hardware_interface.hpp" #include @@ -36,33 +18,87 @@ #include "rclcpp/rclcpp.hpp" #include "talon_ros2_control/talon_control.h" -// #include "talon_control.h" - -#define DEBUG_MODE 0 // 0 for off 1 for on namespace talon_ros2_control { TALONHardwareInterface::TALONHardwareInterface() { } +void TALONHardwareInterface::logger_function(){ + + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nTALON Logger"; + std::string control_mode = ""; + std::ostringstream oss; + std::string status; + + // HWI Specific + oss << "\n--- HWI Specific ---\n" + << "CAN Interface: " << can_interface + << " | HWI Update Rate: " << update_rate + << " | Logger Update Rate: " << logger_rate << "\n" + << "Elapsed Time since first update: " << elapsed_time << "\n" + << "\n--- Joint Specific ---"; + + for (int i = 0; i < num_joints; i++) { + if(static_cast(control_level_[i]) == 1) { + control_mode = "POSITION"; + } + else if(static_cast(control_level_[i]) == 2) { + control_mode = "VELOCITY"; + } + else { + control_mode = "UNDEFINED"; + } + + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] << "\n" + << "-- Commands --\n" + << "Control Mode: " << control_mode << "\n" + << "Joint Command Position: " << joint_command_position_[i] << "\n" + << "Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] << "\n" + << "Joint Velocity: " << joint_state_velocity_[i] << "\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), log_msg.c_str()); +} + hardware_interface::CallbackReturn TALONHardwareInterface::on_init( const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file { - if ( - hardware_interface::SystemInterface::on_init(info) != - hardware_interface::CallbackReturn::SUCCESS) + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } - /* - IF YOU WANT TO USE PARAMETERS FROM ROS2_CONTROL XACRO, DO THAT HERE!!! AS OF RIGHT NOW, NOT SURE - IF NEEDED. USE info - */ + // -- Parameters -- + can_interface = info_.hardware_parameters.at("can_interface"); + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); for (auto& joint : info_.joints) { joint_node_ids.push_back(std::stoi(joint.parameters.at("node_id"))); - initial_position_.push_back(std::stof(joint.state_interfaces[0].initial_value)); + + std::string joint_type = joint.parameters.at("joint_type"); + + // Revolute or Prismatic + if (joint_type == "revolute") { + joint_type_.push_back(joint_type_t::REVOLUTE); + max_disp.push_back(std::nan("")); // Not used for revolute joints + } else if (joint_type == "prismatic" && joint.parameters.count("max_disp")) { + joint_type_.push_back(joint_type_t::PRISMATIC); + max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); + } else { + RCLCPP_ERROR(rclcpp::get_logger("TALONHardwareInterface"), "Invalid joint_type parameter for joint %s. Must be 'revolute' or 'prismatic'. If prismatic, it must have a 'max_disp' parameter.", joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } } num_joints = static_cast(info_.joints.size()); @@ -108,7 +144,6 @@ std::vector TALONHardwareInterface::export for(int i = 0; i < num_joints; i++){ command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_command_position_[i])); - command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); } @@ -124,7 +159,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_configure( RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Beginning configure."); for(int i = 0; i < num_joints; i++){ - TalonSRX *motor = new TalonSRX(joint_node_ids[i], "can0"); + TalonSRX *motor = new TalonSRX(joint_node_ids[i], can_interface); talon_motors.push_back(motor); RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "talon_motor initialized."); initMotor(motor); @@ -163,7 +198,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_activate( is_running.store(true); worker = std::thread(&TALONHardwareInterface::enable_system_thread, this); - // setPosition(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself + // setPositionFromDisplacement(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Successfully activated!"); @@ -179,7 +214,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Deactivating ...please wait..."); for(int i = 0; i < num_joints; i++){ - setVelocity(talon_motors[i], 0.0, 50); + setVelocityFromLinearVelocity(talon_motors[i], 0.0, 50); } is_running.store(false); @@ -187,7 +222,7 @@ hardware_interface::CallbackReturn TALONHardwareInterface::on_deactivate( worker.join(); } - // setPosition(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself + // setPositionFromDisplacement(talon_motor, 0.0, 50); // dangerous atm since this is an incremental encoder, dont want this to crush itself RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Successfully deactivated!"); @@ -200,49 +235,64 @@ hardware_interface::return_type TALONHardwareInterface::read( for(int i = 0; i < num_joints; i++){ joint_state_position_[i] = getPositionDistance(talon_motors[i]); // meters joint_state_velocity_[i] = getClawVelocity(talon_motors[i]); // m/s - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Reading for Talon - Joint position: %f Joint velocity: %f \n", - joint_state_position_[i], - joint_state_velocity_[i]); - } } return hardware_interface::return_type::OK; } hardware_interface::return_type talon_ros2_control::TALONHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { - float max_dist = 0.06; //in m - for(int i = 0; i < num_joints; i++){ - if(control_level_[i] == integration_level_t::POSITION && std::isfinite(joint_command_position_[i])) { + elapsed_update_time+=period.seconds(); + double update_period = 1.0/update_rate; + int32_t joint_angle = 0; + int32_t joint_velocity = 0; - // TO DO: implement joint limits so i dont gotta do this - if(joint_command_position_[i] > max_dist) { - joint_command_position_[i] = max_dist; - } - else if(joint_command_position_[i] < 0.0) { - joint_command_position_[i] = 0.0; - } - - // COMMAND POSITION - setPosition(talon_motors[i], joint_command_position_[i], 50); + elapsed_time+=period.seconds(); + + // Logger update + elapsed_logger_time+=period.seconds(); + double logging_period = 1.0/logger_rate; + if(elapsed_logger_time > logging_period){ + elapsed_logger_time = 0.0; + if (logger_state == 1) { + logger_function(); + } + } - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Writing positions for Talon - Joint command: %f", - joint_command_position_[i]); - } - } else if(control_level_[i] == integration_level_t::VELOCITY && std::isfinite(joint_command_velocity_[i])) { + // HWI can only go as fast as the controller manager. To limit frequency of bus messages, + // keep track of time passed over iterations of this function and if it exceeds the + // desired frequency of the HWI, skip message + if(elapsed_update_time > update_period){ + elapsed_update_time = 0.0; + + // Motor Command for each joint at given frequency + for(int i = 0; i < num_joints; i++){ + if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_position_[i])) { + + // TO DO: implement joint limits so i dont gotta do this + joint_command_position_[i] = std::clamp(joint_command_position_[i], 0.0, max_disp[i]); + // COMMAND PRISMATIC POSITION + setPositionFromDisplacement(talon_motors[i], joint_command_position_[i], 50); + + } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::PRISMATIC && std::isfinite(joint_command_velocity_[i])) { + + // COMMAND PRISMATIC VELOCITY + setVelocityFromLinearVelocity(talon_motors[i], joint_command_velocity_[i], 50); - // COMMAND VELOCITY - setVelocity(talon_motors[i], joint_command_velocity_[i], 50); + } else if(control_level_[i] == integration_level_t::POSITION && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_position_[i])) { + + // COMMAND REVOLUTE POSITION + setPositionFromJointCommand(talon_motors[i], joint_command_position_[i], 50); + + } else if(control_level_[i] == integration_level_t::VELOCITY && joint_type_[i] == joint_type_t::REVOLUTE && std::isfinite(joint_command_velocity_[i])) { + + // COMMAND REVOLUTE VELOCITY + setVelocityFromAngularVelocity(talon_motors[i], joint_command_velocity_[i], 50); - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Writing velocities for Talon - Joint command: %f", - joint_command_velocity_[i]); } - } else { - // RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Joint command value not found or undefined command state"); + else { + // RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), "Joint command value not found or undefined command state"); + } } } @@ -253,48 +303,82 @@ hardware_interface::return_type TALONHardwareInterface::perform_command_mode_swi const std::vector& start_interfaces, const std::vector& stop_interfaces) { - std::vector new_modes = {}; - for (std::string key : start_interfaces) - { - for (int i = 0; i < num_joints; i++){ - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION){ - new_modes.push_back(integration_level_t::POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY){ - new_modes.push_back(integration_level_t::VELOCITY); + // Debug: print incoming requests + std::ostringstream ss; + ss << "perform_command_mode_switch called. start_interfaces: ["; + for (auto &s : start_interfaces) ss << s << ","; + ss << "] stop_interfaces: ["; + for (auto &s : stop_interfaces) ss << s << ","; + ss << "]"; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), ss.str().c_str()); + + // For each joint, decide its new control mode based on start/stop interfaces. + // We allow partial starts/stops: only affected joints are switched. + std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); + + // Process stop interfaces first: mark those joints as UNDEFINED + for (const auto &ifname : stop_interfaces) { + for (int i = 0; i < num_joints; ++i) { + const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + if (ifname == pos_if || ifname == vel_if || ifname.find(info_.joints[i].name) != std::string::npos) { + requested_modes[i] = integration_level_t::UNDEFINED; } } } - - // All joints must be given new command mode at the same time - if (new_modes.size() != static_cast(num_joints)){ - return hardware_interface::return_type::ERROR; - } - // All joints must have the same command mode - if (!std::all_of( - new_modes.begin() + 1, new_modes.end(), - [&](integration_level_t mode) { return mode == new_modes[0]; })) - { - return hardware_interface::return_type::ERROR; - } - // Stop motion on all relevant joints that are stopping - for (std::string key : stop_interfaces) { - for (int i = 0; i < num_joints; i++){ - if (key.find(info_.joints[i].name) != std::string::npos) { - joint_command_position_[i] = 0; - joint_command_velocity_[i] = 0; - control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined + // Process start interfaces: set POSITION or VELOCITY per joint + for (const auto &ifname : start_interfaces) { + for (int i = 0; i < num_joints; ++i) { + const std::string pos_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_POSITION); + const std::string vel_if = info_.joints[i].name + "/" + std::string(hardware_interface::HW_IF_VELOCITY); + if (ifname == pos_if) { + requested_modes[i] = integration_level_t::POSITION; + } else if (ifname == vel_if) { + requested_modes[i] = integration_level_t::VELOCITY; } } } - // Set the new command modes. By this point everything should be undefined after the "stop motion" loop - for (int i = 0; i < num_joints; i++){ - if (control_level_[i] != integration_level_t::UNDEFINED) { - // Something else is using the joint! Abort! - return hardware_interface::return_type::ERROR; + + // Now apply the requested_modes to control_level_. + // For any joint with UNDEFINED in requested_modes, we only change it if it was explicitly stopped. + for (int i = 0; i < num_joints; ++i) { + if (requested_modes[i] == integration_level_t::UNDEFINED) { + // if stop requested, set to UNDEFINED; otherwise leave existing mode + // (we only set to UNDEFINED if this joint was mentioned in stop_interfaces) + bool was_stopped = false; + for (const auto &ifname : stop_interfaces) { + if (ifname.find(info_.joints[i].name) != std::string::npos) { + was_stopped = true; + break; + } + } + if (was_stopped) { + control_level_[i] = integration_level_t::UNDEFINED; + joint_command_velocity_[i] = 0; + // optional: reset position cmd to current state to be safe + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), + "Joint %s: stopped -> set UNDEFINED", info_.joints[i].name.c_str()); + } + // else, leave control_level_ as-is + } else { + // Set the mode requested + control_level_[i] = requested_modes[i]; + // If switching to velocity, optionally set command velocity to current state to avoid jumps + if (requested_modes[i] == integration_level_t::VELOCITY) { + // joint_command_velocity_[i] = joint_state_velocity_[i]; + joint_command_velocity_[i] = 0; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), + "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", + info_.joints[i].name.c_str(), joint_command_velocity_[i]); + } else if (requested_modes[i] == integration_level_t::POSITION) { + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("TALONHardwareInterface"), + "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", + info_.joints[i].name.c_str(), joint_command_position_[i]); + } } - control_level_[i] = new_modes[i]; } return hardware_interface::return_type::OK; diff --git a/src/subsystems/arm/README.md b/src/subsystems/arm/README.md index 82bd24bd..fa3f71e5 100644 --- a/src/subsystems/arm/README.md +++ b/src/subsystems/arm/README.md @@ -65,9 +65,9 @@ Use *ip link* to check that there is a can0/vcan0. **Open another terminal, source the workspace, and call the service to set controllers:** ```bash source install/setup.bash -ros2 service call /set_controller athena_arm_msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" +ros2 service call /set_controller msgs/srv/SetController "{controller_names: [INCLUDE CONTROLLER(S) YOU WANT WITHIN BRACKETS]}" ``` -Example: `ros2 service call /set_controller athena_arm_msgs/srv/SetController "{controller_names: [joint_trajectory_controller, arm_velocity_controller]}"` +Example: `ros2 service call /set_controller msgs/srv/SetController "{controller_names: [joint_trajectory_controller, arm_velocity_controller]}"` --- diff --git a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml index 0708f2e7..99c4cf7d 100644 --- a/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml +++ b/src/subsystems/arm/arm_bringup/config/athena_arm_controllers.yaml @@ -1,6 +1,6 @@ controller_manager: ros__parameters: - update_rate: 10 # Hz + update_rate: 100 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -100,18 +100,21 @@ manual_arm_cylindrical_controller: manual_arm_joint_by_joint_controller: ros__parameters: - joints: + velocity_joints: - base_yaw - shoulder_pitch - elbow_pitch - wrist_pitch - wrist_roll - # - gripper_claw - interface_name: velocity + - gripper_claw + position_joints: + - actuator + # interface_name: velocity joint_max_velocities: - 62.83185307 # 10 rot/s, please change dawg - 0.05148721 # 2.95 dps - 0.05148721 - 0.785398 # 45 dps - 0.785398 - # - 0.005 # TODO: Find this + - 0.005 # TODO: Find this + - 0.03 # Actuator max displacement diff --git a/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml b/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml index 9b03cd76..c66dadc2 100644 --- a/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml +++ b/src/subsystems/arm/arm_controllers/athena_arm_controllers.xml @@ -1,23 +1,3 @@ - - diff --git a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml index bd5d8f23..03f73d72 100644 --- a/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml +++ b/src/subsystems/arm/arm_controllers/config/manual_arm_joint_by_joint_controller.yaml @@ -18,36 +18,46 @@ # manual_arm_joint_by_joint_controller: - joints: { + position_joints: { type: string_array, default_value: [], - description: "Specifies joints used by the controller. If state joints parameter is defined, then only command joints are defined with this parameter.", + description: "Specifies joints which are controlled via position.", read_only: true, validation: { unique<>: null, not_empty<>: null, } } - state_joints: { + velocity_joints: { type: string_array, default_value: [], - description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + description: "Specifies joints which are controlled via velocity.", read_only: true, validation: { unique<>: null, - } - } - interface_name: { - type: string, - default_value: "", - description: "Name of the interface used by the controller on joints and command_joints.", - read_only: true, - validation: { not_empty<>: null, - one_of<>: [["position", "velocity", "acceleration", "effort",]], - forbidden_interface_name_prefix: null } } + # state_joints: { + # type: string_array, + # default_value: [], + # description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + # read_only: true, + # validation: { + # unique<>: null, + # } + # } + # interface_name: { + # type: string, + # default_value: "", + # description: "Name of the interface used by the controller on joints and command_joints.", + # read_only: true, + # validation: { + # not_empty<>: null, + # one_of<>: [["position", "velocity", "acceleration", "effort",]], + # forbidden_interface_name_prefix: null + # } + # } joint_max_velocities: { type: double_array, default_value: [], diff --git a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp index 05ceed33..bbe8eb5b 100644 --- a/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp +++ b/src/subsystems/arm/arm_controllers/include/athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp @@ -1,22 +1,3 @@ -// Copyright (c) 2025, UMDLoop -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. - -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// #ifndef ATHENA_ARM_CONTROLLERS__MANUAL_ARM_JOINT_BY_JOINT_CONTROLLER_HPP_ #define ATHENA_ARM_CONTROLLERS__MANUAL_ARM_JOINT_BY_JOINT_CONTROLLER_HPP_ @@ -100,7 +81,9 @@ class ManualArmJointByJointController : public controller_interface::ControllerI std::shared_ptr param_listener_; manual_arm_joint_by_joint_controller::Params params_; - std::vector state_joints_; + int num_joints; + bool actuator_active_ = false; + double actuator_iterator = 0.0; // Represents velocities of each joint std::vector joint_velocities_; @@ -128,4 +111,4 @@ class ManualArmJointByJointController : public controller_interface::ControllerI } // namespace arm_controllers -#endif // ATHENA_ARM_CONTROLLERS__manual_arm_joint_by_joint_CONTROLLER_HPP_ +#endif // ATHENA_ARM_CONTROLLERS__MANUAL_ARM_JOINT_BY_JOINT_CONTROLLER_HPP_ diff --git a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp index cdf10dca..cbd74874 100644 --- a/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp +++ b/src/subsystems/arm/arm_controllers/src/manual_arm_joint_by_joint_controller.cpp @@ -1,23 +1,3 @@ -// Copyright (c) 2025, UMDLoop -// Copyright (c) 2025, Stogl Robotics Consulting UG (haftungsbeschränkt) (template) -// -// 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. - -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. -// - #include "athena_arm_controllers/manual_arm_joint_by_joint_controller.hpp" #include @@ -81,25 +61,8 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu { params_ = param_listener_->get_params(); - if (!params_.state_joints.empty()) - { - state_joints_ = params_.state_joints; - } - else - { - state_joints_ = params_.joints; - } - - if (params_.joints.size() != state_joints_.size()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of 'joints' (%zu) and 'state_joints' (%zu) parameters has to be the same!", - params_.joints.size(), state_joints_.size()); - return CallbackReturn::FAILURE; - } - - joint_velocities_.resize(params_.joints.size(), 0.0); // Output + num_joints = static_cast(params_.position_joints.size()) + static_cast(params_.velocity_joints.size()); + joint_velocities_.resize(num_joints, 0.0); // Output max_velocities_ = params_.joint_max_velocities; // topics QoS @@ -154,7 +117,7 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu // TODO(anyone): Reserve memory in state publisher depending on the message type state_publisher_->lock(); - state_publisher_->msg_.header.frame_id = params_.joints[0]; + state_publisher_->msg_.header.frame_id = params_.velocity_joints[0]; state_publisher_->unlock(); RCLCPP_INFO(get_node()->get_logger(), "configure successful"); @@ -163,20 +126,6 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_configu void ManualArmJointByJointController::reference_callback(const std::shared_ptr msg) { - /* - if (msg->joint_names.size() == params_.joints.size()) - { - input_ref_.writeFromNonRT(msg); - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received %zu , but expected %zu joints in command. Ignoring message.", - msg->joint_names.size(), params_.joints.size()); - } - */ - input_ref_.writeFromNonRT(msg); } @@ -185,10 +134,16 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::co controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names.reserve(params_.joints.size()); - for (const auto & joint : params_.joints) + command_interfaces_config.names.reserve(num_joints); + + for (const auto & joint : params_.velocity_joints) { - command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + command_interfaces_config.names.push_back(joint + "/velocity"); + } + + for (const auto & joint : params_.position_joints) + { + command_interfaces_config.names.push_back(joint + "/position"); } return command_interfaces_config; @@ -199,10 +154,13 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::st controller_interface::InterfaceConfiguration state_interfaces_config; state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - state_interfaces_config.names.reserve(state_joints_.size()); - for (const auto & joint : state_joints_) - { - state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + state_interfaces_config.names.reserve(num_joints); + for (const auto & joint : params_.velocity_joints) { + state_interfaces_config.names.push_back(joint + "/velocity"); + } + + for (const auto & joint : params_.position_joints) { + state_interfaces_config.names.push_back(joint + "/position"); } return state_interfaces_config; @@ -211,10 +169,6 @@ controller_interface::InterfaceConfiguration ManualArmJointByJointController::st controller_interface::CallbackReturn ManualArmJointByJointController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - // TODO(anyone): if you have to manage multiple interfaces that need to be sorted check - // `on_activate` method in `JointTrajectoryController` for exemplary use of - // `controller_interface::get_ordered_interfaces` helper function - // Set default value in command reset_controller_reference_msg(*(input_ref_.readFromRT)(), joystick_axes, joystick_buttons); @@ -224,8 +178,6 @@ controller_interface::CallbackReturn ManualArmJointByJointController::on_activat controller_interface::CallbackReturn ManualArmJointByJointController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - // TODO(anyone): depending on number of interfaces, use definitions, e.g., `CMD_MY_ITFS`, - // instead of a loop for (size_t i = 0; i < command_interfaces_.size(); ++i) { command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); @@ -240,18 +192,22 @@ controller_interface::return_type ManualArmJointByJointController::update( if (!std::isnan((*current_ref)->axes[0])) { - // ODrive + // Base Yaw: L/R Left Stick joint_velocities_[0] = ((*current_ref)->buttons[1] == 1) ? 0.0 : (*current_ref)->axes[0] * max_velocities_[0]; - // RMD - joint_velocities_[1] = ((*current_ref)->buttons[1] == 1) ? 0.0 : -(*current_ref)->axes[1] * max_velocities_[1]; // Shoulder Pitch - joint_velocities_[2] = (*current_ref)->axes[3] * max_velocities_[2]; // Elbow Pitch + // Shoulder Pitch: U/D Left Stick + joint_velocities_[1] = ((*current_ref)->buttons[1] == 1) ? 0.0 : -(*current_ref)->axes[1] * max_velocities_[1]; + + // Elbow Pitch: U/D Right Stick + joint_velocities_[2] = (*current_ref)->axes[3] * max_velocities_[2]; - // SMC + // Wrist Pitch: U/D on Left Joystick AND O button joint_velocities_[3] = -(*current_ref)->axes[1] * static_cast((*current_ref)->buttons[1]) * max_velocities_[3]; + + // Wrist Roll: L/R on Left Joystick AND O button joint_velocities_[4] = -(*current_ref)->axes[0] * static_cast((*current_ref)->buttons[1]) * max_velocities_[4]; - // Talons + // Gripper Claw: Bumpers and Triggers if ((*current_ref)->buttons[4] && (*current_ref)->buttons[5]) { // closeClaw(motor); } else if ((*current_ref)->buttons[4]) { // Left bumper @@ -265,13 +221,33 @@ controller_interface::return_type ManualArmJointByJointController::update( } else{ joint_velocities_[5] = 0.0; } - // RCLCPP_INFO(get_node()->get_logger(), "Talon: %f", joint_velocities_[5]); + + // Actuator (EXPERIMENTAL) + // Pressing square activates actuator movement + if((*current_ref)->buttons[3] == 1){ + actuator_active_ = true; + actuator_iterator = 0.001; + joint_velocities_[6] = 0.001; // to start off + } + + // Once actuator reaches original position, stop movement + if (actuator_active_ = true && joint_velocities_[6] == 0.0 && actuator_iterator < 0){ + actuator_active_ = false; + } + + // Move actuator up to max position, then begin moving it down + if (joint_velocities_[6] >= max_velocities_[6] && actuator_active_ == true && actuator_iterator > 0){ + actuator_iterator = -0.001; + } + joint_velocities_[6] = joint_velocities_[6] + actuator_iterator; } else{ - RCLCPP_INFO(get_node()->get_logger(), "Returning NaN"); - joint_velocities_.resize(6, 0.0); + // RCLCPP_INFO(get_node()->get_logger(), "Returning NaN"); + joint_velocities_.resize(7, 0.0); } + // RCLCPP_INFO(get_node()->get_logger(), "Size of Command Interface: %d", command_interfaces_.size()); + for (size_t i = 0; i < command_interfaces_.size(); ++i) { if (*(control_mode_.readFromRT()) == control_mode_type::SLOW) diff --git a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py index 4594ea45..26f22e20 100644 --- a/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py +++ b/src/subsystems/drive/drive_bringup/launch/athena_drive.launch.py @@ -384,7 +384,7 @@ def generate_launch_description(): robot_state_pub_node, joystick_publisher, teleop_twist_joy, - joint_state_publisher, + # joint_state_publisher, # delay_can_node_after_control_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, delay_rviz_after_joint_state_broadcaster_spawner, diff --git a/src/subsystems/science/science_controllers/scripts/joystick_publisher.py b/src/subsystems/science/science_controllers/scripts/joystick_publisher.py index 3b9abba8..cf2e2e9e 100644 --- a/src/subsystems/science/science_controllers/scripts/joystick_publisher.py +++ b/src/subsystems/science/science_controllers/scripts/joystick_publisher.py @@ -48,7 +48,7 @@ def __init__(self): # Only begin once a joystick is connected while(joysticks == 0): - self.get_logger().info("No controllers are connected!") + # self.get_logger().info("No controllers are connected!") time.sleep(0.25) for event in pygame.event.get(): if event.type == pygame.JOYDEVICEADDED: diff --git a/src/teleop/teleop/teleop/joystick_publisher.py b/src/teleop/teleop/teleop/joystick_publisher.py index 18f1ce7a..7b39c8a0 100755 --- a/src/teleop/teleop/teleop/joystick_publisher.py +++ b/src/teleop/teleop/teleop/joystick_publisher.py @@ -57,7 +57,7 @@ def __init__(self): # Only begin once a joystick is connected while(joysticks == 0): - self.get_logger().info("No controllers are connected!") + # self.get_logger().info("No controllers are connected!") time.sleep(0.25) for event in pygame.event.get(): if event.type == pygame.JOYDEVICEADDED: