diff --git a/.gitignore b/.gitignore index 86668ec0..2bf691f1 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ build/ install/ log/ .vscode/ +.codex # Byte-compiled / optimized / DLL files __pycache__/ 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.dc.ros2_control.xacro b/src/description/ros2_control/science/science.dc.ros2_control.xacro new file mode 100644 index 00000000..442585dc --- /dev/null +++ b/src/description/ros2_control/science/science.dc.ros2_control.xacro @@ -0,0 +1,49 @@ + + + + + + + + + dc_ros2_control/DCHardwareInterface + 20 + 5 + 1 + can0 + 0x70 + + + + 0 + revolute + + 50.9 + true + + + + + + + + + + 2 + revolute + + 50.9 + true + + + + + + + + + + + + + \ 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.servo.ros2_control.xacro b/src/description/ros2_control/science/science.servo.ros2_control.xacro index c441e9b5..bf6af20a 100644 --- a/src/description/ros2_control/science/science.servo.ros2_control.xacro +++ b/src/description/ros2_control/science/science.servo.ros2_control.xacro @@ -6,7 +6,7 @@ servo_ros2_control/SERVOHardwareInterface 20 5 - 1 + 0 can0 0x80 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..224e3b9a 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 + 0x0 + 1 + 20 + continuous - 0.0 - - - 1 - - - 1 - + + 0x1 + 1 + 20 + continuous - 0.0 - - - 1 - - - 1 - - + \ No newline at end of file diff --git a/src/description/urdf/athena_science.urdf.xacro b/src/description/urdf/athena_science.urdf.xacro index 80fc5314..95e14105 100644 --- a/src/description/urdf/athena_science.urdf.xacro +++ b/src/description/urdf/athena_science.urdf.xacro @@ -6,8 +6,9 @@ - - + + + @@ -30,6 +31,9 @@ + + + diff --git a/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt b/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..4c11c388 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.8) +project(dc_ros2_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + umdloop_can_library +) + +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +include_directories(include) + +add_library(dc_ros2_control SHARED + src/dc_hardware_interface.cpp +) + +target_compile_features(dc_ros2_control PUBLIC cxx_std_20) + +target_include_directories(dc_ros2_control PUBLIC + $ + $ +) + +# Link umdloop_can_library directly via CMake target (same as servo HWI) +target_link_libraries(dc_ros2_control PUBLIC umdloop_can_library::umdloop_can_library) + +ament_target_dependencies(dc_ros2_control PUBLIC + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) + +# Export hardware plugin +pluginlib_export_plugin_description_file(hardware_interface dc_hardware_interface.xml) + +# Install +install( + DIRECTORY include/ + DESTINATION include/dc_ros2_control +) + +install(TARGETS dc_ros2_control + EXPORT export_dc_ros2_control + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_targets(export_dc_ros2_control) +ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml b/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml new file mode 100644 index 00000000..76bd8b7e --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/dc_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + UMDLoop's ros2_control plugin for the DC motors + + + \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp b/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp new file mode 100644 index 00000000..5284c343 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/include/dc_ros2_control/dc_hardware_interface.hpp @@ -0,0 +1,176 @@ +#ifndef DC_HARDWARE_INTERFACE_HPP_ +#define DC_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" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include +#include +#include + +#include "umdloop_can_library/SocketCanBus.hpp" +#include "umdloop_can_library/CanFrame.hpp" + +namespace dc_ros2_control +{ +class DCHardwareInterface : public hardware_interface::SystemInterface +{ +public: + RCLCPP_SHARED_PTR_DEFINITIONS(DCHardwareInterface) + + // Lifecycle + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + 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; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::return_type perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) override; + + hardware_interface::return_type read( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + hardware_interface::return_type write( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + // -- Helper Functions -- + void on_can_message(const CANLib::CanFrame& frame); + void logger_function(); + + // Unit conversions + double calculate_joint_position_from_motor_position(double motor_pos_deg, int gear_ratio); + double calculate_joint_displacement_from_motor_position(double motor_pos_deg, int gear_ratio, double dist_per_rev); + double calculate_joint_angular_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio); + double calculate_joint_linear_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio, double dist_per_rev); + + 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 dist_per_rev); + 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 dist_per_rev); + +private: + // Hardware Interface Parameters + int update_rate; + double elapsed_update_time; + double elapsed_time; + double elapsed_logger_time; + int logger_rate; + int logger_state; + int write_count; + + int num_joints; + + // Stores Arbitration IDs + int can_command_id; + uint32_t can_response_id; + + // Joint state (read from hardware) + std::vector joint_state_position_; + std::vector joint_state_velocity_; + + // Previous joint state (change detection) + std::vector prev_joint_state_position_; + std::vector prev_joint_state_velocity_; + + // Joint commands (written to hardware) + std::vector joint_command_position_; + std::vector joint_command_velocity_; + + // Previous joint commands (change detection) + std::vector prev_joint_command_position_; + std::vector prev_joint_command_velocity_; + + // Raw motor data from CAN bus (populated in on_can_message, consumed in read) + std::vector motor_position; + std::vector motor_velocity; + std::vector device_status; + + // Telemetry data (TODO: implement CAN telemetry commands) + std::vector motor_temperature_; + std::vector motor_torque_current_; + + // Maximum displacement for prismatic joints + std::vector max_disp; + + // Prismatic-specific: distance per revolution (future use) + std::vector distance_per_rev; + + // CAN Library + CANLib::SocketCanBus canBus; + CANLib::CanFrame can_tx_frame_; + CANLib::CanFrame can_rx_frame_; + std::string can_interface; + + // Per-joint parameters (read from XACRO) + std::vector joint_node_ids; + std::vector joint_gear_ratios; + std::vector joint_inverted; + + // Control mode + enum class integration_level_t : std::uint8_t + { + UNDEFINED = 0, + POSITION = 1, + VELOCITY = 2, + }; + + std::vector control_level_; + + // Joint type + enum class joint_type_t : std::uint8_t + { + REVOLUTE = 0, + PRISMATIC = 1, + }; + + std::vector joint_type_; + + // CAN Command Bytes (full byte values, used as: CMD + port_id) + 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 DC_SPECS_CMD = 0x70; + + // DC Motor Maintenance Sub-Commands + static constexpr uint8_t MAINTENANCE_ZERO_ROM = 0; + static constexpr uint8_t MAINTENANCE_REQ_VECTORS = 1; + static constexpr uint8_t MAINTENANCE_STOP = 2; + static constexpr uint8_t MAINTENANCE_SHUTDOWN = 3; + static constexpr uint8_t MAINTENANCE_CLEAR_ERRORS = 4; +}; + +} // namespace dc_ros2_control + +#endif // DC_HARDWARE_INTERFACE_HPP_ \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/package.xml b/src/hardware_interfaces/dc_ros2_control/package.xml new file mode 100644 index 00000000..6f174cc8 --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/package.xml @@ -0,0 +1,35 @@ + + + + dc_ros2_control + 0.0.0 + TODO: Package description + henry + TODO: License declaration + + ament_cmake + ament_cmake + + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + msgs + + + umdloop_can_library + umdloop_can_library + + ament_lint_auto + ament_lint_common + + ament_cmake + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + \ No newline at end of file diff --git a/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp new file mode 100644 index 00000000..6a92f5ed --- /dev/null +++ b/src/hardware_interfaces/dc_ros2_control/src/dc_hardware_interface.cpp @@ -0,0 +1,617 @@ +#include "dc_ros2_control/dc_hardware_interface.hpp" + +#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" + +namespace dc_ros2_control +{ + +// ============================================================================= +// Unit Conversions (matches servo HWI structure) +// ============================================================================= + +double DCHardwareInterface::calculate_joint_position_from_motor_position(double motor_pos_deg, int gear_ratio) { + // deg -> radians with gear ratio + return (motor_pos_deg * (M_PI / 180.0)) / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_displacement_from_motor_position(double motor_pos_deg, int gear_ratio, double dist_per_rev) { + // deg -> meters: (deg / 360) * distance_per_rev / gear_ratio + return (motor_pos_deg / 360.0) * dist_per_rev / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_angular_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio) { + // deg/s -> rad/s with gear ratio + return (motor_vel_dps * (M_PI / 180.0)) / gear_ratio; +} + +double DCHardwareInterface::calculate_joint_linear_velocity_from_motor_velocity(double motor_vel_dps, int gear_ratio, double dist_per_rev) { + // deg/s -> m/s + return (motor_vel_dps / 360.0) * dist_per_rev / gear_ratio; +} + +int16_t DCHardwareInterface::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.0 / M_PI)) * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_position_from_desired_joint_displacement(double joint_position, int gear_ratio, double dist_per_rev) { + // meters -> deg: (m / distance_per_rev) * 360 * gear_ratio + return static_cast(std::round((joint_position / dist_per_rev) * 360.0 * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_velocity_from_desired_joint_angular_velocity(double joint_velocity, int gear_ratio) { + // rad/s -> deg/s with gear ratio + return static_cast(std::round((joint_velocity * (180.0 / M_PI)) * gear_ratio)); +} + +int16_t DCHardwareInterface::calculate_motor_velocity_from_desired_joint_linear_velocity(double joint_velocity, int gear_ratio, double dist_per_rev) { + // m/s -> deg/s + return static_cast(std::round((joint_velocity / dist_per_rev) * 360.0 * gear_ratio)); +} + +// ============================================================================= +// Logger +// ============================================================================= + +void DCHardwareInterface::logger_function() +{ + if (num_joints == 0) return; + + std::string log_msg = "\033[2J\033[H \nDC Motor Logger"; + std::ostringstream oss; + std::string control_mode; + std::string joint_type_str; + std::string status; + + 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: " << std::dec << 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 (control_level_[i]) { + case integration_level_t::POSITION: control_mode = "POSITION"; break; + case integration_level_t::VELOCITY: control_mode = "VELOCITY"; break; + default: control_mode = "UNDEFINED"; break; + } + + switch (joint_type_[i]) { + case joint_type_t::REVOLUTE: joint_type_str = "REVOLUTE"; break; + case joint_type_t::PRISMATIC: joint_type_str = "PRISMATIC"; break; + } + + switch (device_status[i]) { + case 0: status = "Undefined"; break; + case 1: status = "Idle"; break; + case 2: status = "Startup Sequence"; break; + case 3: status = "Error (Invalid Request)"; break; + case 4: status = "Error (Motor Disarmed)"; break; + case 5: status = "Error (Motor Failed)"; break; + case 6: status = "Error (Controller Failed)"; break; + case 7: status = "Error (ESTOP Requested)"; break; + case 8: status = "Error (Unknown Position)"; break; + case 9: status = "Position Control"; break; + case 10: status = "Velocity Control"; break; + case 11: status = "Motor Stopped"; break; + default: status = "UNKNOWN (" + std::to_string(device_status[i]) + ")"; break; + } + + oss << "\nJOINT: " << info_.joints[i].name << "\n" + << "Parameters: Node ID: 0x" << std::hex << std::uppercase << joint_node_ids[i] + << " | Gear Ratio: " << std::dec << joint_gear_ratios[i] + << " | Inverted: " << (joint_inverted[i] ? "true" : "false") + << " | Device Status: " << status + << " | Control Mode: " << control_mode + << " | Joint Type: " << joint_type_str << "\n" + << "-- Commands --\n" + << "Joint Command Position: " << joint_command_position_[i] + << " | Joint Command Velocity: " << joint_command_velocity_[i] << "\n" + << "-- State --\n" + << "Joint Position: " << joint_state_position_[i] + << " | Joint Velocity: " << joint_state_velocity_[i] << "\n" + << "-- Telemetry --\n" + << "Temperature: " << motor_temperature_[i] << " C" + << " | Torque Current: " << motor_torque_current_[i] << " A\n"; + } + + log_msg += oss.str(); + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "%s", log_msg.c_str()); +} + +// ============================================================================= +// Initialization +// ============================================================================= + +hardware_interface::CallbackReturn DCHardwareInterface::on_init( + const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != + hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + + // -- Per-joint parameters -- + for (auto& joint : info_.joints) { + joint_node_ids.push_back( + std::clamp(std::stoi(joint.parameters.at("node_id"), nullptr, 0), 0x0, 0xF)); + + // Gear ratio (default: 1) + if (joint.parameters.count("gear_ratio")) { + joint_gear_ratios.push_back(std::abs(std::stoi(joint.parameters.at("gear_ratio")))); + } else { + joint_gear_ratios.push_back(1); + } + + // Inverted (default: false) + if (joint.parameters.count("inverted")) { + joint_inverted.push_back(joint.parameters.at("inverted") == "true"); + } else { + joint_inverted.push_back(false); + } + + // Joint type (default: revolute) + std::string jtype = "revolute"; + if (joint.parameters.count("joint_type")) { + jtype = joint.parameters.at("joint_type"); + } + + if (jtype == "revolute") { + joint_type_.push_back(joint_type_t::REVOLUTE); + max_disp.push_back(std::nan("")); + distance_per_rev.push_back(0.0); + } else if (jtype == "prismatic" && joint.parameters.count("max_disp") && joint.parameters.count("distance_per_rev")) { + joint_type_.push_back(joint_type_t::PRISMATIC); + max_disp.push_back(std::abs(std::stod(joint.parameters.at("max_disp")))); + distance_per_rev.push_back(std::stod(joint.parameters.at("distance_per_rev"))); + } else { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Invalid joint_type for joint %s. Must be 'revolute' or 'prismatic' (with 'max_disp' and 'distance_per_rev').", + joint.name.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + } + + // -- Hardware-level parameters -- + 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; + write_count = 0; + + // -- Command and State Interface initialization -- + joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + prev_joint_state_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_state_velocity_.assign(num_joints, 0.0); + prev_joint_state_velocity_.assign(num_joints, 0.0); + + joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + prev_joint_command_position_.assign(num_joints, std::numeric_limits::quiet_NaN()); + joint_command_velocity_.assign(num_joints, 0.0); + prev_joint_command_velocity_.assign(num_joints, 0.0); + + motor_position.assign(num_joints, 0.0); + motor_velocity.assign(num_joints, 0.0); + device_status.assign(num_joints, 0); + + // Telemetry placeholders (TODO: populate via CAN telemetry command) + motor_temperature_.assign(num_joints, 0.0); + motor_torque_current_.assign(num_joints, 0.0); + + // Default control mode: position + control_level_.resize(num_joints, integration_level_t::POSITION); + + return hardware_interface::CallbackReturn::SUCCESS; +} + +// ============================================================================= +// Lifecycle +// ============================================================================= + +hardware_interface::CallbackReturn DCHardwareInterface::on_shutdown( + const rclcpp_lifecycle::State & previous_state) +{ + return this->on_cleanup(previous_state); +} + +std::vector +DCHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + 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 (TODO: populate via CAN telemetry command) + 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])); + } + + return state_interfaces; +} + +std::vector +DCHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + + 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])); + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Configuring DC Motor HWI..."); + + if (!canBus.open(can_interface, + std::bind(&DCHardwareInterface::on_can_message, this, std::placeholders::_1))) + { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Failed to open CAN interface: %s", can_interface.c_str()); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "CAN interface opened successfully."); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_cleanup( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleaning up... please wait..."); + + // Send shutdown command (maintenance sub-command 3) to all joints + 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; + can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = MAINTENANCE_SHUTDOWN; + canBus.send(can_tx_frame_); + } + + canBus.close(); + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Cleanup complete."); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Activating... please wait..."); + + // Initialize command positions to current state + joint_command_position_ = joint_state_position_; + + for (size_t i = 0; i < joint_command_position_.size(); ++i) { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %zu command position initialized to: %f", i, joint_command_position_[i]); + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully activated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn DCHardwareInterface::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Deactivating... please wait..."); + + // Send stop command (maintenance sub-command 2) to all joints + 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; + can_tx_frame_.data[0] = MAINTENANCE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = MAINTENANCE_STOP; + canBus.send(can_tx_frame_); + } + + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "Successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +// ============================================================================= +// CAN Receive Callback +// ============================================================================= + +void DCHardwareInterface::on_can_message(const CANLib::CanFrame& frame) +{ + can_rx_frame_ = frame; + + int data[7] = {0x00}; + uint8_t cmd_byte = can_rx_frame_.data[0]; + uint8_t command_nibble = cmd_byte & 0xF0; + uint8_t device_id_nibble = cmd_byte & 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 || device_id_nibble != joint_node_ids[i]) + continue; + + if (command_nibble == MOTOR_STATE_CMD || + command_nibble == ABSOLUTE_POS_CONTROL_CMD || + command_nibble == VELOCITY_CONTROL_CMD) + { + // DECODING CAN MESSAGE + 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: int16 sign extension (NOT int32) + raw_motor_position = static_cast(static_cast((data[2] << 8) | data[1])); + + // VELOCITY: int16 + raw_motor_velocity = static_cast(static_cast((data[4] << 8) | data[3])); + + // Apply inversion + double dir = joint_inverted[i] ? -1.0 : 1.0; + + // CALCULATING JOINT STATE + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_position[i] = dir * calculate_joint_position_from_motor_position(raw_motor_position, joint_gear_ratios[i]); + motor_velocity[i] = dir * calculate_joint_angular_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i]); + } + else if (joint_type_[i] == joint_type_t::PRISMATIC) { + motor_position[i] = dir * calculate_joint_displacement_from_motor_position(raw_motor_position, joint_gear_ratios[i], distance_per_rev[i]); + motor_velocity[i] = dir * calculate_joint_linear_velocity_from_motor_velocity(raw_motor_velocity, joint_gear_ratios[i], distance_per_rev[i]); + } + else { + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "The joint type for joint %s is undefined.", info_.joints[i].name.c_str()); + } + } + else if (command_nibble == MOTOR_STATUS_CMD) + { + device_status[i] = can_rx_frame_.data[1]; + } + // TODO: Handle telemetry response frames here when implemented + } +} + +// ============================================================================= +// Read (update state from CAN data) +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::read( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + for (int i = 0; i < num_joints; i++) { + if (prev_joint_state_velocity_[i] != motor_velocity[i]) { + joint_state_velocity_[i] = motor_velocity[i]; + prev_joint_state_velocity_[i] = joint_state_velocity_[i]; + } + + if (prev_joint_state_position_[i] != motor_position[i]) { + joint_state_position_[i] = motor_position[i]; + prev_joint_state_position_[i] = joint_state_position_[i]; + } + + // Check device status — allow Undefined(0), Idle(1), Position Control(9), Velocity Control(10) + int s = device_status[i]; + if (s != 0 && s != 1 && s != 9 && s != 10) { + RCLCPP_ERROR(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s in error state: %d", info_.joints[i].name.c_str(), s); + return hardware_interface::return_type::ERROR; + } + + // TODO: Telemetry — populate motor_temperature_[i] and motor_torque_current_[i] via CAN + } + + return hardware_interface::return_type::OK; +} + +// ============================================================================= +// Write (send CAN commands) +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::write( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + elapsed_update_time += period.seconds(); + elapsed_time += period.seconds(); + double update_period = 1.0 / update_rate; + + // 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(); + } + } + + // Rate-limited CAN writes — all joints per tick + if (elapsed_update_time > update_period) { + elapsed_update_time = 0.0; + + for (int i = 0; i < num_joints; i++) { + can_tx_frame_ = CANLib::CanFrame(); + can_tx_frame_.id = can_command_id; + + // Apply inversion for outgoing commands + double dir = joint_inverted[i] ? -1.0 : 1.0; + + if (control_level_[i] == integration_level_t::POSITION && + std::isfinite(joint_command_position_[i]) && + joint_command_position_[i] != prev_joint_command_position_[i]) + { + // Clamp prismatic joints to [0, max_disp] + if (joint_type_[i] == joint_type_t::PRISMATIC) { + joint_command_position_[i] = std::clamp( + joint_command_position_[i], 0.0, max_disp[i]); + } + + int16_t motor_pos; + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_pos = calculate_motor_position_from_desired_joint_position( + dir * joint_command_position_[i], joint_gear_ratios[i]); + } else { + motor_pos = calculate_motor_position_from_desired_joint_displacement( + dir * joint_command_position_[i], joint_gear_ratios[i], distance_per_rev[i]); + } + + can_tx_frame_.dlc = 3; + can_tx_frame_.data[0] = ABSOLUTE_POS_CONTROL_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = static_cast(motor_pos & 0xFF); + can_tx_frame_.data[2] = static_cast((motor_pos >> 8) & 0xFF); + + prev_joint_command_position_[i] = joint_command_position_[i]; + } + else if (control_level_[i] == integration_level_t::VELOCITY && + std::isfinite(joint_command_velocity_[i]) && + joint_command_velocity_[i] != prev_joint_command_velocity_[i]) + { + int16_t motor_vel; + if (joint_type_[i] == joint_type_t::REVOLUTE) { + motor_vel = calculate_motor_velocity_from_desired_joint_angular_velocity( + dir * joint_command_velocity_[i], joint_gear_ratios[i]); + } else { + motor_vel = calculate_motor_velocity_from_desired_joint_linear_velocity( + dir * joint_command_velocity_[i], joint_gear_ratios[i], distance_per_rev[i]); + } + + can_tx_frame_.dlc = 3; + can_tx_frame_.data[0] = VELOCITY_CONTROL_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = static_cast(motor_vel & 0xFF); + can_tx_frame_.data[2] = static_cast((motor_vel >> 8) & 0xFF); + + prev_joint_command_velocity_[i] = joint_command_velocity_[i]; + } + else + { + // No new command — poll motor state + can_tx_frame_.dlc = 2; + can_tx_frame_.data[0] = MOTOR_STATE_CMD + (joint_node_ids[i] & 0x0F); + can_tx_frame_.data[1] = 1; // Validate request + } + + canBus.send(can_tx_frame_); + } + } + + return hardware_interface::return_type::OK; +} + +// ============================================================================= +// Command Mode Switch +// ============================================================================= + +hardware_interface::return_type DCHardwareInterface::perform_command_mode_switch( + const std::vector& start_interfaces, + const std::vector& stop_interfaces) +{ + std::ostringstream ss; + ss << "perform_command_mode_switch called. start: ["; + for (auto &s : start_interfaces) ss << s << ","; + ss << "] stop: ["; + for (auto &s : stop_interfaces) ss << s << ","; + ss << "]"; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), "%s", ss.str().c_str()); + + std::vector requested_modes(num_joints, integration_level_t::UNDEFINED); + + // Process stop interfaces + for (const auto &ifname : stop_interfaces) { + for (int i = 0; i < num_joints; ++i) { + if (ifname.find(info_.joints[i].name) != std::string::npos) { + requested_modes[i] = integration_level_t::UNDEFINED; + } + } + } + + // 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); + if (ifname == pos_if) { + requested_modes[i] = integration_level_t::POSITION; + } else if (ifname == vel_if) { + requested_modes[i] = integration_level_t::VELOCITY; + } + } + } + + // Apply modes + for (int i = 0; i < num_joints; ++i) { + if (requested_modes[i] == integration_level_t::UNDEFINED) { + 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; + joint_command_position_[i] = joint_state_position_[i]; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "Joint %s: stopped -> UNDEFINED", info_.joints[i].name.c_str()); + } + } else { + control_level_[i] = requested_modes[i]; + if (requested_modes[i] == integration_level_t::VELOCITY) { + joint_command_velocity_[i] = 0; + RCLCPP_INFO(rclcpp::get_logger("DCHardwareInterface"), + "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("DCHardwareInterface"), + "Joint %s: switched to POSITION (initialized to %f)", + info_.joints[i].name.c_str(), joint_command_position_[i]); + } + } + } + + return hardware_interface::return_type::OK; +} + +} // namespace dc_ros2_control + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + dc_ros2_control::DCHardwareInterface, hardware_interface::SystemInterface) \ No newline at end of file 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 89439f77..31267cf1 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 @@ -547,12 +547,12 @@ hardware_interface::return_type servo_ros2_control::SERVOHardwareInterface::writ RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint %s is standard type and cannot be velocity controlled.", info_.joints[i].name.c_str()); } else{ - RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state for joint %s", info_.joints[i].name.c_str()); } + // RCLCPP_WARN(rclcpp::get_logger("SERVOHardwareInterface"), "Joint command value not found or undefined command state for joint %s", info_.joints[i].name.c_str()); } } } canBus.send(can_tx_frame_); - // } + } } return hardware_interface::return_type::OK; 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..27942946 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; + + // Initialize timing + elapsed_update_time = 0.0; + elapsed_time = 0.0; + elapsed_logger_time = 0.0; + current_joint_ = 0; - motor_temperature_.assign(num_joints, 0.0); - motor_torque_current_.assign(num_joints, 0.0); + // Initialize state variables + is_connected_ = 0.0; + can_connected_ = false; - encoder_position = 0.0; - motor_position = 0.0; - motor_velocity = 0.0; + // 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); - joint_initialization_.assign(num_joints, false); + // 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); - control_level_.resize(num_joints, integration_level_t::POSITION); + // Steppers default to velocity control + control_level_.resize(num_joints, integration_level_t::VELOCITY); - current_iteration = 0; + 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 - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, "motor_temperature", &motor_temperature_[i])); + if (num_joints > 0) { 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,326 @@ 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); - } - - // 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."); + 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; } - } - 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 & 0xF0) | 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 + RCLCPP_INFO(rclcpp::get_logger("STEPPERHardwareInterface"), "Data[0]: 0x%02X | Data[1-2] (speed_dps): %d | Joint: %s", + can_tx_frame_.data[0], speed_dps, info_.joints[i].name.c_str()); -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 +541,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 +552,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 +578,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 diff --git a/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp b/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp index 0b153200..77c24b43 100644 --- a/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp +++ b/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp @@ -84,7 +84,7 @@ bool SocketCanBus::open(const std::string& interface_name, ReceiveCallback callb } // Disable loopback (do not receive our own transmitted frames) - int loopback = 0; + int loopback = 1; setsockopt(socketFd_, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback)); // Disable CAN FD — we only use classical CAN frames diff --git a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml index 299f7b03..d47bf6dd 100644 --- a/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml +++ b/src/subsystems/science/science_bringup/config/athena_science_controllers.yaml @@ -31,10 +31,10 @@ joint_group_position_controller: - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - talon_scoop + - dc_scoop - servo_sampler_lift_l - servo_sampler_lift_l - - talon_auger + - dc_auger - servo_auger_lift interface_name: position @@ -47,10 +47,10 @@ joint_group_velocity_controller: - servo_rack_and_pinion_r - servo_scoop_a - servo_scoop_b - - talon_scoop + - dc_scoop - servo_sampler_lift_l - servo_sampler_lift_r - - talon_auger + - dc_auger - servo_auger_lift interface_name: velocity @@ -66,12 +66,12 @@ science_controller: # Scoops scoop_servos: ["servo_scoop_a", "servo_scoop_b"] - scoop_spinner: "talon_scoop" + scoop_spinner: "dc_scoop" # Sampler sampler_lift_l: "servo_sampler_lift_l" sampler_lift_r: "servo_sampler_lift_r" - auger_spinner: "talon_auger" + auger_spinner: "dc_auger" cap: "servo_auger_lift" interface_name: position command_interfaces: [position, velocity] @@ -98,8 +98,8 @@ motor_status_broadcaster: joints: - stepper_motor_a - stepper_motor_b - - talon_lift - - talon_scoop + - dc_lift + - dc_scoop interfaces: - motor_temperature - torque_current diff --git a/src/subsystems/science/science_bringup/config/joystick.yaml b/src/subsystems/science/science_bringup/config/joystick.yaml index df9a1073..02e9ea17 100644 --- a/src/subsystems/science/science_bringup/config/joystick.yaml +++ b/src/subsystems/science/science_bringup/config/joystick.yaml @@ -1,4 +1,5 @@ /**: ros__parameters: # 0: PS4 1: Thrustmaster - joystick_type: 0 + joystick_type: 2 + joystick_index: -1 diff --git a/src/subsystems/science/science_bringup/launch/athena_science.launch.py b/src/subsystems/science/science_bringup/launch/athena_science.launch.py index 32ea8109..7dddbf9e 100644 --- a/src/subsystems/science/science_bringup/launch/athena_science.launch.py +++ b/src/subsystems/science/science_bringup/launch/athena_science.launch.py @@ -177,11 +177,11 @@ def generate_launch_description(): arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - motor_status_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=["motor_status_broadcaster", "-c", "/controller_manager"], - ) + # motor_status_broadcaster_spawner = Node( + # package="controller_manager", + # executable="spawner", + # arguments=["motor_status_broadcaster", "-c", "/controller_manager"], + # ) # CONTROLLER MANAGERS @@ -265,12 +265,12 @@ def generate_launch_description(): ) # Delay motor_status_broadcaster (inactive) after joint_state_broadcaster - delay_motor_status_broadcaster_after_joint_state_broadcaster = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_broadcaster_spawner, - on_exit=[motor_status_broadcaster_spawner], - ) - ) + # delay_motor_status_broadcaster_after_joint_state_broadcaster = RegisterEventHandler( + # event_handler=OnProcessExit( + # target_action=joint_state_broadcaster_spawner, + # on_exit=[motor_status_broadcaster_spawner], + # ) + # ) # Delay loading and activation of `joint_state_broadcaster` after start of ros2_control_node delay_joint_state_broadcaster_spawner_after_ros2_control_node = RegisterEventHandler( @@ -332,7 +332,7 @@ def generate_launch_description(): robot_state_pub_node, rviz_node, delay_joint_state_broadcaster_spawner_after_ros2_control_node, - delay_motor_status_broadcaster_after_joint_state_broadcaster, + # delay_motor_status_broadcaster_after_joint_state_broadcaster, # umdloop_can_node, controller_switcher_node, joystick_publisher, diff --git a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp index 75872f50..6af6d49c 100644 --- a/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp +++ b/src/subsystems/science/science_controllers/include/science_controllers/science_controller.hpp @@ -140,6 +140,7 @@ class ScienceManual : public controller_interface::ControllerInterface std::vector talon_joints_; std::vector servo_joints_; std::vector rack_pinion_joints_; + std::vector dc_joints_; std::vector joints_; //std::string talon_auger_; diff --git a/src/subsystems/science/science_controllers/src/science_controller.cpp b/src/subsystems/science/science_controllers/src/science_controller.cpp index ebbaea9e..c6997dd7 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.cpp +++ b/src/subsystems/science/science_controllers/src/science_controller.cpp @@ -85,8 +85,8 @@ controller_interface::CallbackReturn ScienceManual::on_configure( params_.pump_b }; - talon_joints_.clear(); - talon_joints_ = { + dc_joints_.clear(); + dc_joints_ = { params_.scoop_spinner, params_.auger_spinner }; @@ -246,7 +246,7 @@ controller_interface::CallbackReturn ScienceManual::on_deactivate( command_interfaces_[i].set_value(std::numeric_limits::quiet_NaN()); } stepper_pump_joints_.clear(); - talon_joints_.clear(); + dc_joints_.clear(); servo_joints_.clear(); rack_pinion_joints_.clear(); state_joints_.clear(); @@ -375,7 +375,7 @@ controller_interface::return_type ScienceManual::update( // Stepper motors (position) command_interfaces_[IDX_PUMP_A_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); command_interfaces_[IDX_PUMP_B_VELOCITY].set_value(stepper_cmd * (M_PI / 180.0)); - // Talons (velocity) + // DC motors (velocity) command_interfaces_[IDX_SAMPLER_LIFT_LEFT_VELOCITY].set_value(sampler_lift_pos_l * (M_PI / 180.0)); command_interfaces_[IDX_SAMPLER_LIFT_RIGHT_VELOCITY].set_value(sampler_lift_pos_r * (M_PI / 180.0)); command_interfaces_[IDX_SCOOP_SPINNER_VELOCITY].set_value(scoop_spinner_cmd); diff --git a/src/subsystems/science/science_controllers/src/science_controller.yaml b/src/subsystems/science/science_controllers/src/science_controller.yaml index 7371edf2..eb1938cf 100644 --- a/src/subsystems/science/science_controllers/src/science_controller.yaml +++ b/src/subsystems/science/science_controllers/src/science_controller.yaml @@ -74,8 +74,8 @@ science_manual: scoop_spinner: type: string - default_value: "talon_scoop" - description: "Scoop Talon motor controller" + default_value: "dc_scoop" + description: "Scoop DC motor" read_only: true validation: not_empty<>: null @@ -94,7 +94,7 @@ science_manual: auger_spinner: type: string - default_value: "talon_auger" + default_value: "dc_auger" description: "(Optional)" read_only: false