diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..02e32c61 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,69 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp" + } +} \ No newline at end of file diff --git a/src/description/ros2_control/science/science.mock.ros2_control.xacro b/src/description/ros2_control/science/science.mock.ros2_control.xacro index f1686fcc..37987cf4 100644 --- a/src/description/ros2_control/science/science.mock.ros2_control.xacro +++ b/src/description/ros2_control/science/science.mock.ros2_control.xacro @@ -55,13 +55,13 @@ - + - + diff --git a/src/description/ros2_control/science/science.stepper.ros2_control.xacro b/src/description/ros2_control/science/science.stepper.ros2_control.xacro index 0b4e8e57..0c37e13b 100644 --- a/src/description/ros2_control/science/science.stepper.ros2_control.xacro +++ b/src/description/ros2_control/science/science.stepper.ros2_control.xacro @@ -7,39 +7,37 @@ stepper_ros2_control/STEPPERHardwareInterface + 10 + 5 + 0 + can0 + 0x90 + 0x2 + 1 + 20 + continuous - 0.0 - - - 1 - - - 1 - + + 0x3 + 1 + 20 + continuous - 0.0 - - - 1 - - - 1 - - + \ No newline at end of file diff --git a/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt b/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt index 0bb0b393..75482a92 100644 --- a/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt +++ b/src/hardware_interfaces/stepper_ros2_control/CMakeLists.txt @@ -11,7 +11,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle realtime_tools - msgs umdloop_can_library ) @@ -26,7 +25,6 @@ endforeach() # umdloop_can_library is found through the dependency loop above ament_auto_find_build_dependencies() -ament_export_dependencies(rosidl_default_runtime) include_directories(include) @@ -52,7 +50,6 @@ ament_target_dependencies( rclcpp rclcpp_lifecycle realtime_tools - msgs ) # Export hardware plugins diff --git a/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp b/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp index d23870ea..5d1809c1 100644 --- a/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp +++ b/src/hardware_interfaces/stepper_ros2_control/include/stepper_ros2_control/stepper_hardware_interface.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2024 UMD Loop // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,21 +11,16 @@ // 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 STEPPER_HARDWARE_INTERFACE_HPP_ #define STEPPER_HARDWARE_INTERFACE_HPP_ -#include #include #include #include #include - #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" @@ -34,30 +29,29 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" - +// Real-time CAN communication library +#include #include #include #include -#include "msgs/msg/cana.hpp" + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" namespace stepper_ros2_control { -class STEPPERHardwareInterface : public hardware_interface::SystemInterface // Inheriting from System Interface +class STEPPERHardwareInterface : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(STEPPERHardwareInterface) - // Initialization, so 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 hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; @@ -85,61 +79,74 @@ class STEPPERHardwareInterface : public hardware_interface::SystemInterface // I const rclcpp::Time & time, const rclcpp::Duration & period) override; // Helper Functions - 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); + // CAN callback for receiving messages + void onCanMessage(const CANLib::CanFrame& frame); + void logger_function(); private: + 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 + int num_joints; + int current_joint_; + std::string can_interface; - // EXPERIMENTING - std::vector initial_position_; + uint32_t can_command_id; + uint32_t can_response_id; - // Store the state for the simulated robot + // Store the state for the robot std::vector joint_state_position_; std::vector joint_state_velocity_; - // Store the command for the simulated robot + // Store the command for the robot std::vector joint_command_position_; std::vector joint_command_velocity_; - // Telemetry data from CAN responses - std::vector motor_temperature_; // Motor temperature in °C - std::vector motor_torque_current_; // Motor torque current in A - - double encoder_position; - double motor_velocity; - double motor_position; + // Motor state from CAN (per joint) + std::vector motor_position; + std::vector motor_velocity; + std::vector rated_max; + std::vector device_status; - std::vector joint_initialization_; + // CAN communication + CANLib::SocketCanBus canBus_; + CANLib::CanFrame can_tx_frame_; + CANLib::CanFrame can_rx_frame_; - rclcpp::Publisher::SharedPtr science_can_publisher_; - rclcpp::Subscription::SharedPtr science_can_subscriber_; - rclcpp::Node::SharedPtr node_; - uint16_t current_iteration; - - - msgs::msg::CANA received_joint_data_; + bool can_connected_; + double is_connected_; // State interface for connection status + // Joint parameters + std::vector joint_node_ids; std::vector joint_gear_ratios; - std::vector joint_orientation; - + std::vector joint_type_; - - enum integration_level_t : std::uint8_t + enum class integration_level_t : std::uint8_t { UNDEFINED = 0, POSITION = 1, VELOCITY = 2, }; + + // 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 = 0x40; + static constexpr uint8_t MOTOR_STATUS_CMD = 0x50; + static constexpr uint8_t MAINTENANCE_CMD = 0x60; + static constexpr uint8_t SERVO_SPECS_CMD = 0x70; // Active control mode for each actuator std::vector control_level_; - }; } // namespace stepper_ros2_control diff --git a/src/hardware_interfaces/stepper_ros2_control/package.xml b/src/hardware_interfaces/stepper_ros2_control/package.xml index c1a4f127..b09db3b4 100644 --- a/src/hardware_interfaces/stepper_ros2_control/package.xml +++ b/src/hardware_interfaces/stepper_ros2_control/package.xml @@ -15,7 +15,6 @@ rclcpp rclcpp_lifecycle realtime_tools - msgs umdloop_can_library @@ -25,9 +24,6 @@ ament_lint_common ament_cmake - rosidl_default_runtime - - rosidl_interface_packages ament_cmake diff --git a/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp b/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp index d12c3d15..f355b3a4 100644 --- a/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp +++ b/src/hardware_interfaces/stepper_ros2_control/src/stepper_hardware_interface.cpp @@ -1,4 +1,4 @@ -// Copyright (c) 2021, Stogl Robotics Consulting UG (haftungsbeschränkt) +// Copyright (c) 2024 UMD Loop // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,134 +11,201 @@ // 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 "stepper_ros2_control/stepper_hardware_interface.hpp" -#include -#include -#include +#include #include -#include -#include +#include #include #include -#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 stepper_ros2_control { + +void STEPPERHardwareInterface::logger_function() +{ + // Prevents breaking the logger + if (num_joints == 0) return; + + // Building Message + std::string log_msg = "\033[2J\033[H \nSTEPPER 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("STEPPERHardwareInterface"), log_msg.c_str()); +} + hardware_interface::CallbackReturn STEPPERHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) // Info stores all parameters in xacro file + const hardware_interface::HardwareInfo & info) { - 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 + + // Parse joint parameters for (auto& joint : info_.joints) { - joint_gear_ratios.push_back(std::stoi(joint.parameters.at("gear_ratio"))); - initial_position_.push_back(std::stof(joint.parameters.at("initial_position"))); + 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")))); + rated_max.push_back(std::abs(std::stod(joint.parameters.at("rated_max"))) * (M_PI / 180.0)); - joint_orientation.push_back(std::stoi(joint.parameters.at("joint_orientation"))); + std::string joint_type = joint.parameters.at("joint_type"); + if (joint_type != "standard" && joint_type != "continuous") { + RCLCPP_ERROR( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Invalid joint_type parameter for joint %s. Must be 'standard' or 'continuous'.", + joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + joint_type_.push_back(joint_type); } num_joints = static_cast(info_.joints.size()); - for (size_t i = 0; i < initial_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu initial position in on_init: %f", i, initial_position_[i]); + // Parse hardware parameters with defaults + if (info_.hardware_parameters.count("update_rate")) { + update_rate = std::stoi(info_.hardware_parameters.at("update_rate")); + } else { + update_rate = 10; } - - // Initializes command and state interface values - joint_state_position_.assign(num_joints, 0.0); - joint_state_velocity_.assign(num_joints, 0.0); - // joint_command_position_.assign(num_joints, 0); - joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + if (info_.hardware_parameters.count("logger_rate")) { + logger_rate = std::stoi(info_.hardware_parameters.at("logger_rate")); + } else { + logger_rate = 5; + } - // joint_command_position_ = initial_position_; - joint_command_velocity_.assign(num_joints, 0.0); + if (info_.hardware_parameters.count("logger_state")) { + logger_state = std::stoi(info_.hardware_parameters.at("logger_state")); + } else { + logger_state = 0; + } + + if (info_.hardware_parameters.count("can_interface")) { + can_interface = info_.hardware_parameters.at("can_interface"); + } else { + can_interface = "can0"; + } - for (size_t i = 0; i < initial_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu command vel in on_init: %f", i, joint_command_velocity_[i]); + if (info_.hardware_parameters.count("can_id")) { + can_command_id = static_cast(std::stoul(info_.hardware_parameters.at("can_id"), nullptr, 0)); + } else { + can_command_id = 0x90; } + can_response_id = can_command_id + 0x1; - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); + // Initialize timing + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; + current_joint_ = 0; - encoder_position = 0.0; - motor_position = 0.0; - motor_velocity = 0.0; + // Initialize state variables + is_connected_ = 0.0; + can_connected_ = false; - joint_initialization_.assign(num_joints, false); + // Initialize command and state interface values + joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_state_velocity_.assign(num_joints, 0.0); + joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_command_velocity_.assign(num_joints, 0.0); - control_level_.resize(num_joints, integration_level_t::POSITION); + // Motor state storage (per joint) + motor_position.assign(num_joints, 0.0); + motor_velocity.assign(num_joints, 0.0); + device_status.assign(num_joints, 0); - current_iteration = 0; + // Steppers default to velocity control + control_level_.resize(num_joints, integration_level_t::VELOCITY); + + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Initialized stepper on CAN interface %s with ID 0x%X (%d joints)", + can_interface.c_str(), can_command_id, num_joints); return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn STEPPERHardwareInterface::on_shutdown( const rclcpp_lifecycle::State & previous_state) { - // 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); } - -std::vector STEPPERHardwareInterface::export_state_interfaces() +std::vector +STEPPERHardwareInterface::export_state_interfaces() { std::vector state_interfaces; - // Each STEPPER motor corresponds to a different joint. - for(int i = 0; i < num_joints; i++){ + for (int i = 0; i < num_joints; i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &joint_state_position_[i])); state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_state_velocity_[i])); + } - // Telemetry interfaces + if (num_joints > 0) { state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, "motor_temperature", &motor_temperature_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, "torque_current", &motor_torque_current_[i])); + info_.joints[0].name, "is_connected", &is_connected_)); } - + + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Exported %zu state interfaces", state_interfaces.size()); + return state_interfaces; } - std::vector STEPPERHardwareInterface::export_command_interfaces() { std::vector command_interfaces; - for(int i = 0; i < num_joints; i++){ + 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])); @@ -146,422 +213,323 @@ STEPPERHardwareInterface::export_command_interfaces() info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &joint_command_velocity_[i])); } + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Exported %zu command interfaces", command_interfaces.size()); + return command_interfaces; } hardware_interface::CallbackReturn STEPPERHardwareInterface::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - // TODO: Find a way to publish from the lifecyle node itself rather than create our own node within it - // Node to publish to umdloop_can_node - node_ = rclcpp::Node::make_shared("stepper_hardware_node"); - science_can_publisher_ = node_->create_publisher("can_tx", rclcpp::QoS(10)); - - // Lambda function that takes the message as a shared pointer, dereferences it, - // and stores it in received_joint_data_ to be used - science_can_subscriber_ = node_->create_subscription( - "can_rx", - rclcpp::QoS(50).reliable(), - [this](const msgs::msg::CANA::SharedPtr received_message) - { - received_joint_data_ = *received_message; - } - ); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Configuring stepper hardware..."); + + if (!canBus_.open(can_interface, + std::bind(&STEPPERHardwareInterface::onCanMessage, this, std::placeholders::_1))) + { + RCLCPP_WARN( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Failed to open CAN interface %s - running in SIMULATION mode", + can_interface.c_str()); + can_connected_ = false; + } else { + can_connected_ = true; + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Successfully opened CAN interface %s", can_interface.c_str()); + } + + is_connected_ = can_connected_ ? 1.0 : 0.0; + + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper hardware configured (%s)", can_connected_ ? "CAN MODE" : "SIMULATION"); return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( - const rclcpp_lifecycle::State & /*previous_state*/) +// Per protocol spec, response decoding uses 16-bit values (not 24-bit). +// All responses return: data[1-2] = position (int16, deg), data[3-4] = velocity (int16, deg/s). +void STEPPERHardwareInterface::onCanMessage(const CANLib::CanFrame& frame) { - // 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++){ - auto joint_tx = msgs::msg::CANA(); - - // Motor Shutdown Command - joint_tx.data = {0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); - - // Brake Lock Command (don't think this works) - joint_tx.data = {0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); + RCLCPP_INFO( + rclcpp::get_logger("STEPPER"), + "RX id=0x%X dlc=%d b0=0x%02X", + frame.id, frame.dlc, frame.data[0]); + + can_rx_frame_ = frame; + + if (can_rx_frame_.id != can_response_id) { + return; } - // Reset shared pointers which essentially deletes it - science_can_publisher_.reset(); - science_can_subscriber_.reset(); - node_.reset(); + uint8_t command_nibble = (can_rx_frame_.data[0] >> 4) & 0x0F; + uint8_t device_id_nibble = can_rx_frame_.data[0] & 0x0F; - return hardware_interface::CallbackReturn::SUCCESS; + for (int i = 0; i < num_joints; i++) { + if (device_id_nibble != static_cast(joint_node_ids[i] & 0x0F)) { + continue; + } + + if (command_nibble == MOTOR_STATE_CMD) { + // Per protocol spec, position and velocity are 16-bit signed (deg, deg/s), little endian + int16_t raw_position = static_cast( + can_rx_frame_.data[1] | (can_rx_frame_.data[2] << 8)); + int16_t raw_velocity = static_cast( + can_rx_frame_.data[3] | (can_rx_frame_.data[4] << 8)); + + // Convert deg -> rad and apply gear ratio + motor_position[i] = (static_cast(raw_position) * M_PI / 180.0) / joint_gear_ratios[i]; + motor_velocity[i] = (static_cast(raw_velocity) * M_PI / 180.0) / joint_gear_ratios[i]; + + } else if (command_nibble == MOTOR_STATUS_CMD) { + device_status[i] = can_rx_frame_.data[1]; + } + + break; // Matched joint, no need to keep iterating + } } hardware_interface::CallbackReturn STEPPERHardwareInterface::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Activating ...please wait..."); - - for(int i = 0; i < num_joints; i++){ - auto joint_tx = msgs::msg::CANA(); - - // Brake Release command (pretty sure brakes don't work) - joint_tx.data = {0x77, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); - } + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Activating ...please wait..."); + + joint_command_position_ = joint_state_position_; - for (size_t i = 0; i < initial_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu initial position in on_activate: %f", i, initial_position_[i]); - } - - // Initialize command positions and velocities to zero to prevent initial movement - // Controllers will override these once they start - joint_command_position_.assign(num_joints, 0.0); - joint_command_velocity_.assign(num_joints, 0.0); - for (size_t i = 0; i < joint_command_position_.size(); ++i) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Joint %zu command velocity initialized to: %f", i, joint_command_velocity_[i]); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); } - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully activated!"); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper hardware activated"); + return hardware_interface::CallbackReturn::SUCCESS; } - hardware_interface::CallbackReturn STEPPERHardwareInterface::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Deactivating ...please wait..."); - - for(int i = 0; i < num_joints; i++){ - auto joint_tx = msgs::msg::CANA(); - - // Motor Stop Command - joint_tx.data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - science_can_publisher_->publish(joint_tx); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Deactivating stepper hardware..."); + + if (can_connected_) { + 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; + + // (MAINTENANCE_CMD << 4) | port_id = 0x60 | port_id, maintenance cmd 1 = Stop stepper + uint8_t device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((MAINTENANCE_CMD << 4) | device_id_nibble); + can_tx_frame_.data[1] = 1; // Maintenance cmd 1 = Stop stepper + canBus_.send(can_tx_frame_); + } } - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Successfully deactivated all STEPPER motors!"); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper hardware deactivated%s", can_connected_ ? "" : " (simulated)"); return hardware_interface::CallbackReturn::SUCCESS; } -double STEPPERHardwareInterface::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 STEPPERHardwareInterface::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; -} +// can_connected_ and is_connected_ are always reset regardless of whether CAN was open. +hardware_interface::CallbackReturn STEPPERHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Cleaning up stepper hardware..."); + + if (can_connected_) { + 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; + + // FIX #4: Assign device_id_nibble (was previously using uninitialized variable) + uint8_t device_id_nibble = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((MAINTENANCE_CMD << 4) | device_id_nibble); + can_tx_frame_.data[1] = 2; // Maintenance cmd 2 = Shutdown stepper + canBus_.send(can_tx_frame_); + } -int32_t STEPPERHardwareInterface::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)); -} + canBus_.close(); + } -int32_t STEPPERHardwareInterface::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)); -} + // Always reset connection state + can_connected_ = false; + is_connected_ = 0.0; + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Stepper hardware cleanup complete"); + return hardware_interface::CallbackReturn::SUCCESS; +} hardware_interface::return_type STEPPERHardwareInterface::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - - if (!node_) { - RCLCPP_ERROR(rclcpp::get_logger("STEPPERHardwareInterface"), - "read() called but node_ is nullptr!"); - return hardware_interface::return_type::ERROR; + if (can_connected_) { + current_joint_ = (current_joint_ + 1) % num_joints; + + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 2; + + // MOTOR_STATE_CMD = 0x4 → (0x4 << 4) | port_id = 0x40 | port_id + uint8_t port_id = joint_node_ids[current_joint_] & 0x0F; + can_tx_frame_.data[0] = static_cast((MOTOR_STATE_CMD << 4) | port_id); + can_tx_frame_.data[1] = 0x01; // Validate the request + canBus_.send(can_tx_frame_); } - if (!science_can_publisher_) { - RCLCPP_ERROR(rclcpp::get_logger("STEPPERHardwareInterface"), - "read() called but science_can_publisher_ is nullptr!"); - return hardware_interface::return_type::ERROR; - } + // Copy motor state (updated asynchronously by onCanMessage) into joint state + for (int i = 0; i < num_joints; i++) { + joint_state_position_[i] = motor_position[i]; + joint_state_velocity_[i] = motor_velocity[i]; - if (rclcpp::ok()) - { - rclcpp::spin_some(node_); + // Return error on any fault status + if (device_status[i] != 0x00 && device_status[i] != 0x01 && device_status[i] != -1) { + RCLCPP_ERROR( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Joint %s has fault device_status=0x%02X", info_.joints[i].name.c_str(), device_status[i]); + return hardware_interface::return_type::ERROR; + } } - 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 - current_iteration++; - for(int i = 0; i < num_joints; i++) { - auto joint_tx = msgs::msg::CANA(); - if(current_iteration%2 == 0){ - // Command to read multi-turn angle - - joint_tx.data = {0x92, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - } - else if(current_iteration%2 == 1){ - // Command to read motor status 2 + return hardware_interface::return_type::OK; +} - joint_tx.data = {0x9C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - } - science_can_publisher_->publish(joint_tx); - } +hardware_interface::return_type STEPPERHardwareInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + elapsed_update_time += period.seconds(); + double update_period = 1.0 / update_rate; - // Safety check - if (received_joint_data_.data.size() < 8) { - if (DEBUG_MODE == 1) { - RCLCPP_INFO( - rclcpp::get_logger("STEPPERHardwareInterface"), - "read(): received_joint_data_ has only %zu bytes, waiting for full frame", - received_joint_data_.data.size()); - } + // Rate-limit CAN messages to configured update rate + if (elapsed_update_time < update_period) { return hardware_interface::return_type::OK; } - // Values retrieved - for(int i = 0; i < num_joints; i++){ - if(received_joint_data_.data[0] == 0x9C) { - - // DECODING CAN MESSAGE FOR VELOCITY - data[0] = 0x9C; - data[1] = received_joint_data_.data[1]; // Motor Temperature - data[2] = received_joint_data_.data[2]; // Torque low byte - data[3] = received_joint_data_.data[3]; // Torque high byte - data[4] = received_joint_data_.data[4]; // speed low byte - data[5] = received_joint_data_.data[5]; // speed high byte - data[6] = received_joint_data_.data[6]; // encoder position low byte - data[7] = received_joint_data_.data[7]; // encoder position high byte - - // ENCODER POSITION - // uint16 -> int16 -> double (for calcs) - encoder_position = static_cast(static_cast((data[7] << 8) | data[6])); - - // SPEED - // uint16 -> int16 -> double (for calcs) - motor_velocity = static_cast(static_cast((data[5] << 8) | data[4])); - - // CALCULATING JOINT STATE - joint_state_velocity_[i] = calculate_joint_velocity_from_motor_velocity(motor_velocity, joint_gear_ratios[i]); - - // TEMPERATURE (1 byte, degrees C) - motor_temperature_[i] = static_cast(data[1]); - - // TORQUE CURRENT (16-bit signed, 0.01A per LSB) - motor_torque_current_[i] = static_cast(static_cast((data[3] << 8) | data[2])) * 0.01; - - } - else if(received_joint_data_.data[0] == 0x92) { - - // DECODING CAN MESSAGE FOR POSITION - data[0] = 0x92; - data[1] = 0x00; - data[2] = 0x00; - data[3] = 0x00; - data[4] = received_joint_data_.data[4]; // Multi-turn low byte - data[5] = received_joint_data_.data[5]; - data[6] = received_joint_data_.data[6]; - data[7] = received_joint_data_.data[7]; // Multi-turn high byte - - // POSITION - // uint32 -> int32 -> double (for calcs) - motor_position = 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("STEPPERHardwareInterface"), "Received at index %d: %d", j, received_joint_data_.data[j]); - } - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Motor Position: %f", motor_position); + elapsed_update_time = 0.0; + elapsed_time += period.seconds(); + + for (int i = 0; i < num_joints; i++) { + if (control_level_[i] == integration_level_t::VELOCITY && + std::isfinite(joint_command_velocity_[i])) + { + // Stepper only accepts three discrete speeds: +900, -900, or 0 (deg/s). + // Map the signed velocity command to the nearest valid value. + int16_t speed_dps; + if (joint_command_velocity_[i] > 0.0) { + speed_dps = 900; + } else if (joint_command_velocity_[i] < 0.0) { + speed_dps = -900; + } else { + speed_dps = 0; } - - // CALCULATING JOINT STATE - joint_state_position_[i] = calculate_joint_position_from_motor_position(motor_position, joint_gear_ratios[i]); - - // This initializes the command position for the joint (will not work if state isn't read immediately) - // if(!joint_initialization_[i]){ - // joint_command_position_[i] = joint_state_position_[i]; - // joint_initialization_[i] = true; - // } - } - else{ - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Reply not heard."); - } - } - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Reading for joint: %s Motor Position: %f Joint position: %f Joint velocity: %f \nJoint Initialization State: %d\n", - info_.joints[i].name.c_str(), - motor_position, - joint_state_position_[i], - joint_state_velocity_[i], - static_cast(joint_initialization_[i])); - } + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 3; // 3 bytes per spec - } - return hardware_interface::return_type::OK; -} + // VELOCITY_CONTROL_CMD = 0x3 → (0x3 << 4) | port_id = 0x30 | port_id + uint8_t port_id = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((VELOCITY_CONTROL_CMD << 4) | port_id); + can_tx_frame_.data[1] = static_cast(speed_dps & 0xFF); // low byte + can_tx_frame_.data[2] = static_cast((speed_dps >> 8) & 0xFF); // high byte - -hardware_interface::return_type stepper_ros2_control::STEPPERHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) -{ - int data[8] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; - int32_t joint_angle = 0; - int16_t operating_velocity = 200; - int32_t joint_velocity = 0; - - for(int i = 0; i < num_joints; i++) { - auto joint_tx = msgs::msg::CANA(); // Must reinstantiate else data from past iteration gets repeated - - - 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("STEPPERHardwareInterface"), "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]); + canBus_.send(can_tx_frame_); } - - // 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; - } - 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]); - - if(DEBUG_MODE == 1) { - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Writing velocities for: %s Joint velocity of motor (0.01 dps): %d Joint command velocity: %f Orientation: %d", - info_.joints[i].name.c_str(), - joint_velocity, - joint_command_velocity_[i], - joint_orientation[i]); + else if (control_level_[i] == integration_level_t::POSITION && + std::isfinite(joint_command_position_[i])) + { + // Convert rad -> deg, apply gear ratio, clamp to int16 range + int16_t position_deg = static_cast(std::clamp( + joint_command_position_[i] * (180.0 / M_PI) * joint_gear_ratios[i], + static_cast(std::numeric_limits::min()), + static_cast(std::numeric_limits::max()) + )); + + if (can_connected_) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + can_tx_frame_.dlc = 3; + + // ABSOLUTE_POS_CONTROL_CMD = 0x2 → (0x2 << 4) | port_id = 0x20 | port_id + uint8_t port_id = joint_node_ids[i] & 0x0F; + can_tx_frame_.data[0] = static_cast((ABSOLUTE_POS_CONTROL_CMD << 4) | port_id); + can_tx_frame_.data[1] = static_cast(position_deg & 0xFF); // low byte + can_tx_frame_.data[2] = static_cast((position_deg >> 8) & 0xFF); // high byte + + canBus_.send(can_tx_frame_); } - - // 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("STEPPERHardwareInterface"), "Joint command value not found or undefined command state"); } + } - for(int j = 0; j < 8; j++){ - joint_tx.data.push_back(static_cast(data[j])); + // Optional logger + if (logger_state == 1) { + elapsed_logger_time += period.seconds(); + double logger_period = 1.0 / logger_rate; + if (elapsed_logger_time >= logger_period) { + elapsed_logger_time = 0.0; + logger_function(); } - - science_can_publisher_->publish(joint_tx); } - - return hardware_interface::return_type::OK; } -// hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_switch( -// 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); -// } -// } -// } -// // 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) { -// // fix this -// joint_command_position_[i] = initial_position_[i]; -// joint_command_velocity_[i] = 0; -// control_level_[i] = integration_level_t::UNDEFINED; // Revert to undefined -// } -// } -// } -// // 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; -// } -// control_level_[i] = new_modes[i]; -// } - -// return hardware_interface::return_type::OK; -// } - hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_switch( const std::vector& start_interfaces, const std::vector& stop_interfaces) { - // 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("STEPPERHardwareInterface"), ss.str().c_str()); + RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "%s", 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 + // Process stop interfaces first 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) { + 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; } } } - // Process start interfaces: set POSITION or VELOCITY per joint + // Process start interfaces 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); + 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) { @@ -570,12 +538,9 @@ hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_s } } - // 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. + // Apply requested modes 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) { @@ -584,29 +549,25 @@ hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_s } } if (was_stopped) { - control_level_[i] = integration_level_t::UNDEFINED; + 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("STEPPERHardwareInterface"), + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), "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("STEPPERHardwareInterface"), - "Joint %s: switched to VELOCITY (cmd vel initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_velocity_[i]); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Joint %s: switched to VELOCITY", info_.joints[i].name.c_str()); } else if (requested_modes[i] == integration_level_t::POSITION) { joint_command_position_[i] = joint_state_position_[i]; - RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), - "Joint %s: switched to POSITION (cmd pos initialized from state: %f)", - info_.joints[i].name.c_str(), joint_command_position_[i]); + RCLCPP_INFO( + rclcpp::get_logger("STEPPERHardwareInterface"), + "Joint %s: switched to POSITION", info_.joints[i].name.c_str()); } } } @@ -614,10 +575,10 @@ hardware_interface::return_type STEPPERHardwareInterface::perform_command_mode_s return hardware_interface::return_type::OK; } - -} // namespace stepper_hardware_interface +} // namespace stepper_ros2_control #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - stepper_ros2_control::STEPPERHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file + stepper_ros2_control::STEPPERHardwareInterface, + hardware_interface::SystemInterface) \ No newline at end of file