From e11416a537cc40f5f96e7d78ca2b8bdccdaa40bb Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Sun, 11 Aug 2024 22:10:59 +0800 Subject: [PATCH 01/36] dm motor hardware interface created --- .../motor_driver/dm_motor_driver.hpp | 62 ++++++ .../motor_network/dm_motor_network.hpp | 75 +++++++ .../meta_hardware/src/dm_motor_interface.cpp | 194 ++++++++++++++++++ .../src/motor_driver/dm_motor_driver.cpp | 61 ++++++ .../src/motor_network/dm_motor_network.cpp | 137 +++++++++++++ 5 files changed, 529 insertions(+) create mode 100644 execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp create mode 100644 execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp create mode 100644 execution/meta_hardware/src/dm_motor_interface.cpp create mode 100644 execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp create mode 100644 execution/meta_hardware/src/motor_network/dm_motor_network.cpp diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp new file mode 100644 index 0000000..d16c44c --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -0,0 +1,62 @@ +#ifndef META_HARDWARE__MOTOR_DRIVER__DM_MOTOR_DRIVER_HPP_ +#define META_HARDWARE__MOTOR_DRIVER__DM_MOTOR_DRIVER_HPP_ +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" + +namespace meta_hardware { +class DmMotor { + public: + DmMotor(const std::string &motor_model, uint32_t dm_motor_id,std::string mode, + double max_vel, double max_pos, double max_effort); + + ~DmMotor() = default; + + enum DmMode { + MIT, + POS, + VEL, + }; + + uint32_t get_dm_motor_id() const; + uint32_t get_tx_can_id() const; + uint32_t get_rx_can_id() const; + DmMode get_mode() const { return mode_; } + + void set_motor_feedback(uint8_t error_code, uint8_t id, + uint16_t position_raw, uint16_t velocity_raw, + uint16_t effort_raw, uint8_t temperature_mos, + uint8_t temperature_rotor); + std::tuple get_motor_feedback() const; + + private: + // Motor information + std::string motor_model_; + uint32_t dm_motor_id_; + DmMode mode_; + uint32_t tx_can_id_; + uint32_t rx_can_id_; + + double max_vel_; + double max_pos_; + double max_effort_; + + // Motor feedback + uint8_t error_code_{0}; + uint8_t id_{0}; + double position_{0.0}; + double velocity_{0.0}; + double toruqe_{0.0}; + uint8_t temperature_mos_{0}; + uint8_t temperature_rotor_{0}; + +}; + +} // namespace meta_hardware + +#endif // META_HARDWARE__MOTOR_DRIVER__DM_MOTOR_DRIVER_HPP_ diff --git a/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp new file mode 100644 index 0000000..a4c19ce --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp @@ -0,0 +1,75 @@ +#ifndef META_HARDWARE__MOTOR_NETWORK__DM_MOTOR_NETWORK_HPP_ +#define META_HARDWARE__MOTOR_NETWORK__DM_MOTOR_NETWORK_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "meta_hardware/can_driver/can_driver.hpp" +#include "meta_hardware/motor_driver/dm_motor_driver.hpp" + +namespace meta_hardware { + +class DmMotorNetwork { + public: + DmMotorNetwork( + std::string can_interface, + uint32_t master_id, + const std::vector> + &motor_params); + ~DmMotorNetwork(); + + /** + * @brief Read the motor feedback + * @param joint_id The joint ID of the motor + * @return A tuple of (position, velocity, effort) + */ + std::tuple read(uint32_t joint_id) const; + + /** + * @brief Write the motor command + * @param joint_id The joint ID of the motor + * @param effort The effort to write + */ + void write(uint32_t joint_id, double effort); + + /** + * @brief Transmit the motor commands + */ + void tx(); + + private: + void rx_loop(); + std::unique_ptr rx_thread_; + std::atomic rx_thread_running_{true}; + + uint32_t master_id_; + + // Five CAN frames for tx + // 0x1FE: GM6020(current) motor 1-4 + // 0x1FF: M3508/M2006 motor 5-8 / GM6020(voltage) motor 1-4 + // 0x200: M3508/M2006 motor 1-4 + // 0x2FE: GM6020(current) motor 5-8 + // 0x2FF: GM6020(voltage) motor 5-8 + std::unordered_map tx_frames_; + + // CAN driver + std::unique_ptr can_driver_; + + // [rx_can_id] -> dm_motor + // This makes it easy to find the motor object in rx_loop + std::map> motor_id2motor_; + + // [joint_id] -> dm_motor + // This makes it easy to find the motor object in read() and write() + std::vector> motors_; +}; + +} // namespace meta_hardware + +#endif // META_HARDWARE__MOTOR_NETWORK__DM_MOTOR_NETWORK_HPP_ diff --git a/execution/meta_hardware/src/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp new file mode 100644 index 0000000..d6ffe45 --- /dev/null +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -0,0 +1,194 @@ +#include +#include +#include +#include + +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "meta_hardware/dji_motor_interface.hpp" +#include "meta_hardware/motor_network/dji_motor_network.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace meta_hardware { +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + +MetaRobotDjiMotorNetwork::~MetaRobotDjiMotorNetwork() { + on_deactivate(rclcpp_lifecycle::State()); +} + +hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_init( + const hardware_interface::HardwareInfo &info) { + if (hardware_interface::SystemInterface::on_init(info) != + CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + joint_interface_data_.resize(info_.joints.size()); + joint_motors_info_.resize(info_.joints.size()); + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + + std::vector> motor_params; + + // Add the motors to the motor networks + for (size_t i = 0; i < info_.joints.size(); ++i) { + const auto &joint_params = info_.joints[i].parameters; + motor_params.push_back(joint_params); + joint_motors_info_[i].joint_name = info_.joints[i].name; + joint_motors_info_[i].mechanical_reduction = + std::stod(joint_params.at("mechanical_reduction")); + joint_motors_info_[i].offset = std::stod(joint_params.at("offset")); + } + + std::string can_interface = + info_.hardware_parameters.at("can_network_name"); + dji_motor_network_ = + std::make_unique(can_interface, motor_params); + + return CallbackReturn::SUCCESS; +} + +std::vector +MetaRobotDjiMotorNetwork::export_state_interfaces() { + std::vector state_interfaces; + + // Helper function to check if the interface exists + auto contains_interface = + [](const std::vector &interfaces, + const std::string &interface_name) { + return std::find_if( + interfaces.begin(), interfaces.end(), + [&interface_name]( + const hardware_interface::InterfaceInfo &interface) { + return interface.name == interface_name; + }) != interfaces.end(); + }; + + for (size_t i = 0; i < info_.joints.size(); ++i) { + const auto &joint_state_interfaces = info_.joints[i].state_interfaces; + if (contains_interface(joint_state_interfaces, "position")) { + state_interfaces.emplace_back( + info_.joints[i].name, HW_IF_POSITION, + &joint_interface_data_[i].state_position); + } + if (contains_interface(joint_state_interfaces, "velocity")) { + state_interfaces.emplace_back( + info_.joints[i].name, HW_IF_VELOCITY, + &joint_interface_data_[i].state_velocity); + } + if (contains_interface(joint_state_interfaces, "effort")) { + state_interfaces.emplace_back( + info_.joints[i].name, HW_IF_EFFORT, + &joint_interface_data_[i].state_effort); + } + } + + return state_interfaces; +} + +std::vector +MetaRobotDjiMotorNetwork::export_command_interfaces() { + std::vector command_interfaces; + + // Helper function to check if the interface exists + auto contains_interface = + [](const std::vector &interfaces, + const std::string &interface_name) { + return std::find_if( + interfaces.begin(), interfaces.end(), + [&interface_name]( + const hardware_interface::InterfaceInfo &interface) { + return interface.name == interface_name; + }) != interfaces.end(); + }; + + for (size_t i = 0; i < info_.joints.size(); ++i) { + const auto &joint_command_interfaces = + info_.joints[i].command_interfaces; + if (contains_interface(joint_command_interfaces, "effort")) { + command_interfaces.emplace_back( + info_.joints[i].name, HW_IF_EFFORT, + &joint_interface_data_[i].command_effort); + } + } + + return command_interfaces; +} + +hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type +MetaRobotDjiMotorNetwork::read(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + + for (size_t i = 0; i < joint_motors_info_.size(); ++i) { + auto [position, velocity, effort] = dji_motor_network_->read(i); + + double reduction = joint_motors_info_[i].mechanical_reduction; + double offset = joint_motors_info_[i].offset; + position = position / reduction + offset; + velocity /= reduction; + if (reduction < 0) { + effort = -effort; + } + + joint_interface_data_[i].state_position = position; + joint_interface_data_[i].state_velocity = velocity; + joint_interface_data_[i].state_effort = effort; + } + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +MetaRobotDjiMotorNetwork::write(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + + for (size_t i = 0; i < joint_motors_info_.size(); ++i) { + double effort = joint_interface_data_[i].command_effort; + + // Check if the command is valid + // If a command interface exists, the command must not be NaN + if (std::isnan(effort)) { + continue; + } + + // Even though DJI motors receive proportional effort commands, + // the mechanical reduction can be negative, which means that the + // motor direction is inverted. In this case, the effort must be negated. + if (joint_motors_info_[i].mechanical_reduction < 0) { + effort = -effort; + } + + // Write the command to the motor network + dji_motor_network_->write(i, effort); + } + + // Some motor network implementations require a separate tx() call + dji_motor_network_->tx(); + + return hardware_interface::return_type::OK; +} + +} // namespace meta_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(meta_hardware::MetaRobotDjiMotorNetwork, + hardware_interface::SystemInterface) diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp new file mode 100644 index 0000000..91d4454 --- /dev/null +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -0,0 +1,61 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "meta_hardware/motor_driver/dm_motor_driver.hpp" + +namespace meta_hardware { +DmMotor::DmMotor(const std::string &motor_model, uint32_t dm_motor_id, std::string mode, + double max_vel, double max_pos, double max_effort) + : motor_model_(motor_model), dm_motor_id_(dm_motor_id), + max_vel_(max_vel), max_pos_(max_pos), max_effort_(max_effort) { + + uint32_t id_offeset = 0; + if (mode == "MIT") { + mode_ = DmMode::MIT; + } else if (mode == "POS") { + mode_ = DmMode::POS; + id_offeset = 0x100; + } else if (mode == "VEL") { + mode_ = DmMode::VEL; + id_offeset = 0x200; + } else { + throw std::runtime_error("Unknown motor mode: " + mode); + } + + if (motor_model_ == "4310") { + tx_can_id_ = dm_motor_id_ + id_offeset; + } else { + throw std::runtime_error("Unknown motor model: " + motor_model_); + } +} + +uint32_t DmMotor::get_dm_motor_id() const { return dm_motor_id_; } +canid_t DmMotor::get_tx_can_id() const { return tx_can_id_; } + +void DmMotor::set_motor_feedback(uint8_t error_code, uint8_t id, + uint16_t position_raw, uint16_t velocity_raw, + uint16_t effort_raw, uint8_t temperature_mos, + uint8_t temperature_rotor) { + + double position = static_cast(position_raw - (1 << 15)) / (1 << 16) * max_pos_; + position_ += angles::shortest_angular_distance(position_, position); + velocity_ = static_cast(velocity_raw - (1 << 11)) / (1 << 12) * max_vel_; + toruqe_ = static_cast(effort_raw - (1 << 11)) / (1 << 12) * max_effort_; + error_code_ = error_code; + id_ = id; + temperature_mos_ = temperature_mos; + temperature_rotor_ = temperature_rotor; +} + +std::tuple DmMotor::get_motor_feedback() const { + return {position_, velocity_, toruqe_}; +} + +} // namespace meta_hardware diff --git a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp new file mode 100644 index 0000000..56c74fa --- /dev/null +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -0,0 +1,137 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "meta_hardware/motor_driver/dm_motor_driver.hpp" +#include "meta_hardware/motor_network/dm_motor_network.hpp" + +namespace meta_hardware { + +DmMotorNetwork::DmMotorNetwork( + std::string can_interface, + uint32_t master_id, + const std::vector> + &motor_params) : master_id_(master_id) { + std::vector can_filters; + + for (const auto &motor_param : motor_params) { + std::string motor_model = motor_param.at("motor_model"); + uint32_t dm_motor_id = std::stoi(motor_param.at("motor_id")); + std::string motor_mode = motor_param.at("motor_mode"); + double max_vel = std::stod(motor_param.at("max_vel")); + double max_pos = std::stod(motor_param.at("max_pos")); + double max_torque = std::stod(motor_param.at("max_torque")); + + + auto dm_motor = + std::make_shared(motor_model, dm_motor_id, motor_mode, max_vel, max_pos, max_torque); + motors_.emplace_back(dm_motor); + + motor_id2motor_[dm_motor_id] = dm_motor; + + } + + can_filters.push_back( + {.can_id = master_id, .can_mask = CAN_SFF_MASK}); + + // Initialize CAN driver + can_driver_ = + std::make_unique(can_interface, false, can_filters); + + // Initialize RX thread + rx_thread_ = + std::make_unique(&DmMotorNetwork::rx_loop, this); +} + +DmMotorNetwork::~DmMotorNetwork() { + // Send zero effort to all motors + for (canid_t tx_can_id : {0x1FE, 0x1FF, 0x200, 0x2FE, 0x2FF}) { + can_frame tx_frame{.can_id = tx_can_id, .len = 8, .data = {0}}; + can_driver_->write(tx_frame); + } + + // Stop the RX thread + rx_thread_running_ = false; +} + +std::tuple +DmMotorNetwork::read(uint32_t joint_id) const { + return motors_[joint_id]->get_motor_feedback(); +} + +void DmMotorNetwork::write(uint32_t joint_id, double effort) { + const auto &motor = motors_[joint_id]; + uint32_t dm_motor_id = motor->get_dm_motor_id(); + uint32_t tx_can_id = motor->get_tx_can_id(); + + // uint32_t maximum_raw_effort = motor->get_maximum_raw_effort(); + // effort = std::clamp(effort, -1.0, 1.0); + // auto effort_raw = static_cast(effort * maximum_raw_effort); + + // can_frame &tx_frame = tx_frames_[tx_can_id]; + // tx_frame.data[2 * ((dm_motor_id - 1) % 4)] = effort_raw >> 8; + // tx_frame.data[2 * ((dm_motor_id - 1) % 4) + 1] = effort_raw & 0xFF; +} + +void DmMotorNetwork::rx_loop() { + while (rx_thread_running_) { + try { + can_frame can_msg = can_driver_->read(); + + const auto &motor = motor_id2motor_.at(can_msg.can_id); + + auto error_code = static_cast + (static_cast(can_msg.data[0]) >> 4); + + auto id = static_cast + ((static_cast(can_msg.data[0]) & 0x0F)); + + auto position_raw = static_cast + (((static_cast(can_msg.data[1]) & 0xFF) << 8) | + (static_cast(can_msg.data[2]) & 0xFF) + ); + + auto velocity_raw = static_cast + (((static_cast(can_msg.data[3]) & 0xFF) << 8) | + ((static_cast(can_msg.data[4]) & 0xC0) >> 12) + ); + + auto torque_raw = static_cast + (((static_cast(can_msg.data[4]) & 0x3F) << 4) | + (static_cast(can_msg.data[5]) & 0xFF) + ); + + auto temperature_mos = static_cast + (static_cast(can_msg.data[6])); + + + auto temperature_rotor = static_cast + (static_cast(can_msg.data[7])); + + motor->set_motor_feedback(error_code, id, position_raw, velocity_raw, + torque_raw, temperature_mos, temperature_rotor); + } catch (std::runtime_error &e) { + std::cerr << "Error reading CAN message: " << e.what() << std::endl; + } + } +} + +void DmMotorNetwork::tx() { + try { + for (auto &frame_id : {0x1fe, 0x1ff, 0x200, 0x2fe, 0x2ff}) { + if (tx_frames_.contains(frame_id)) { + can_driver_->write(tx_frames_[frame_id]); + } + } + } catch (std::runtime_error &e) { + std::cerr << "Error writing CAN message: " << e.what() << std::endl; + } +} + +} // namespace meta_hardware From 86382d0a95d068332977bb522a8db28668d24046 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Tue, 13 Aug 2024 07:00:49 +0800 Subject: [PATCH 02/36] Add error message to can_driver --- .../meta_hardware/can_driver/can_driver.hpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/can_driver/can_driver.hpp b/execution/meta_hardware/include/meta_hardware/can_driver/can_driver.hpp index fc6d967..583560a 100644 --- a/execution/meta_hardware/include/meta_hardware/can_driver/can_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/can_driver/can_driver.hpp @@ -24,7 +24,7 @@ class CanDriver { can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (can_socket_ < 0) { - throw CanException("Failed to create socket"); + throw CanException("Failed to create socket: " + std::string(strerror(errno))); } // Get the interface index @@ -32,7 +32,7 @@ class CanDriver { std::strncpy(ifr.ifr_name, can_interface.c_str(), IFNAMSIZ); if (ioctl(can_socket_, SIOCGIFINDEX, &ifr) < 0) { - throw CanException("Failed to get interface index"); + throw CanException("Failed to get interface index: " + std::string(strerror(errno))); } // Bind the socket to the interface @@ -48,11 +48,11 @@ class CanDriver { // Set the socket to non-blocking int flags = fcntl(can_socket_, F_GETFL, 0); if (flags < 0) { - throw CanException("Failed to get socket flags"); + throw CanException("Failed to get socket flags: " + std::string(strerror(errno))); } if (fcntl(can_socket_, F_SETFL, flags | O_NONBLOCK) < 0) { - throw CanException("Failed to set socket flags"); + throw CanException("Failed to set socket flags: " + std::string(strerror(errno))); } // Set the filters @@ -60,12 +60,12 @@ class CanDriver { if (setsockopt(can_socket_, SOL_CAN_RAW, CAN_RAW_JOIN_FILTERS, &can_filters[0], can_filters.size() * sizeof(can_filter)) < 0) { - throw CanException("Failed to set filters"); + throw CanException("Failed to set filters: " + std::string(strerror(errno))); } } else if (!can_filters.empty()) { if (setsockopt(can_socket_, SOL_CAN_RAW, CAN_RAW_FILTER, &can_filters[0], can_filters.size() * sizeof(can_filter)) < 0) { - throw CanException("Failed to set filters"); + throw CanException("Failed to set filters: " + std::string(strerror(errno))); } } @@ -83,7 +83,7 @@ class CanDriver { if (poll_ret == 0) { throw CanIOTimedOutException("Timed out waiting for a frame"); } else if (poll_ret < 0) { - throw CanIOException("Failed to poll for frame"); + throw CanIOException("Failed to poll for frame: " + std::string(strerror(errno))); } if (poll_fd_.revents & POLLIN) { @@ -96,7 +96,7 @@ class CanDriver { if (errno == EAGAIN || errno == EWOULDBLOCK) { continue; } else { - throw CanIOException("Failed to read frame"); + throw CanIOException("Failed to read frame: " + std::string(strerror(errno))); } } @@ -111,7 +111,7 @@ class CanDriver { // If any error occurs, the caller should handle it via exception. ssize_t nbytes = ::write(can_socket_, &frame, sizeof(frame)); if (nbytes < 0) { - throw CanIOException("Failed to write frame"); + throw CanIOException("Failed to write frame: " + std::string(strerror(errno))); } return nbytes; } From 507d12698a54528d6dc7739e68be82e1d11b2b9f Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Tue, 13 Aug 2024 07:02:45 +0800 Subject: [PATCH 03/36] Remove redundant catch and add necessary catch in destructor. --- .../src/motor_network/dji_motor_network.cpp | 8 ++++++-- .../src/motor_network/mi_motor_network.cpp | 17 +++-------------- 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/execution/meta_hardware/src/motor_network/dji_motor_network.cpp b/execution/meta_hardware/src/motor_network/dji_motor_network.cpp index 06ef1ca..ebde67f 100644 --- a/execution/meta_hardware/src/motor_network/dji_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dji_motor_network.cpp @@ -36,7 +36,7 @@ DjiMotorNetwork::DjiMotorNetwork( // Update the can filter can_filters.push_back( - {.can_id = dji_motor->get_rx_can_id(), .can_mask = CAN_SFF_MASK}); + {.can_id = dji_motor->get_rx_can_id(), .can_mask = CAN_EFF_MASK}); } // Initialize CAN driver @@ -52,7 +52,11 @@ DjiMotorNetwork::~DjiMotorNetwork() { for (canid_t tx_can_id : {0x1FE, 0x1FF, 0x200, 0x2FE, 0x2FF}) { if (tx_frames_.contains(tx_can_id)) { can_frame tx_frame{.can_id = tx_can_id, .len = 8, .data = {0}}; - can_driver_->write(tx_frame); + try { + can_driver_->write(tx_frame); + } catch (CanIOException &e) { + std::cerr << "Error writing CAN message: " << e.what() << std::endl; + } } } } diff --git a/execution/meta_hardware/src/motor_network/mi_motor_network.cpp b/execution/meta_hardware/src/motor_network/mi_motor_network.cpp index 0b4d7f4..7b73971 100644 --- a/execution/meta_hardware/src/motor_network/mi_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/mi_motor_network.cpp @@ -34,23 +34,12 @@ MiMotorNetwork::MiMotorNetwork(const string &can_network_name, uint32_t host_id, } // Initialize CAN driver - try { - can_driver_ = std::make_unique(can_network_name); - } catch (CanException &e) { - std::cerr << "Failed to initialize CAN driver: " << e.what() << std::endl; - throw std::runtime_error("Failed to initialize MI motor network"); - } + can_driver_ = std::make_unique(can_network_name); // Enable all motors for (const auto &motor : mi_motors_) { - try { - can_driver_->write(motor->get_motor_runmode_frame()); - can_driver_->write(motor->motor_enable_frame()); - } catch (CanIOException &e) { - std::cerr << "Error writing MI motor enable CAN message: " << e.what() - << std::endl; - throw std::runtime_error("Failed to enable MI motors"); - } + can_driver_->write(motor->get_motor_runmode_frame()); + can_driver_->write(motor->motor_enable_frame()); } // Start RX thread From 2dc0215d2b2d40691b73d4238f6e19d1ee7bb4e0 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Tue, 13 Aug 2024 07:11:16 +0800 Subject: [PATCH 04/36] Fix linter warnings in code --- .../motor_network/dji_motor_network.hpp | 4 +- .../meta_hardware/src/dji_motor_interface.cpp | 46 ++++++++----------- .../src/motor_network/dji_motor_network.cpp | 4 +- 3 files changed, 23 insertions(+), 31 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/motor_network/dji_motor_network.hpp b/execution/meta_hardware/include/meta_hardware/motor_network/dji_motor_network.hpp index ea99b18..5e91de2 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_network/dji_motor_network.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_network/dji_motor_network.hpp @@ -26,14 +26,14 @@ class DjiMotorNetwork { * @param joint_id The joint ID of the motor * @return A tuple of (position, velocity, effort) */ - std::tuple read(uint32_t joint_id) const; + std::tuple read(size_t joint_id) const; /** * @brief Write the motor command * @param joint_id The joint ID of the motor * @param effort The effort to write */ - void write(uint32_t joint_id, double effort); + void write(size_t joint_id, double effort); /** * @brief Transmit the motor commands diff --git a/execution/meta_hardware/src/dji_motor_interface.cpp b/execution/meta_hardware/src/dji_motor_interface.cpp index c7682b4..ae7f8d4 100644 --- a/execution/meta_hardware/src/dji_motor_interface.cpp +++ b/execution/meta_hardware/src/dji_motor_interface.cpp @@ -16,10 +16,9 @@ using hardware_interface::HW_IF_VELOCITY; MetaRobotDjiMotorNetwork::~MetaRobotDjiMotorNetwork() = default; -hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_init( - const hardware_interface::HardwareInfo &info) { - if (hardware_interface::SystemInterface::on_init(info) != - CallbackReturn::SUCCESS) { +hardware_interface::CallbackReturn +MetaRobotDjiMotorNetwork::on_init(const hardware_interface::HardwareInfo &info) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -44,10 +43,8 @@ hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_configure( joint_motors_info_[i].offset = std::stod(joint_params.at("offset")); } - std::string can_interface = - info_.hardware_parameters.at("can_network_name"); - dji_motor_network_ = - std::make_unique(can_interface, motor_params); + std::string can_interface = info_.hardware_parameters.at("can_network_name"); + dji_motor_network_ = std::make_unique(can_interface, motor_params); return CallbackReturn::SUCCESS; } @@ -60,8 +57,8 @@ MetaRobotDjiMotorNetwork::export_state_interfaces() { auto contains_interface = [](const std::vector &interfaces, const std::string &interface_name) { - return std::find_if( - interfaces.begin(), interfaces.end(), + return std::ranges::find_if( + interfaces, [&interface_name]( const hardware_interface::InterfaceInfo &interface) { return interface.name == interface_name; @@ -71,19 +68,16 @@ MetaRobotDjiMotorNetwork::export_state_interfaces() { for (size_t i = 0; i < info_.joints.size(); ++i) { const auto &joint_state_interfaces = info_.joints[i].state_interfaces; if (contains_interface(joint_state_interfaces, "position")) { - state_interfaces.emplace_back( - info_.joints[i].name, HW_IF_POSITION, - &joint_interface_data_[i].state_position); + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_POSITION, + &joint_interface_data_[i].state_position); } if (contains_interface(joint_state_interfaces, "velocity")) { - state_interfaces.emplace_back( - info_.joints[i].name, HW_IF_VELOCITY, - &joint_interface_data_[i].state_velocity); + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_VELOCITY, + &joint_interface_data_[i].state_velocity); } if (contains_interface(joint_state_interfaces, "effort")) { - state_interfaces.emplace_back( - info_.joints[i].name, HW_IF_EFFORT, - &joint_interface_data_[i].state_effort); + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, + &joint_interface_data_[i].state_effort); } } @@ -98,8 +92,8 @@ MetaRobotDjiMotorNetwork::export_command_interfaces() { auto contains_interface = [](const std::vector &interfaces, const std::string &interface_name) { - return std::find_if( - interfaces.begin(), interfaces.end(), + return std::ranges::find_if( + interfaces, [&interface_name]( const hardware_interface::InterfaceInfo &interface) { return interface.name == interface_name; @@ -107,12 +101,10 @@ MetaRobotDjiMotorNetwork::export_command_interfaces() { }; for (size_t i = 0; i < info_.joints.size(); ++i) { - const auto &joint_command_interfaces = - info_.joints[i].command_interfaces; + const auto &joint_command_interfaces = info_.joints[i].command_interfaces; if (contains_interface(joint_command_interfaces, "effort")) { - command_interfaces.emplace_back( - info_.joints[i].name, HW_IF_EFFORT, - &joint_interface_data_[i].command_effort); + command_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, + &joint_interface_data_[i].command_effort); } } @@ -168,7 +160,7 @@ MetaRobotDjiMotorNetwork::write(const rclcpp::Time & /*time*/, } // Even though DJI motors receive proportional effort commands, - // the mechanical reduction can be negative, which means that the + // the mechanical reduction can be negative, which means that the // motor direction is inverted. In this case, the effort must be negated. if (joint_motors_info_[i].mechanical_reduction < 0) { effort = -effort; diff --git a/execution/meta_hardware/src/motor_network/dji_motor_network.cpp b/execution/meta_hardware/src/motor_network/dji_motor_network.cpp index ebde67f..be65beb 100644 --- a/execution/meta_hardware/src/motor_network/dji_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dji_motor_network.cpp @@ -61,11 +61,11 @@ DjiMotorNetwork::~DjiMotorNetwork() { } } -std::tuple DjiMotorNetwork::read(uint32_t joint_id) const { +std::tuple DjiMotorNetwork::read(size_t joint_id) const { return motors_[joint_id]->get_motor_feedback(); } -void DjiMotorNetwork::write(uint32_t joint_id, double effort) { +void DjiMotorNetwork::write(size_t joint_id, double effort) { const auto &motor = motors_[joint_id]; uint32_t dji_motor_id = motor->get_dji_motor_id(); uint32_t tx_can_id = motor->get_tx_can_id(); From 88f7e7c9bff9d633dd282456b38ae46ad86f93ab Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Tue, 13 Aug 2024 16:02:21 +0800 Subject: [PATCH 05/36] Add parameter setting to MI motor position and velocity mode. --- .../motor_driver/mi_motor_driver.hpp | 13 ++++- .../src/motor_driver/mi_motor_driver.cpp | 51 +++++++++++++++---- .../src/motor_network/mi_motor_network.cpp | 15 +++++- 3 files changed, 65 insertions(+), 14 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/mi_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/mi_motor_driver.hpp index bb0c47b..c0374b5 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_driver/mi_motor_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/mi_motor_driver.hpp @@ -46,9 +46,13 @@ class MiMotor { ~MiMotor() = default; - can_frame get_motor_runmode_frame() const; + can_frame motor_runmode_frame() const; can_frame motor_enable_frame() const; can_frame motor_disable_frame() const; + can_frame motor_limit_frame() const; + can_frame motor_loc_kp_frame() const; + can_frame motor_spd_kp_frame() const; + can_frame motor_spd_ki_frame() const; can_frame motor_dyn_frame(double position, double velocity, double effort) const; can_frame motor_pos_frame(double position) const; can_frame motor_vel_frame(double velocity) const; @@ -60,6 +64,8 @@ class MiMotor { std::tuple get_motor_feedback() const; + RunMode get_run_mode() const { return run_mode_; } + private: // Motor information std::string motor_model_; @@ -75,6 +81,11 @@ class MiMotor { uint16_t Kp_raw_{0}; uint16_t Kd_raw_{0}; RunMode run_mode_; + float limit_cur_{0.0}; + float limit_spd_{0.0}; + float loc_kp_{0.0}; + float spd_kp_{0.0}; + float spd_ki_{0.0}; }; } // namespace meta_hardware diff --git a/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp index 85878e7..c2bd02f 100644 --- a/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp @@ -32,16 +32,27 @@ MiMotor::MiMotor(const std::unordered_map &motor_param motor_model_ = motor_param.at("motor_model"); mi_motor_id_ = static_cast(std::stoi(motor_param.at("motor_id"))); + if (motor_model_ != "CyberGear") { + throw std::runtime_error("Unknown motor model: " + motor_model_); + } + std::string control_mode = motor_param.at("control_mode"); - double Kp = std::numeric_limits::quiet_NaN(); - double Kd = std::numeric_limits::quiet_NaN(); if (control_mode == "dynamic") { - Kp = std::stod(motor_param.at("Kp")); - Kd = std::stod(motor_param.at("Kd")); + double Kp = std::stod(motor_param.at("Kp")); + double Kd = std::stod(motor_param.at("Kd")); + Kp_raw_ = static_cast((Kp / MAX_KP) * MAX_RAW_KP); + Kd_raw_ = static_cast((Kd / MAX_KD) * MAX_RAW_KD); run_mode_ = RunMode::DYNAMIC; } else if (control_mode == "position") { + limit_spd_ = std::stof(motor_param.at("limit_spd")); + loc_kp_ = std::stof(motor_param.at("loc_kp")); + spd_kp_ = std::stof(motor_param.at("spd_kp")); + spd_ki_ = std::stof(motor_param.at("spd_ki")); run_mode_ = RunMode::POSITION; } else if (control_mode == "velocity") { + limit_cur_ = std::stof(motor_param.at("limit_cur")); + spd_kp_ = std::stof(motor_param.at("spd_kp")); + spd_ki_ = std::stof(motor_param.at("spd_ki")); run_mode_ = RunMode::VELOCITY; } else if (control_mode == "current") { run_mode_ = RunMode::CURRENT; @@ -49,15 +60,9 @@ MiMotor::MiMotor(const std::unordered_map &motor_param throw std::runtime_error("Unknown control mode: " + control_mode); } - if (motor_model_ == "CyberGear") { - Kp_raw_ = static_cast((Kp / MAX_KP) * MAX_RAW_KP); - Kd_raw_ = static_cast((Kd / MAX_KD) * MAX_RAW_KD); - } else { - throw std::runtime_error("Unknown motor model: " + motor_model_); - } } -can_frame MiMotor::get_motor_runmode_frame() const { +can_frame MiMotor::motor_runmode_frame() const { return motor_wr_param_frame(0x7005, static_cast(run_mode_)); } @@ -75,6 +80,30 @@ can_frame MiMotor::motor_disable_frame() const { .data = {0, 0, 0, 0, 0, 0, 0, 0}}); } +can_frame MiMotor::motor_limit_frame() const { + using enum RunMode; + switch (run_mode_) { + case POSITION: + return motor_wr_param_frame(0x7017, limit_spd_); + case VELOCITY: + return motor_wr_param_frame(0x7018, limit_cur_); + default: + throw std::runtime_error("Cannot set limits for this run mode"); + } +} + +can_frame MiMotor::motor_loc_kp_frame() const { + return motor_wr_param_frame(0x701E, loc_kp_); +} + +can_frame MiMotor::motor_spd_kp_frame() const { + return motor_wr_param_frame(0x701F, spd_kp_); +} + +can_frame MiMotor::motor_spd_ki_frame() const { + return motor_wr_param_frame(0x7020, spd_ki_); +} + can_frame MiMotor::motor_dyn_frame(double position, double velocity, double effort) const { position = std::clamp(position, -MAX_ABS_POSITION, MAX_ABS_POSITION); diff --git a/execution/meta_hardware/src/motor_network/mi_motor_network.cpp b/execution/meta_hardware/src/motor_network/mi_motor_network.cpp index 7b73971..ed29f3c 100644 --- a/execution/meta_hardware/src/motor_network/mi_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/mi_motor_network.cpp @@ -36,10 +36,21 @@ MiMotorNetwork::MiMotorNetwork(const string &can_network_name, uint32_t host_id, // Initialize CAN driver can_driver_ = std::make_unique(can_network_name); - // Enable all motors + // Enable all motors and set parameters for (const auto &motor : mi_motors_) { - can_driver_->write(motor->get_motor_runmode_frame()); + can_driver_->write(motor->motor_runmode_frame()); can_driver_->write(motor->motor_enable_frame()); + + if (motor->get_run_mode() == MiMotor::RunMode::POSITION) { + can_driver_->write(motor->motor_limit_frame()); + can_driver_->write(motor->motor_loc_kp_frame()); + can_driver_->write(motor->motor_spd_kp_frame()); + can_driver_->write(motor->motor_spd_ki_frame()); + } else if (motor->get_run_mode() == MiMotor::RunMode::VELOCITY) { + can_driver_->write(motor->motor_limit_frame()); + can_driver_->write(motor->motor_spd_kp_frame()); + can_driver_->write(motor->motor_spd_ki_frame()); + } } // Start RX thread From 7d84c2fa742f359ec9fd5422a54a7806b98fa882 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Tue, 13 Aug 2024 16:25:55 +0800 Subject: [PATCH 06/36] Add CAN filtering to MI motors --- .../motor_network/mi_motor_network.hpp | 6 +++--- .../src/motor_driver/mi_motor_driver.cpp | 13 +++---------- .../src/motor_network/mi_motor_network.cpp | 11 ++++++++--- 3 files changed, 14 insertions(+), 16 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/motor_network/mi_motor_network.hpp b/execution/meta_hardware/include/meta_hardware/motor_network/mi_motor_network.hpp index a1bd863..2eea345 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_network/mi_motor_network.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_network/mi_motor_network.hpp @@ -16,7 +16,7 @@ namespace meta_hardware { class MiMotorNetwork { public: MiMotorNetwork( - const std::string &can_network_name, uint32_t host_id, + const std::string &can_network_name, uint8_t host_id, const std::vector> &joint_params); ~MiMotorNetwork(); @@ -52,14 +52,14 @@ class MiMotorNetwork { // [motor_id] -> mi_motor // This makes it easy to find the motor object in rx_loop - std::map> motor_id2motor_; + std::map> motor_id2motor_; // [joint_id] -> mi_motor // This makes it easy to find the motor object in read() and write() std::vector> mi_motors_; // Host ID - uint32_t host_id_; + uint8_t host_id_; }; } // namespace meta_hardware diff --git a/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp index c2bd02f..48587ee 100644 --- a/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp @@ -59,7 +59,6 @@ MiMotor::MiMotor(const std::unordered_map &motor_param } else { throw std::runtime_error("Unknown control mode: " + control_mode); } - } can_frame MiMotor::motor_runmode_frame() const { @@ -167,15 +166,9 @@ can_frame MiMotor::motor_wr_param_frame(uint16_t index, uint8_t value) const { } void MiMotor::set_motor_feedback(const mi_can_frame &can_msg) { - auto position_raw = - static_cast((static_cast(can_msg.data[0]) << 8) | - static_cast(can_msg.data[1])); - auto velocity_raw = - static_cast((static_cast(can_msg.data[2]) << 8) | - static_cast(can_msg.data[3])); - auto torque_raw = - static_cast((static_cast(can_msg.data[4]) << 8) | - static_cast(can_msg.data[5])); + auto position_raw = static_cast(can_msg.data[0] << 8 | can_msg.data[1]); + auto velocity_raw = static_cast(can_msg.data[2] << 8 | can_msg.data[3]); + auto torque_raw = static_cast(can_msg.data[4] << 8 | can_msg.data[5]); position_ = (position_raw / double(MAX_RAW_POSITION)) * (2 * MAX_ABS_POSITION) - MAX_ABS_POSITION; diff --git a/execution/meta_hardware/src/motor_network/mi_motor_network.cpp b/execution/meta_hardware/src/motor_network/mi_motor_network.cpp index ed29f3c..7a13c98 100644 --- a/execution/meta_hardware/src/motor_network/mi_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/mi_motor_network.cpp @@ -19,22 +19,27 @@ using std::string; using std::unordered_map; using std::vector; -MiMotorNetwork::MiMotorNetwork(const string &can_network_name, uint32_t host_id, +MiMotorNetwork::MiMotorNetwork(const string &can_network_name, uint8_t host_id, const vector> &joint_params) : host_id_(host_id) { + vector can_filters; + // Initialize MI motor drivers for (const auto &joint_param : joint_params) { string motor_model = joint_param.at("motor_model"); - uint32_t mi_motor_id = std::stoi(joint_param.at("motor_id")); + uint8_t mi_motor_id = std::stoi(joint_param.at("motor_id")); auto mi_motor = std::make_shared(joint_param, host_id); motor_id2motor_[mi_motor_id] = mi_motor; mi_motors_.emplace_back(mi_motor); + + can_filters.push_back({.can_id = static_cast(mi_motor_id << 8 | host_id), + .can_mask = 0xFFFF}); } // Initialize CAN driver - can_driver_ = std::make_unique(can_network_name); + can_driver_ = std::make_unique(can_network_name, false, can_filters); // Enable all motors and set parameters for (const auto &motor : mi_motors_) { From 59a325feb56ffc8ce006c9653e48076ddf475fc8 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Wed, 14 Aug 2024 16:16:08 +0800 Subject: [PATCH 07/36] First implementation of a transform-aware gimbal controller --- .../gimbal_controller/gimbal_controller.hpp | 10 ++- .../meta_gimbal_controller/package.xml | 1 + .../src/gimbal_controller.cpp | 62 +++++++++++-------- 3 files changed, 46 insertions(+), 27 deletions(-) diff --git a/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp index df565fb..643aee3 100644 --- a/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp +++ b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp @@ -16,6 +16,10 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" +#include "tf2/tf2/LinearMath/Matrix3x3.h" +#include "tf2/tf2/LinearMath/Quaternion.h" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" #include "behavior_interface/msg/aim.hpp" #include "control_msgs/msg/multi_dof_state_stamped.hpp" @@ -30,7 +34,7 @@ static constexpr size_t CMD_MY_ITFS = 0; class GimbalController : public controller_interface::ChainableControllerInterface { public: - GimbalController(); + GimbalController() = default; controller_interface::CallbackReturn on_init() override; @@ -73,6 +77,10 @@ class GimbalController : public controller_interface::ChainableControllerInterfa realtime_tools::RealtimeBuffer> input_feedback_; + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + tf2::Quaternion q_imu2gimbal_; + std::shared_ptr yaw_pos2vel_pid_; std::shared_ptr pitch_pos2vel_pid_; std::shared_ptr yaw_vel2eff_pid_; diff --git a/decomposition/meta_gimbal_controller/package.xml b/decomposition/meta_gimbal_controller/package.xml index 929b8c9..d92220b 100644 --- a/decomposition/meta_gimbal_controller/package.xml +++ b/decomposition/meta_gimbal_controller/package.xml @@ -26,6 +26,7 @@ std_srvs tf2 tf2_ros + tf2_geometry_msgs ament_cmake diff --git a/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp b/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp index 9daa761..7fc58ca 100644 --- a/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp +++ b/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp @@ -4,27 +4,16 @@ #include #include #include +#include +#include +#include +#include #include #include "angles/angles.h" #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" -#include "tf2/tf2/LinearMath/Matrix3x3.h" -#include "tf2/tf2/LinearMath/Quaternion.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" - -static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; constexpr double NaN = std::numeric_limits::quiet_NaN(); @@ -32,7 +21,6 @@ using ControllerReferenceMsg = gimbal_controller::GimbalController::ControllerReferenceMsg; using ControllerFeedbackMsg = gimbal_controller::GimbalController::ControllerFeedbackMsg; -// called from RT control loop void reset_controller_reference_msg( const std::shared_ptr &msg, const std::shared_ptr & /*node*/) { @@ -58,11 +46,8 @@ void reset_controller_feedback_msg( using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; -// using hardware_interface::HW_IF_VELOCITY; namespace gimbal_controller { -GimbalController::GimbalController() - : controller_interface::ChainableControllerInterface() {} controller_interface::CallbackReturn GimbalController::on_init() { @@ -133,6 +118,9 @@ GimbalController::on_configure(const rclcpp_lifecycle::State & /*previous_state* reset_controller_feedback_msg(feedback_msg, get_node()); input_feedback_.writeFromNonRT(feedback_msg); + tf_buffer_ = std::make_unique(get_node()->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + try { // State publisher s_publisher_ = get_node()->create_publisher( @@ -224,12 +212,27 @@ GimbalController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/ reference_interfaces_.assign(reference_interfaces_.size(), NaN); + // Get transform from IMU to gimbal + geometry_msgs::msg::TransformStamped trans; + + try { + trans = tf_buffer_->lookupTransform("gimbal_imu", "pitch_gimbal", + tf2::TimePointZero, tf2::Duration(1)); + } catch (tf2::LookupException &e) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to lookup transform: %s", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + q_imu2gimbal_ = + tf2::Quaternion(trans.transform.rotation.x, trans.transform.rotation.y, + trans.transform.rotation.z, trans.transform.rotation.w); + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn GimbalController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { - RCLCPP_INFO(get_node()->get_logger(), "deactivate successful"); return controller_interface::CallbackReturn::SUCCESS; } @@ -254,14 +257,21 @@ GimbalController::update_and_write_commands(const rclcpp::Time &time, // Collect IMU feedback auto current_feedback = *(input_feedback_.readFromRT()); + // Transform IMU feedback to gimbal frame + tf2::Quaternion q_imu; + fromMsg(current_feedback->orientation, q_imu); + auto q_gimbal = q_imu * q_imu2gimbal_; + + // Transform angular velocity feedback to gimbal frame + tf2::Vector3 v_imu; + fromMsg(current_feedback->angular_velocity, v_imu); + auto v_gimbal = tf2::quatRotate(q_imu2gimbal_.inverse(), v_imu); + double yaw_pos_fb, pitch_pos_fb, roll_pos_fb; - auto q = - tf2::Quaternion(current_feedback->orientation.x, current_feedback->orientation.y, - current_feedback->orientation.z, current_feedback->orientation.w); - tf2::Matrix3x3(q).getRPY(roll_pos_fb, pitch_pos_fb, yaw_pos_fb); + tf2::Matrix3x3(q_gimbal).getRPY(roll_pos_fb, pitch_pos_fb, yaw_pos_fb); - double yaw_vel_fb = -current_feedback->angular_velocity.z; - double pitch_vel_fb = -current_feedback->angular_velocity.y; + double yaw_vel_fb = v_gimbal.z(); + double pitch_vel_fb = v_gimbal.y(); double yaw_pos_ref = NaN, pitch_pos_ref = NaN; double yaw_pos_err = NaN, pitch_pos_err = NaN; From 311ec9ab64b85e69068e3b56f46184534d4e2a31 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Wed, 14 Aug 2024 16:35:47 +0800 Subject: [PATCH 08/36] working on dm motor --- .../motor_driver/dm_motor_driver.hpp | 25 +++- .../motor_network/dm_motor_network.hpp | 5 - .../src/motor_driver/dm_motor_driver.cpp | 118 ++++++++++++++++-- .../src/motor_network/dm_motor_network.cpp | 28 ----- 4 files changed, 129 insertions(+), 47 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp index d16c44c..512d8e5 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -5,15 +5,20 @@ #include #include #include +#include #include +#include "CanMessage.hpp" +#include + #include "angles/angles.h" namespace meta_hardware { class DmMotor { public: DmMotor(const std::string &motor_model, uint32_t dm_motor_id,std::string mode, - double max_vel, double max_pos, double max_effort); + double max_vel, double max_pos, double max_effort, uint32_t Kp, uint32_t Kd, + uint32_t Tff); ~DmMotor() = default; @@ -28,10 +33,16 @@ class DmMotor { uint32_t get_rx_can_id() const; DmMode get_mode() const { return mode_; } - void set_motor_feedback(uint8_t error_code, uint8_t id, - uint16_t position_raw, uint16_t velocity_raw, - uint16_t effort_raw, uint8_t temperature_mos, - uint8_t temperature_rotor); + sockcanpp::CanMessage get_motor_enable_frame(uint8_t master_id) const; + sockcanpp::CanMessage get_motor_disable_frame(uint8_t master_id) const; + sockcanpp::CanMessage get_motor_save_initial_frame(uint8_t master_id) const; + sockcanpp::CanMessage get_motor_clear_error_frame(uint8_t master_id) const; + sockcanpp::CanMessage get_motor_command_frame(double position, + double velocity, + double effort) const; + + + void set_motor_feedback(const sockcanpp::CanMessage &can_msg); std::tuple get_motor_feedback() const; private: @@ -46,6 +57,10 @@ class DmMotor { double max_pos_; double max_effort_; + uint32_t kp_; + uint32_t kd_; + uint32_t tff_; + // Motor feedback uint8_t error_code_{0}; uint8_t id_{0}; diff --git a/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp index a4c19ce..028c6f2 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp @@ -51,11 +51,6 @@ class DmMotorNetwork { uint32_t master_id_; // Five CAN frames for tx - // 0x1FE: GM6020(current) motor 1-4 - // 0x1FF: M3508/M2006 motor 5-8 / GM6020(voltage) motor 1-4 - // 0x200: M3508/M2006 motor 1-4 - // 0x2FE: GM6020(current) motor 5-8 - // 0x2FF: GM6020(voltage) motor 5-8 std::unordered_map tx_frames_; // CAN driver diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index 91d4454..71ca011 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -7,14 +7,22 @@ #include #include +#include "CanMessage.hpp" #include "angles/angles.h" #include "meta_hardware/motor_driver/dm_motor_driver.hpp" + + namespace meta_hardware { + +using sockcanpp::CanMessage; +using std::tuple; + DmMotor::DmMotor(const std::string &motor_model, uint32_t dm_motor_id, std::string mode, - double max_vel, double max_pos, double max_effort) + double max_vel, double max_pos, double max_effort, uint32_t Kp, uint32_t Kd, + uint32_t Tff) : motor_model_(motor_model), dm_motor_id_(dm_motor_id), - max_vel_(max_vel), max_pos_(max_pos), max_effort_(max_effort) { + max_vel_(max_vel), max_pos_(max_pos), max_effort_(max_effort), kp_(Kp), kd_(Kd), tff_(Tff) { uint32_t id_offeset = 0; if (mode == "MIT") { @@ -39,19 +47,111 @@ DmMotor::DmMotor(const std::string &motor_model, uint32_t dm_motor_id, std::stri uint32_t DmMotor::get_dm_motor_id() const { return dm_motor_id_; } canid_t DmMotor::get_tx_can_id() const { return tx_can_id_; } -void DmMotor::set_motor_feedback(uint8_t error_code, uint8_t id, - uint16_t position_raw, uint16_t velocity_raw, - uint16_t effort_raw, uint8_t temperature_mos, - uint8_t temperature_rotor) { +CanMessage DmMotor::get_motor_enable_frame(uint8_t master_id) const{ + canid_t enable_can_id = 3 << 24; + enable_can_id |= master_id << 8; + enable_can_id |= dm_motor_id_; + can_frame enable_frame{.can_id = enable_can_id, + .can_dlc = 8, + .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC}}; + return CanMessage(enable_frame); +} + +CanMessage DmMotor::get_motor_disable_frame(uint8_t master_id) const{ + canid_t disable_can_id = 4 << 24; + disable_can_id |= master_id << 8; + disable_can_id |= dm_motor_id_; + can_frame disable_frame{.can_id = disable_can_id, + .can_dlc = 8, + .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD}}; + return CanMessage(disable_frame); +} + +CanMessage DmMotor::get_motor_command_frame(double position,double velocity,double effort) const{ + uint32_t can_id_offset = 0x00; + + if (!std::isnan(position) && std::isnan(velocity) && + std::isnan(effort)) { // Position only mode + velocity = 0.0; // Set target velocity to 0 + effort = 0.0; // Set feedforward torque to 0 + } else if (std::isnan(position) && !std::isnan(velocity) && + std::isnan(effort)) { // Velocity only mode + effort = 0.0; // Set feedforward torque to 0 + can_id_offset = 0x100; + } else{ + can_id_offset = 0x200; + } + + // Only Implement the MIT mode + + can_frame command_frame; + command_frame.can_id = id_ + can_id_offset; + + auto position_raw = static_cast(); + auto velocity_raw = static_cast(); + auto effort_raw = static_cast(); + + command_frame.can_dlc = 8; + + command_frame.data[0] = position_raw >> 8; + command_frame.data[1] = position_raw & 0xFF; + + command_frame.data[2] = velocity_raw >> 8; + command_frame.data[3] = velocity_raw & 0xFF; + + command_frame.data[4] = Kp_raw_ >> 8; + command_frame.data[5] = Kp_raw_ & 0xFF; - double position = static_cast(position_raw - (1 << 15)) / (1 << 16) * max_pos_; + command_frame.data[6] = Kd_raw_ >> 8; + command_frame.data[7] = Kd_raw_ & 0xFF; + + return CanMessage(command_frame); +} + + +void DmMotor::set_motor_feedback(const sockcanpp::CanMessage &can_msg){ + + auto error_code = static_cast + (static_cast(can_msg.getRawFrame().data[0]) >> 4); + + auto id = static_cast + ((static_cast(can_msg.getRawFrame().data[0]) & 0x0F)); + + auto position_raw = static_cast + (((static_cast(can_msg.getRawFrame().data[1]) & 0xFF) << 8) | + (static_cast(can_msg.getRawFrame().data[2]) & 0xFF) + ); + + auto velocity_raw = static_cast + (((static_cast(can_msg.getRawFrame().data[3]) & 0xFF) << 8) | + ((static_cast(can_msg.getRawFrame().data[4]) & 0xC0) >> 12) + ); + + auto effort_raw = static_cast + (((static_cast(can_msg.getRawFrame().data[4]) & 0x3F) << 4) | + (static_cast(can_msg.getRawFrame().data[5]) & 0xFF) + ); + + auto temperature_mos = static_cast + (static_cast(can_msg.getRawFrame().data[6])); + + + auto temperature_rotor = static_cast + (static_cast(can_msg.getRawFrame().data[7])); + + + double position = static_cast(position_raw) / ((1 << 16) - 1) * 2 * max_pos_ - max_pos_; position_ += angles::shortest_angular_distance(position_, position); - velocity_ = static_cast(velocity_raw - (1 << 11)) / (1 << 12) * max_vel_; - toruqe_ = static_cast(effort_raw - (1 << 11)) / (1 << 12) * max_effort_; + + velocity_ = static_cast(velocity_raw) / ((1 << 12) - 1) * 2 * max_vel_ - max_vel_; + + toruqe_ = static_cast(effort_raw) / ((1 << 12) - 1) * 2 * max_effort_ - max_effort_; + error_code_ = error_code; id_ = id; temperature_mos_ = temperature_mos; temperature_rotor_ = temperature_rotor; + } std::tuple DmMotor::get_motor_feedback() const { diff --git a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp index 56c74fa..b00496a 100644 --- a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -86,34 +86,6 @@ void DmMotorNetwork::rx_loop() { const auto &motor = motor_id2motor_.at(can_msg.can_id); - auto error_code = static_cast - (static_cast(can_msg.data[0]) >> 4); - - auto id = static_cast - ((static_cast(can_msg.data[0]) & 0x0F)); - - auto position_raw = static_cast - (((static_cast(can_msg.data[1]) & 0xFF) << 8) | - (static_cast(can_msg.data[2]) & 0xFF) - ); - - auto velocity_raw = static_cast - (((static_cast(can_msg.data[3]) & 0xFF) << 8) | - ((static_cast(can_msg.data[4]) & 0xC0) >> 12) - ); - - auto torque_raw = static_cast - (((static_cast(can_msg.data[4]) & 0x3F) << 4) | - (static_cast(can_msg.data[5]) & 0xFF) - ); - - auto temperature_mos = static_cast - (static_cast(can_msg.data[6])); - - - auto temperature_rotor = static_cast - (static_cast(can_msg.data[7])); - motor->set_motor_feedback(error_code, id, position_raw, velocity_raw, torque_raw, temperature_mos, temperature_rotor); } catch (std::runtime_error &e) { From d4df87e8520af7570f3ae7f2be407252ea87b572 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Wed, 14 Aug 2024 17:29:22 +0800 Subject: [PATCH 09/36] Add motor with external encoder to playground --- .../urdf/playground/multiple_motors.xacro | 2 +- .../urdf/playground/single_motor.xacro | 2 +- .../playground/single_motor_ext_encoder.xacro | 81 +++++++++++ .../meta_hardware/brt_encoder_interface.hpp | 6 +- .../src/motor_network/brt_encoder_network.cpp | 11 +- .../config/single_motor_ext_encoder.yaml | 34 +++++ .../launch/single_motor_ext_encoder.launch.py | 130 ++++++++++++++++++ 7 files changed, 253 insertions(+), 13 deletions(-) create mode 100644 decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro create mode 100644 meta_bringup/config/single_motor_ext_encoder.yaml create mode 100644 meta_bringup/launch/single_motor_ext_encoder.launch.py diff --git a/decomposition/metav_description/urdf/playground/multiple_motors.xacro b/decomposition/metav_description/urdf/playground/multiple_motors.xacro index 64150d6..cffbd63 100644 --- a/decomposition/metav_description/urdf/playground/multiple_motors.xacro +++ b/decomposition/metav_description/urdf/playground/multiple_motors.xacro @@ -87,7 +87,7 @@ - $(find metav_description)/config/multiple_motors.yaml + $(find meta_bringup)/config/multiple_motors.yaml diff --git a/decomposition/metav_description/urdf/playground/single_motor.xacro b/decomposition/metav_description/urdf/playground/single_motor.xacro index cf32759..b9f7948 100644 --- a/decomposition/metav_description/urdf/playground/single_motor.xacro +++ b/decomposition/metav_description/urdf/playground/single_motor.xacro @@ -48,7 +48,7 @@ - $(find metav_description)/config/single_motor.yaml + $(find meta_bringup)/config/single_motor.yaml diff --git a/decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro b/decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro new file mode 100644 index 0000000..78f7418 --- /dev/null +++ b/decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + meta_hardware/MetaRobotMiMotorNetwork + can0 + + + + + + CyberGear + 1 + -1.0 + 0.0 + dynamic + 0.0 + 1.0 + + + + + + meta_hardware/MetaRobotBrtEncoderNetwork + can0 + + + + BRT38-1024 + 1 + -1.0 + 0.0 + + + + + + + + $(find meta_bringup)/config/single_motor_ext_encoder.yaml + + + + \ No newline at end of file diff --git a/execution/meta_hardware/include/meta_hardware/brt_encoder_interface.hpp b/execution/meta_hardware/include/meta_hardware/brt_encoder_interface.hpp index e930308..027ef4f 100644 --- a/execution/meta_hardware/include/meta_hardware/brt_encoder_interface.hpp +++ b/execution/meta_hardware/include/meta_hardware/brt_encoder_interface.hpp @@ -1,5 +1,5 @@ -#ifndef META_HARDWARE__MI_MOTOR_INTERFACE_HPP_ -#define META_HARDWARE__MI_MOTOR_INTERFACE_HPP_ +#ifndef META_HARDWARE__BRITTER_ENCODER_INTERFACE_HPP_ +#define META_HARDWARE__BRITTER_ENCODER_INTERFACE_HPP_ #include #include @@ -73,4 +73,4 @@ class MetaRobotBrtEncoderNetwork : public hardware_interface::SystemInterface { } // namespace meta_hardware -#endif // META_HARDWARE__MI_MOTOR_INTERFACE_HPP_ +#endif // META_HARDWARE__BRITTER_ENCODER_INTERFACE_HPP_ diff --git a/execution/meta_hardware/src/motor_network/brt_encoder_network.cpp b/execution/meta_hardware/src/motor_network/brt_encoder_network.cpp index c5490bb..05dae4f 100644 --- a/execution/meta_hardware/src/motor_network/brt_encoder_network.cpp +++ b/execution/meta_hardware/src/motor_network/brt_encoder_network.cpp @@ -23,7 +23,7 @@ BrtEncoderNetwork::BrtEncoderNetwork(const string &can_network_name, std::vector filters; - // Initialize MI motor drivers + // Initialize BRITTER encoder drivers for (const auto &joint_param : joint_params) { uint32_t brt_encoder_id = std::stoi(joint_param.at("encoder_id")); @@ -36,12 +36,7 @@ BrtEncoderNetwork::BrtEncoderNetwork(const string &can_network_name, filters.push_back({.can_id = 0x001, .can_mask = CAN_EFF_MASK}); // Initialize CAN driver - try { - can_driver_ = std::make_unique(can_network_name, false, filters); - } catch (CanException &e) { - std::cerr << "Failed to initialize CAN driver: " << e.what() << std::endl; - throw std::runtime_error("Failed to initialize MI motor network"); - } + can_driver_ = std::make_unique(can_network_name, false, filters); // Start RX thread rx_thread_ = @@ -62,7 +57,7 @@ void BrtEncoderNetwork::rx_loop(std::stop_token stop_token) { uint8_t encoder_id = can_msg.data[1]; encoder_id2encoder_.at(encoder_id)->set_encoder_feedback(can_msg); } catch (CanIOTimedOutException & /*e*/) { - std::cerr << "Timed out waiting for MI motor feedback." << std::endl; + std::cerr << "Timed out waiting for BRITTER encoder feedback." << std::endl; } catch (CanIOException &e) { std::cerr << "Error reading CAN message: " << e.what() << std::endl; } diff --git a/meta_bringup/config/single_motor_ext_encoder.yaml b/meta_bringup/config/single_motor_ext_encoder.yaml new file mode 100644 index 0000000..1d7e288 --- /dev/null +++ b/meta_bringup/config/single_motor_ext_encoder.yaml @@ -0,0 +1,34 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + motor_pos2vel_pid_controller: + type: pid_controller/PidController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +forward_position_controller: + ros__parameters: + joints: + - motor_pos2vel_pid_controller/motor_joint + interface_name: position + +motor_pos2vel_pid_controller: + ros__parameters: + dof_names: + - motor_joint + + reference_and_state_dof_names: + - motor_joint + + command_interface: velocity + + reference_and_state_interfaces: ["position"] + + gains: + motor_joint: + { p: 10.0, i: 0.0, d: 0.0} diff --git a/meta_bringup/launch/single_motor_ext_encoder.launch.py b/meta_bringup/launch/single_motor_ext_encoder.launch.py new file mode 100644 index 0000000..f28f172 --- /dev/null +++ b/meta_bringup/launch/single_motor_ext_encoder.launch.py @@ -0,0 +1,130 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +# Necessary dirty work that lets us import modules from the meta_bringup package +import os +import sys +from ament_index_python.packages import get_package_share_directory +sys.path.append(os.path.join(get_package_share_directory('meta_bringup'), 'launch')) + +from launch_utils import load_controller, register_loading_order, register_sequential_loading + +ARGUMENTS = [ + DeclareLaunchArgument( + 'enable_simulation', + default_value='false', + description='If true, the simulation will be started'), +] + +def generate_launch_description(): + # Launch Arguments + enable_simulation = LaunchConfiguration('enable_simulation') + + # Get URDF via xacro + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'single_motor_ext_encoder.xacro']), + ' ', + 'is_simulation:=', enable_simulation, + ]) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[ + {'use_sim_time': enable_simulation}, + {'robot_description': robot_description_content},], + output='both', + emulate_tty=True + ) + + # Gazebo related launch + world_sdf = PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'worlds', 'empty_world.sdf']) + bridge_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'ros_gz_bridge.yaml']) + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'launch', 'meta_gazebo.launch.py'])], + ), + launch_arguments=[ + ('world_sdf', world_sdf), + ('robot_name', 'single_motor_ext_encoder'), + ('bridge_config_file', bridge_config), + ], + condition=IfCondition(enable_simulation) + ) + + # ROS2 Control related launch + robot_controller_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'single_motor_ext_encoder.yaml']) + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controller_config], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output='both', + emulate_tty=True, + condition=UnlessCondition(enable_simulation) + ) + + load_joint_state_broadcaster = load_controller('joint_state_broadcaster') + + # List of controllers to be loaded sequentially + # Order in this list is IMPORTANT + load_controllers = [ + load_controller('motor_pos2vel_pid_controller'), + load_controller('forward_position_controller'), + ] + + motor_tester_node = Node( + package='motor_tester', + executable='motor_tester', + name='motor_tester_node', + parameters=[robot_controller_config], + ) + + dbus_control_node = Node( + package='dbus_control', + executable='dbus_control_node', + name='dbus_control_node', + parameters=[robot_controller_config], + ) + + return LaunchDescription([ + # Launch Arguments + *ARGUMENTS, + # Launch Gazebo and ROS2 bridge and spawn robot in Gazebo (also start controller manager) + gazebo_launch, + # Load robot state publisher + node_robot_state_publisher, + # Launch controller manager (if not in simulation) + controller_manager, + # Load joint state broadcaster + load_joint_state_broadcaster, + # Load controllers + *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), + motor_tester_node, + dbus_control_node, + ]) From ca4ae7299c999932473bc49b9488fc08a0e15a94 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Wed, 14 Aug 2024 19:24:31 +0800 Subject: [PATCH 10/36] driver modified --- .../motor_driver/dm_motor_driver.hpp | 53 +++--- .../src/motor_driver/dm_motor_driver.cpp | 161 +++++++++++------- 2 files changed, 134 insertions(+), 80 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp index 512d8e5..b6b9550 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include "CanMessage.hpp" #include @@ -16,50 +17,60 @@ namespace meta_hardware { class DmMotor { public: - DmMotor(const std::string &motor_model, uint32_t dm_motor_id,std::string mode, - double max_vel, double max_pos, double max_effort, uint32_t Kp, uint32_t Kd, - uint32_t Tff); + // DmMotor(const std::string &motor_model, uint32_t dm_motor_id,std::string mode, + // double max_vel, double max_pos, double max_effort, uint32_t Kp, uint32_t Kd, + // uint32_t Tff); + DmMotor(const std::unordered_map &motor_param, + uint8_t master_id); ~DmMotor() = default; - enum DmMode { + enum RunMode { MIT, - POS, - VEL, + POSITION, + VELOCITY, }; uint32_t get_dm_motor_id() const; uint32_t get_tx_can_id() const; uint32_t get_rx_can_id() const; - DmMode get_mode() const { return mode_; } - - sockcanpp::CanMessage get_motor_enable_frame(uint8_t master_id) const; - sockcanpp::CanMessage get_motor_disable_frame(uint8_t master_id) const; - sockcanpp::CanMessage get_motor_save_initial_frame(uint8_t master_id) const; - sockcanpp::CanMessage get_motor_clear_error_frame(uint8_t master_id) const; - sockcanpp::CanMessage get_motor_command_frame(double position, - double velocity, - double effort) const; - - + RunMode get_mode() const { return run_mode_; } + + + can_frame motor_enable_frame(uint8_t master_id) const; + can_frame motor_disable_frame(uint8_t master_id) const; + can_frame motor_save_initial_frame(uint8_t master_id) const; + can_frame motor_clear_error_frame(uint8_t master_id) const; + can_frame motor_mit_frame(double position, double velocity, double effort) const; + can_frame motor_pos_frame(double position, double velocity) const; + can_frame motor_vel_frame(double velocity) const; + void set_motor_feedback(const sockcanpp::CanMessage &can_msg); std::tuple get_motor_feedback() const; private: + // helper function + uint32_t double_to_raw(float value, float max, float min, uint8_t bit) const; + // Motor information std::string motor_model_; uint32_t dm_motor_id_; - DmMode mode_; + RunMode run_mode_; uint32_t tx_can_id_; uint32_t rx_can_id_; + uint32_t master_id_; + double max_vel_; double max_pos_; double max_effort_; - uint32_t kp_; - uint32_t kd_; - uint32_t tff_; + double Kp_; + double Kd_; + uint32_t Tff_; + uint8_t Kp_raw_; + uint16_t Kd_raw_; + uint16_t Tff_raw_; // Motor feedback uint8_t error_code_{0}; diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index 71ca011..3190fae 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -15,26 +15,44 @@ namespace meta_hardware { -using sockcanpp::CanMessage; using std::tuple; -DmMotor::DmMotor(const std::string &motor_model, uint32_t dm_motor_id, std::string mode, - double max_vel, double max_pos, double max_effort, uint32_t Kp, uint32_t Kd, - uint32_t Tff) - : motor_model_(motor_model), dm_motor_id_(dm_motor_id), - max_vel_(max_vel), max_pos_(max_pos), max_effort_(max_effort), kp_(Kp), kd_(Kd), tff_(Tff) { +constexpr double MAX_KP = 500.0; +constexpr double MIN_KP = 0.0; +constexpr double MAX_KD = 5.0; +constexpr double MIN_KD = 0.0; +DmMotor::DmMotor(const std::unordered_map &motor_param, + uint8_t master_id) : master_id_(master_id) { + + motor_model_ = motor_param.at("motor_model"); + dm_motor_id_ = static_cast(std::stoi(motor_param.at("motor_id"))); + + max_pos_ = std::stod(motor_param.at("max_pos")); + max_vel_ = std::stod(motor_param.at("max_vel")); + max_effort_ = std::stod(motor_param.at("max_effort")); + + std::string control_mode = motor_param.at("control_mode"); uint32_t id_offeset = 0; - if (mode == "MIT") { - mode_ = DmMode::MIT; - } else if (mode == "POS") { - mode_ = DmMode::POS; + + if (control_mode == "MIT") { + Kp_ = std::stod(motor_param.at("Kp")); + Kd_ = std::stod(motor_param.at("Kd")); + Tff_ = std::stod(motor_param.at("Tff")); + + Kp_raw_ = static_cast((Kp_ - MAX_KP)/(MAX_KP - MIN_KP)) * ( (1 << 8) - 1); + Kd_raw_ = static_cast((Kd_ - MAX_KD)/(MAX_KD - MIN_KD)) * ( (1 << 12) - 1); + + run_mode_ = RunMode::MIT; + + } else if (control_mode == "postition") { + run_mode_ = RunMode::POSITION; id_offeset = 0x100; - } else if (mode == "VEL") { - mode_ = DmMode::VEL; + } else if (control_mode == "velocity") { + run_mode_ = RunMode::VELOCITY; id_offeset = 0x200; } else { - throw std::runtime_error("Unknown motor mode: " + mode); + throw std::runtime_error("Unknown motor mode: " + control_mode); } if (motor_model_ == "4310") { @@ -47,67 +65,87 @@ DmMotor::DmMotor(const std::string &motor_model, uint32_t dm_motor_id, std::stri uint32_t DmMotor::get_dm_motor_id() const { return dm_motor_id_; } canid_t DmMotor::get_tx_can_id() const { return tx_can_id_; } -CanMessage DmMotor::get_motor_enable_frame(uint8_t master_id) const{ - canid_t enable_can_id = 3 << 24; - enable_can_id |= master_id << 8; - enable_can_id |= dm_motor_id_; +can_frame DmMotor::motor_enable_frame(uint8_t master_id) const{ + canid_t enable_can_id = master_id; can_frame enable_frame{.can_id = enable_can_id, .can_dlc = 8, .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC}}; - return CanMessage(enable_frame); + return enable_frame; } -CanMessage DmMotor::get_motor_disable_frame(uint8_t master_id) const{ - canid_t disable_can_id = 4 << 24; - disable_can_id |= master_id << 8; - disable_can_id |= dm_motor_id_; +can_frame DmMotor::motor_disable_frame(uint8_t master_id) const{ + canid_t disable_can_id = master_id; can_frame disable_frame{.can_id = disable_can_id, .can_dlc = 8, .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD}}; - return CanMessage(disable_frame); + return disable_frame; } -CanMessage DmMotor::get_motor_command_frame(double position,double velocity,double effort) const{ - uint32_t can_id_offset = 0x00; - - if (!std::isnan(position) && std::isnan(velocity) && - std::isnan(effort)) { // Position only mode - velocity = 0.0; // Set target velocity to 0 - effort = 0.0; // Set feedforward torque to 0 - } else if (std::isnan(position) && !std::isnan(velocity) && - std::isnan(effort)) { // Velocity only mode - effort = 0.0; // Set feedforward torque to 0 - can_id_offset = 0x100; - } else{ - can_id_offset = 0x200; - } - - // Only Implement the MIT mode - - can_frame command_frame; - command_frame.can_id = id_ + can_id_offset; - - auto position_raw = static_cast(); - auto velocity_raw = static_cast(); - auto effort_raw = static_cast(); - - command_frame.can_dlc = 8; - - command_frame.data[0] = position_raw >> 8; - command_frame.data[1] = position_raw & 0xFF; - - command_frame.data[2] = velocity_raw >> 8; - command_frame.data[3] = velocity_raw & 0xFF; +can_frame DmMotor::motor_save_initial_frame(uint8_t master_id) const{ + canid_t disable_can_id = master_id; + can_frame disable_frame{.can_id = disable_can_id, + .can_dlc = 8, + .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE}}; + return disable_frame; +} - command_frame.data[4] = Kp_raw_ >> 8; - command_frame.data[5] = Kp_raw_ & 0xFF; +can_frame DmMotor::motor_clear_error_frame(uint8_t master_id) const{ + canid_t disable_can_id = master_id; + can_frame disable_frame{.can_id = disable_can_id, + .can_dlc = 8, + .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB}}; + return disable_frame; +} - command_frame.data[6] = Kd_raw_ >> 8; - command_frame.data[7] = Kd_raw_ & 0xFF; +can_frame DmMotor::motor_mit_frame(double position, double velocity, double effort) const{ + uint16_t velocity_raw = double_to_raw(velocity, max_vel_, -max_vel_, 16); + uint16_t position_raw = double_to_raw(position, max_pos_, -max_pos_, 16); + uint16_t effort_raw = double_to_raw(effort, max_effort_, -max_effort_, 16); + can_frame frame = { + .can_id = tx_can_id_, + .can_dlc = 8, + .data = { static_cast((position_raw & 0xFF00) >> 8), + static_cast(position_raw & 0x00FF), + static_cast((velocity_raw & 0x0FF0) >> 4), + static_cast(((velocity_raw & 0x000F) << 4) | ((Kp_raw_ & 0x0F00) >> 8)), + static_cast(Kp_raw_ & 0x00FF), + static_cast((Kd_raw_ & 0x0FF0) >> 4), + static_cast(((Kd_raw_ & 0x000F) << 4) | ((effort_raw & 0x0F00) >> 8)), + static_cast(effort_raw & 0x00FF)} + }; + return frame; +} - return CanMessage(command_frame); +can_frame DmMotor::motor_pos_frame(double position, double velocity) const{ + uint32_t velocity_raw = double_to_raw(velocity, max_vel_, -max_vel_, 32); + uint32_t position_raw = double_to_raw(position, max_pos_, -max_pos_, 32); + can_frame frame = { + .can_id = tx_can_id_, + .can_dlc = 8, + .data = { static_cast((position_raw & 0xFF000000) >> 24), + static_cast((position_raw & 0x00FF0000) >> 16), + static_cast((position_raw & 0x0000FF00) >> 8), + static_cast(position_raw & 0x000000FF), + static_cast((velocity_raw & 0xFF000000) >> 24), + static_cast((velocity_raw & 0x00FF0000) >> 16), + static_cast((velocity_raw & 0x0000FF00) >> 8), + static_cast(velocity_raw & 0x000000FF)} + }; + return frame; } +can_frame DmMotor::motor_vel_frame(double velocity) const{ + uint32_t velocity_raw = double_to_raw(velocity, max_vel_, -max_vel_, 32); + can_frame frame = { + .can_id = tx_can_id_, + .can_dlc = 4, + .data = { static_cast((velocity_raw & 0xFF000000) >> 24), + static_cast((velocity_raw & 0x00FF0000) >> 16), + static_cast((velocity_raw & 0x0000FF00) >> 8), + static_cast(velocity_raw & 0x000000FF)} + }; + return frame; +} void DmMotor::set_motor_feedback(const sockcanpp::CanMessage &can_msg){ @@ -158,4 +196,9 @@ std::tuple DmMotor::get_motor_feedback() const { return {position_, velocity_, toruqe_}; } +uint32_t DmMotor::double_to_raw(float value, float max, float min, uint8_t bit) const { + double span = max - min; + return (int) ((value-min)*((float)((1< Date: Wed, 14 Aug 2024 20:39:50 +0800 Subject: [PATCH 11/36] dm motor driver and network first version --- .../motor_driver/dm_motor_driver.hpp | 14 +-- .../motor_network/dm_motor_network.hpp | 35 +++--- .../src/motor_driver/dm_motor_driver.cpp | 16 +-- .../src/motor_network/dm_motor_network.cpp | 114 +++++++++--------- 4 files changed, 89 insertions(+), 90 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp index b6b9550..eed15e7 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -34,13 +34,11 @@ class DmMotor { uint32_t get_dm_motor_id() const; uint32_t get_tx_can_id() const; uint32_t get_rx_can_id() const; - RunMode get_mode() const { return run_mode_; } - - - can_frame motor_enable_frame(uint8_t master_id) const; - can_frame motor_disable_frame(uint8_t master_id) const; - can_frame motor_save_initial_frame(uint8_t master_id) const; - can_frame motor_clear_error_frame(uint8_t master_id) const; + + can_frame motor_enable_frame() const; + can_frame motor_disable_frame() const; + can_frame motor_save_initial_frame() const; + can_frame motor_clear_error_frame() const; can_frame motor_mit_frame(double position, double velocity, double effort) const; can_frame motor_pos_frame(double position, double velocity) const; can_frame motor_vel_frame(double velocity) const; @@ -48,6 +46,8 @@ class DmMotor { void set_motor_feedback(const sockcanpp::CanMessage &can_msg); std::tuple get_motor_feedback() const; + RunMode get_run_mode() const { return run_mode_; } + private: // helper function uint32_t double_to_raw(float value, float max, float min, uint8_t bit) const; diff --git a/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp index 028c6f2..1aaa29a 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp @@ -18,10 +18,8 @@ namespace meta_hardware { class DmMotorNetwork { public: DmMotorNetwork( - std::string can_interface, - uint32_t master_id, - const std::vector> - &motor_params); + const std::string &can_network_name, uint8_t master_id, + const std::vector> &joint_params); ~DmMotorNetwork(); /** @@ -34,35 +32,32 @@ class DmMotorNetwork { /** * @brief Write the motor command * @param joint_id The joint ID of the motor + * @param position The position to write + * @param velocity The velocity to write * @param effort The effort to write */ - void write(uint32_t joint_id, double effort); - - /** - * @brief Transmit the motor commands - */ - void tx(); + void write_mit(uint32_t joint_id, double position, double velocity, double effort); + void write_pos(uint32_t joint_id, double position, double velocity); + void write_vel(uint32_t joint_id, double velocity); private: - void rx_loop(); - std::unique_ptr rx_thread_; - std::atomic rx_thread_running_{true}; - - uint32_t master_id_; - - // Five CAN frames for tx - std::unordered_map tx_frames_; - // CAN driver std::unique_ptr can_driver_; + void rx_loop(std::stop_token stop_token); + std::unique_ptr rx_thread_; + + // [rx_can_id] -> dm_motor // This makes it easy to find the motor object in rx_loop std::map> motor_id2motor_; // [joint_id] -> dm_motor // This makes it easy to find the motor object in read() and write() - std::vector> motors_; + std::vector> dm_motors_; + + // Master ID + uint32_t master_id_; }; } // namespace meta_hardware diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index 3190fae..c07a3b3 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -65,32 +65,32 @@ DmMotor::DmMotor(const std::unordered_map &motor_param uint32_t DmMotor::get_dm_motor_id() const { return dm_motor_id_; } canid_t DmMotor::get_tx_can_id() const { return tx_can_id_; } -can_frame DmMotor::motor_enable_frame(uint8_t master_id) const{ - canid_t enable_can_id = master_id; +can_frame DmMotor::motor_enable_frame() const{ + canid_t enable_can_id = tx_can_id_; can_frame enable_frame{.can_id = enable_can_id, .can_dlc = 8, .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC}}; return enable_frame; } -can_frame DmMotor::motor_disable_frame(uint8_t master_id) const{ - canid_t disable_can_id = master_id; +can_frame DmMotor::motor_disable_frame() const{ + canid_t disable_can_id = tx_can_id_; can_frame disable_frame{.can_id = disable_can_id, .can_dlc = 8, .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD}}; return disable_frame; } -can_frame DmMotor::motor_save_initial_frame(uint8_t master_id) const{ - canid_t disable_can_id = master_id; +can_frame DmMotor::motor_save_initial_frame() const{ + canid_t disable_can_id = tx_can_id_; can_frame disable_frame{.can_id = disable_can_id, .can_dlc = 8, .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE}}; return disable_frame; } -can_frame DmMotor::motor_clear_error_frame(uint8_t master_id) const{ - canid_t disable_can_id = master_id; +can_frame DmMotor::motor_clear_error_frame() const{ + canid_t disable_can_id = tx_can_id_; can_frame disable_frame{.can_id = disable_can_id, .can_dlc = 8, .data = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFB}}; diff --git a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp index b00496a..45ad4d2 100644 --- a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -13,25 +13,20 @@ namespace meta_hardware { -DmMotorNetwork::DmMotorNetwork( - std::string can_interface, - uint32_t master_id, - const std::vector> - &motor_params) : master_id_(master_id) { - std::vector can_filters; +DmMotorNetwork::DmMotorNetwork(const std::string &can_network_name, uint8_t master_id, + const std::vector> &joint_params) + : master_id_(master_id) { - for (const auto &motor_param : motor_params) { - std::string motor_model = motor_param.at("motor_model"); - uint32_t dm_motor_id = std::stoi(motor_param.at("motor_id")); - std::string motor_mode = motor_param.at("motor_mode"); - double max_vel = std::stod(motor_param.at("max_vel")); - double max_pos = std::stod(motor_param.at("max_pos")); - double max_torque = std::stod(motor_param.at("max_torque")); + std::vector can_filters; + for (const auto &joint_param : joint_params) { + std::string motor_model = joint_param.at("motor_model"); + uint32_t dm_motor_id = std::stoi(joint_param.at("motor_id")); + std::string motor_mode = joint_param.at("motor_mode"); auto dm_motor = - std::make_shared(motor_model, dm_motor_id, motor_mode, max_vel, max_pos, max_torque); - motors_.emplace_back(dm_motor); + std::make_shared(joint_params, master_id); + dm_motors_.emplace_back(dm_motor); motor_id2motor_[dm_motor_id] = dm_motor; @@ -42,68 +37,77 @@ DmMotorNetwork::DmMotorNetwork( // Initialize CAN driver can_driver_ = - std::make_unique(can_interface, false, can_filters); + std::make_unique(can_network_name, false, can_filters); + + // Enable all motors + for (const auto &motor : dm_motors_) { + can_driver_->write(motor->motor_enable_frame()); + } // Initialize RX thread rx_thread_ = - std::make_unique(&DmMotorNetwork::rx_loop, this); + std::make_unique([this](std::stop_token s) { rx_loop(s); }); } DmMotorNetwork::~DmMotorNetwork() { - // Send zero effort to all motors - for (canid_t tx_can_id : {0x1FE, 0x1FF, 0x200, 0x2FE, 0x2FF}) { - can_frame tx_frame{.can_id = tx_can_id, .len = 8, .data = {0}}; - can_driver_->write(tx_frame); + // Disable all motors + try { + for (const auto &motor : dm_motors_) { + can_driver_->write(motor->motor_disable_frame()); + } + } catch (CanIOException &e) { + std::cerr << "Error writing DM motor disable CAN message: " << e.what() + << std::endl; } - - // Stop the RX thread - rx_thread_running_ = false; } std::tuple DmMotorNetwork::read(uint32_t joint_id) const { - return motors_[joint_id]->get_motor_feedback(); + return dm_motors_[joint_id]->get_motor_feedback(); } -void DmMotorNetwork::write(uint32_t joint_id, double effort) { - const auto &motor = motors_[joint_id]; - uint32_t dm_motor_id = motor->get_dm_motor_id(); - uint32_t tx_can_id = motor->get_tx_can_id(); +void DmMotorNetwork::write_mit(uint32_t joint_id, double position, double velocity, double effort){ + const auto &motor = dm_motors_[joint_id]; + try { + can_driver_->write(motor->motor_mit_frame(position, velocity, effort)); + } catch (CanIOException &e) { + std::cerr << "Error writing DM motor command CAN message: " << e.what() + << std::endl; + } +} - // uint32_t maximum_raw_effort = motor->get_maximum_raw_effort(); - // effort = std::clamp(effort, -1.0, 1.0); - // auto effort_raw = static_cast(effort * maximum_raw_effort); +void DmMotorNetwork::write_pos(uint32_t joint_id, double position, double velocity){ + const auto &motor = dm_motors_[joint_id]; + try { + can_driver_->write(motor->motor_pos_frame(position, velocity)); + } catch (CanIOException &e) { + std::cerr << "Error writing DM motor command CAN message: " << e.what() + << std::endl; + } +} - // can_frame &tx_frame = tx_frames_[tx_can_id]; - // tx_frame.data[2 * ((dm_motor_id - 1) % 4)] = effort_raw >> 8; - // tx_frame.data[2 * ((dm_motor_id - 1) % 4) + 1] = effort_raw & 0xFF; +void DmMotorNetwork::write_vel(uint32_t joint_id, double velocity){ + const auto &motor = dm_motors_[joint_id]; + try { + can_driver_->write(motor->motor_vel_frame(velocity)); + } catch (CanIOException &e) { + std::cerr << "Error writing DM motor command CAN message: " << e.what() + << std::endl; + } } -void DmMotorNetwork::rx_loop() { - while (rx_thread_running_) { +void DmMotorNetwork::rx_loop(std::stop_token stop_token) { + while (!stop_token.stop_requested()) { try { - can_frame can_msg = can_driver_->read(); - + auto can_msg = can_driver_->read(2000); const auto &motor = motor_id2motor_.at(can_msg.can_id); - - motor->set_motor_feedback(error_code, id, position_raw, velocity_raw, - torque_raw, temperature_mos, temperature_rotor); - } catch (std::runtime_error &e) { + motor->set_motor_feedback(can_msg); + } catch (CanIOTimedOutException & /*e*/) { + std::cerr << "Timed out waiting for DM motor feedback." << std::endl; + } catch (CanIOException &e) { std::cerr << "Error reading CAN message: " << e.what() << std::endl; } } } -void DmMotorNetwork::tx() { - try { - for (auto &frame_id : {0x1fe, 0x1ff, 0x200, 0x2fe, 0x2ff}) { - if (tx_frames_.contains(frame_id)) { - can_driver_->write(tx_frames_[frame_id]); - } - } - } catch (std::runtime_error &e) { - std::cerr << "Error writing CAN message: " << e.what() << std::endl; - } -} - } // namespace meta_hardware From f12825ecf3c78c51c7d90378e30f2847196aa6d4 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Thu, 15 Aug 2024 02:46:55 +0800 Subject: [PATCH 12/36] Remove can_network param from wheels --- .../urdf/common/omni_wheel.xacro | 10 +----- .../urdf/sentry/sentry.gimbal.xacro | 2 +- .../urdf/sentry/sentry.xacro | 32 +++++++------------ 3 files changed, 14 insertions(+), 30 deletions(-) diff --git a/decomposition/metav_description/urdf/common/omni_wheel.xacro b/decomposition/metav_description/urdf/common/omni_wheel.xacro index 322f442..9af95b6 100644 --- a/decomposition/metav_description/urdf/common/omni_wheel.xacro +++ b/decomposition/metav_description/urdf/common/omni_wheel.xacro @@ -152,15 +152,7 @@ - - + diff --git a/decomposition/metav_description/urdf/sentry/sentry.gimbal.xacro b/decomposition/metav_description/urdf/sentry/sentry.gimbal.xacro index 6cbe166..244447b 100644 --- a/decomposition/metav_description/urdf/sentry/sentry.gimbal.xacro +++ b/decomposition/metav_description/urdf/sentry/sentry.gimbal.xacro @@ -92,7 +92,7 @@ + params="prefix mechanical_reduction offset motor_id"> diff --git a/decomposition/metav_description/urdf/sentry/sentry.xacro b/decomposition/metav_description/urdf/sentry/sentry.xacro index 539c826..810c6d5 100644 --- a/decomposition/metav_description/urdf/sentry/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry/sentry.xacro @@ -38,18 +38,14 @@ meta_hardware/MetaRobotDjiMotorNetwork can1 - - - - + + + + + motor_id="5" /> + motor_id="6" /> @@ -58,18 +54,14 @@ ign_ros2_control/IgnitionSystem - - - - + + + + + motor_id="5" /> + motor_id="6" /> From 437ad1d1acc7172873a10e2de3a6c359632e1fe1 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Thu, 15 Aug 2024 06:15:56 +0800 Subject: [PATCH 13/36] Fix issues in tf2 transform --- .../include/gimbal_controller/gimbal_controller.hpp | 2 +- .../meta_gimbal_controller/src/gimbal_controller.cpp | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp index 643aee3..35e18ba 100644 --- a/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp +++ b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp @@ -79,7 +79,7 @@ class GimbalController : public controller_interface::ChainableControllerInterfa std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - tf2::Quaternion q_imu2gimbal_; + tf2::Quaternion rot_imu2gimbal_; std::shared_ptr yaw_pos2vel_pid_; std::shared_ptr pitch_pos2vel_pid_; diff --git a/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp b/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp index 7fc58ca..4d384da 100644 --- a/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp +++ b/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -216,15 +217,16 @@ GimbalController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/ geometry_msgs::msg::TransformStamped trans; try { - trans = tf_buffer_->lookupTransform("gimbal_imu", "pitch_gimbal", - tf2::TimePointZero, tf2::Duration(1)); + trans = + tf_buffer_->lookupTransform("gimbal_imu", "pitch_gimbal", tf2::TimePointZero, + tf2::durationFromSec(10.0)); // wait for 10s } catch (tf2::LookupException &e) { RCLCPP_ERROR(get_node()->get_logger(), "Failed to lookup transform: %s", e.what()); return controller_interface::CallbackReturn::ERROR; } - q_imu2gimbal_ = + rot_imu2gimbal_ = tf2::Quaternion(trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w); @@ -260,12 +262,12 @@ GimbalController::update_and_write_commands(const rclcpp::Time &time, // Transform IMU feedback to gimbal frame tf2::Quaternion q_imu; fromMsg(current_feedback->orientation, q_imu); - auto q_gimbal = q_imu * q_imu2gimbal_; + tf2::Quaternion q_gimbal = q_imu * rot_imu2gimbal_; // Transform angular velocity feedback to gimbal frame tf2::Vector3 v_imu; fromMsg(current_feedback->angular_velocity, v_imu); - auto v_gimbal = tf2::quatRotate(q_imu2gimbal_.inverse(), v_imu); + tf2::Vector3 v_gimbal = tf2::quatRotate(rot_imu2gimbal_.inverse(), v_imu); double yaw_pos_fb, pitch_pos_fb, roll_pos_fb; tf2::Matrix3x3(q_gimbal).getRPY(roll_pos_fb, pitch_pos_fb, yaw_pos_fb); From 1f18b5057bd7de929e2bbfa009ff731507fd0923 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Thu, 15 Aug 2024 08:27:35 +0800 Subject: [PATCH 14/36] Add chassis mode topic --- .../meta_chassis_controller/CMakeLists.txt | 63 +--- .../omni_chassis_controller.hpp | 44 ++- .../visibility_control.h | 62 ---- .../meta_chassis_controller/package.xml | 2 + .../src/agv_chassis_controller.yaml | 48 +++ .../src/omni_chassis_controller.cpp | 276 +++++++++--------- interfaces/behavior_interface/CMakeLists.txt | 1 + interfaces/behavior_interface/msg/Chassis.msg | 9 + 8 files changed, 236 insertions(+), 269 deletions(-) delete mode 100644 decomposition/meta_chassis_controller/include/meta_chassis_controller/visibility_control.h create mode 100644 decomposition/meta_chassis_controller/src/agv_chassis_controller.yaml create mode 100644 interfaces/behavior_interface/msg/Chassis.msg diff --git a/decomposition/meta_chassis_controller/CMakeLists.txt b/decomposition/meta_chassis_controller/CMakeLists.txt index 132136a..82e7207 100644 --- a/decomposition/meta_chassis_controller/CMakeLists.txt +++ b/decomposition/meta_chassis_controller/CMakeLists.txt @@ -5,71 +5,34 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) endif() -# find dependencies -set(THIS_PACKAGE_INCLUDE_DEPENDS - angles - control_toolbox - control_msgs - controller_interface - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - realtime_tools - std_srvs - Eigen3 -) - find_package(ament_cmake REQUIRED) -find_package(generate_parameter_library REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) -endforeach() +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() -# Add omni_chassis_controller library related compile commands generate_parameter_library(omni_chassis_controller_parameters src/omni_chassis_controller.yaml - include/meta_chassis_controller/validate_omni_chassis_controller_parameters.hpp ) -add_library( - meta_chassis_controller +generate_parameter_library(agv_chassis_controller_parameters + src/agv_chassis_controller.yaml +) +ament_auto_add_library( + ${PROJECT_NAME} SHARED src/omni_chassis_controller.cpp src/omni_wheel_kinematics.cpp ) -target_include_directories(meta_chassis_controller PUBLIC +target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") -target_link_libraries(meta_chassis_controller omni_chassis_controller_parameters) -ament_target_dependencies(meta_chassis_controller ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(meta_chassis_controller + omni_chassis_controller_parameters + agv_chassis_controller_parameters) pluginlib_export_plugin_description_file( controller_interface meta_chassis_controller.xml) -install( - TARGETS - meta_chassis_controller - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - if(BUILD_TESTING) -endif() -ament_export_include_directories( - include -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -ament_export_libraries( - omni_chassis_controller -) +endif() -ament_package() +ament_auto_package() diff --git a/decomposition/meta_chassis_controller/include/meta_chassis_controller/omni_chassis_controller.hpp b/decomposition/meta_chassis_controller/include/meta_chassis_controller/omni_chassis_controller.hpp index 5025e3e..250c357 100644 --- a/decomposition/meta_chassis_controller/include/meta_chassis_controller/omni_chassis_controller.hpp +++ b/decomposition/meta_chassis_controller/include/meta_chassis_controller/omni_chassis_controller.hpp @@ -5,10 +5,10 @@ #include #include +#include "behavior_interface/msg/chassis.hpp" #include "control_toolbox/pid_ros.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "meta_chassis_controller/omni_wheel_kinematics.hpp" -#include "meta_chassis_controller/visibility_control.h" #include "omni_chassis_controller_parameters.hpp" #include "rclcpp/duration.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" @@ -28,40 +28,29 @@ enum class control_mode_type : std::uint8_t { CHASSIS_FOLLOW_GIMBAL = 2, }; -class OmniChassisController - : public controller_interface::ChainableControllerInterface { +class OmniChassisController : public controller_interface::ChainableControllerInterface { public: - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC OmniChassisController(); - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_init() override; - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override; - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override; - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC - controller_interface::return_type - update_reference_from_subscribers() override; + controller_interface::return_type update_reference_from_subscribers() override; - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC controller_interface::return_type update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) override; @@ -70,18 +59,19 @@ class OmniChassisController using ControllerReferenceMsgUnstamped = geometry_msgs::msg::Twist; using ControllerStateMsg = control_msgs::msg::JointControllerState; - protected: + private: std::shared_ptr param_listener_; omni_chassis_controller::Params params_; std::shared_ptr follow_pid_; - // Command subscribers and Controller State publisher rclcpp::Duration ref_timeout_ = rclcpp::Duration(0, 0); - rclcpp::Subscription::SharedPtr - ref_subscriber_ = nullptr; - realtime_tools::RealtimeBuffer> - input_ref_; + rclcpp::Subscription::SharedPtr twist_sub_; + realtime_tools::RealtimeBuffer> ref_buf_; + + rclcpp::Subscription::SharedPtr chassis_sub_; + realtime_tools::RealtimeBuffer> + chassis_buf_; using ControllerStatePublisher = realtime_tools::RealtimePublisher; @@ -98,11 +88,15 @@ class OmniChassisController bool on_set_chained_mode(bool chained_mode) override; - private: - // callback for topic interface - METAV_CHASSIS_CONTROLLER__VISIBILITY_LOCAL - void reference_callback( - const std::shared_ptr msg); + // Callbacks + void reference_callback(ControllerReferenceMsgUnstamped::UniquePtr msg); + + void chassis_follow_mode(Eigen::Vector3d &twist, const rclcpp::Time &time, + const rclcpp::Duration &period); + void gimbal_mode(Eigen::Vector3d &twist); + void gyro_mode(Eigen::Vector3d &twist); + + void transform_twist_to_gimbal(Eigen::Vector3d &twist, const double &yaw_gimbal_joint_pos) const; }; } // namespace meta_chassis_controller diff --git a/decomposition/meta_chassis_controller/include/meta_chassis_controller/visibility_control.h b/decomposition/meta_chassis_controller/include/meta_chassis_controller/visibility_control.h deleted file mode 100644 index 5219fb2..0000000 --- a/decomposition/meta_chassis_controller/include/meta_chassis_controller/visibility_control.h +++ /dev/null @@ -1,62 +0,0 @@ -// Copyright (c) 2024, Stogl Robotics Consulting UG (haftungsbeschränkt) -// (template) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -// -// Source of this file are templates in -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) -// repository. -// - -#ifndef METAV_CHASSIS_CONTROLLER__VISIBILITY_CONTROL_H_ -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) -#else -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) -#endif -#ifdef METAV_CHASSIS_CONTROLLER__VISIBILITY_BUILDING_DLL -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC \ - METAV_CHASSIS_CONTROLLER__VISIBILITY_EXPORT -#else -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC \ - METAV_CHASSIS_CONTROLLER__VISIBILITY_IMPORT -#endif -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ - METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_LOCAL -#else -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_EXPORT \ - __attribute__((visibility("default"))) -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_IMPORT -#if __GNUC__ >= 4 -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC \ - __attribute__((visibility("default"))) -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_LOCAL \ - __attribute__((visibility("hidden"))) -#else -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_LOCAL -#endif -#define METAV_CHASSIS_CONTROLLER__VISIBILITY_PUBLIC_TYPE -#endif - -#endif // METAV_CHASSIS_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/decomposition/meta_chassis_controller/package.xml b/decomposition/meta_chassis_controller/package.xml index 4ef6b4e..4e4c463 100644 --- a/decomposition/meta_chassis_controller/package.xml +++ b/decomposition/meta_chassis_controller/package.xml @@ -13,6 +13,8 @@ angles + behavior_interface + control_toolbox control_msgs diff --git a/decomposition/meta_chassis_controller/src/agv_chassis_controller.yaml b/decomposition/meta_chassis_controller/src/agv_chassis_controller.yaml new file mode 100644 index 0000000..ea7055c --- /dev/null +++ b/decomposition/meta_chassis_controller/src/agv_chassis_controller.yaml @@ -0,0 +1,48 @@ +agv_chassis_controller: + agv_wheel_joints: + { + type: string_array, + default_value: [], + description: "Specifies joints of the AGV wheels.", + read_only: true, + validation: { unique<>: null, not_empty<>: null }, + } + agv_wheel_center_x: + { + type: double_array, + default_value: [], + description: "Specifies the x coordinate of the AGV wheel center.", + read_only: true, + validation: { not_empty<>: null }, + } + agv_wheel_center_y: + { + type: double_array, + default_value: [], + description: "Specifies the y coordinate of the AGV wheel center.", + read_only: true, + validation: { not_empty<>: null }, + } + agv_wheel_radius: + { type: double, default_value: 0.0, description: "Specifies radius of the AGV wheels." } + control_mode: + { + type: int, + default_value: 0, + description: "Specifies control mode of the AGV wheel controller. 0: CHASSIS, 1: GIMBAL, 2: CHASSIS_FOLLOW_GIMBAL.", + } + yaw_gimbal_joint: + { + type: string, + default_value: "", + description: "Specifies the joint of the yaw gimbal.", + validation: { not_empty<>: null }, + } + follow_pid_target: + { + type: double, + default_value: 0.0, + description: "Specifies target of the follow PID controller. Setting 0.0 meaning the car is always moving towards the front. Usually 0.7853981634(45 degree) gives the maximum speed.", + } + reference_timeout: + { type: double, default_value: 0.1, description: "Specifies timeout for the reference." } diff --git a/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp b/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp index 17af2a6..f8cafab 100644 --- a/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp +++ b/decomposition/meta_chassis_controller/src/omni_chassis_controller.cpp @@ -1,6 +1,7 @@ #include "meta_chassis_controller/omni_chassis_controller.hpp" #include +#include #include #include #include @@ -11,36 +12,27 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/logging.hpp" -namespace { // utility - -static constexpr rmw_qos_profile_t rmw_qos_profile_services_hist_keep_all = { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1, // message queue depth - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, - false}; +constexpr double NaN = std::numeric_limits::quiet_NaN(); using ControllerReferenceMsg = meta_chassis_controller::OmniChassisController::ControllerReferenceMsg; -// called from RT control loop void reset_controller_reference_msg( const std::shared_ptr &msg, const std::shared_ptr &node) { msg->header.stamp = node->now(); - msg->twist.angular.x = std::numeric_limits::quiet_NaN(); - msg->twist.angular.y = std::numeric_limits::quiet_NaN(); - msg->twist.angular.z = std::numeric_limits::quiet_NaN(); - msg->twist.linear.x = std::numeric_limits::quiet_NaN(); - msg->twist.linear.y = std::numeric_limits::quiet_NaN(); - msg->twist.linear.z = std::numeric_limits::quiet_NaN(); + msg->twist.angular.x = NaN; + msg->twist.angular.y = NaN; + msg->twist.angular.z = NaN; + msg->twist.linear.x = NaN; + msg->twist.linear.y = NaN; + msg->twist.linear.z = NaN; } -} // namespace +void reset_chassis_cmd_msg(const std::shared_ptr &msg) { + msg->mode = behavior_interface::msg::Chassis::CHASSIS; + msg->max_power = NaN; +} namespace meta_chassis_controller { using hardware_interface::HW_IF_POSITION; @@ -50,15 +42,12 @@ OmniChassisController::OmniChassisController() : controller_interface::ChainableControllerInterface() {} controller_interface::CallbackReturn OmniChassisController::on_init() { - // control_mode_.initRT(control_mode_type::CHASSIS); try { param_listener_ = - std::make_shared( - get_node()); + std::make_shared(get_node()); } catch (const std::exception &e) { - fprintf(stderr, - "Exception thrown during controller's init with message: %s \n", + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", e.what()); return controller_interface::CallbackReturn::ERROR; } @@ -66,8 +55,8 @@ controller_interface::CallbackReturn OmniChassisController::on_init() { return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn OmniChassisController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +OmniChassisController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); omni_wheel_kinematics_ = std::make_unique( @@ -82,23 +71,38 @@ controller_interface::CallbackReturn OmniChassisController::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - ref_subscriber_ = - get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&OmniChassisController::reference_callback, this, - std::placeholders::_1)); + twist_sub_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&OmniChassisController::reference_callback, this, + std::placeholders::_1)); + + subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.reliable(); + subscribers_qos.transient_local(); + + chassis_sub_ = get_node()->create_subscription( + "/chassis_cmd", subscribers_qos, + [this](behavior_interface::msg::Chassis::UniquePtr msg) { + chassis_buf_.writeFromNonRT(std::move(msg)); + }); + + { + auto msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + ref_buf_.writeFromNonRT(msg); + } - std::shared_ptr msg = - std::make_shared(); - reset_controller_reference_msg(msg, get_node()); - input_ref_.writeFromNonRT(msg); + { + auto msg = std::make_shared(); + reset_chassis_cmd_msg(msg); + chassis_buf_.writeFromNonRT(msg); + } - follow_pid_ = std::make_shared( - get_node(), "follow_pid_gains", true); + follow_pid_ = + std::make_shared(get_node(), "follow_pid_gains", true); if (!follow_pid_->initPid()) { - RCLCPP_ERROR(get_node()->get_logger(), - "Failed to initialize chassis follow PID"); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize chassis follow PID"); return controller_interface::CallbackReturn::FAILURE; } @@ -106,8 +110,7 @@ controller_interface::CallbackReturn OmniChassisController::on_configure( // State publisher s_publisher_ = get_node()->create_publisher( "~/state", rclcpp::SystemDefaultsQoS()); - state_publisher_ = - std::make_unique(s_publisher_); + state_publisher_ = std::make_unique(s_publisher_); } catch (const std::exception &e) { fprintf(stderr, "Exception thrown during publisher creation at configure stage " @@ -143,45 +146,40 @@ OmniChassisController::state_interface_configuration() const { state_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - // Joint position state of yaw gimbal is required for GIMBAL or - // CHASSIS_FOLLOW_GIMBAL control mode - if (params_.control_mode == static_cast(control_mode_type::GIMBAL) || - params_.control_mode == - static_cast(control_mode_type::CHASSIS_FOLLOW_GIMBAL)) { - state_interfaces_config.names.reserve(1); - state_interfaces_config.names.push_back(params_.yaw_gimbal_joint + "/" + - HW_IF_POSITION); - } + // Joint position state of yaw gimbal is required + state_interfaces_config.names.reserve(1); + state_interfaces_config.names.push_back(params_.yaw_gimbal_joint + "/" + + HW_IF_POSITION); return state_interfaces_config; } void OmniChassisController::reference_callback( - const std::shared_ptr msg) { + ControllerReferenceMsgUnstamped::UniquePtr msg) { auto stamped_msg = std::make_shared(); stamped_msg->header.stamp = get_node()->now(); stamped_msg->twist = *msg; - input_ref_.writeFromNonRT(stamped_msg); + ref_buf_.writeFromNonRT(stamped_msg); } std::vector OmniChassisController::on_export_reference_interfaces() { - reference_interfaces_.resize(3, std::numeric_limits::quiet_NaN()); + reference_interfaces_.resize(3, NaN); std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/x/") + HW_IF_VELOCITY, - &reference_interfaces_[0])); + reference_interfaces.emplace_back(get_node()->get_name(), + std::string("linear/x/") + HW_IF_VELOCITY, + &reference_interfaces_[0]); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("linear/y/") + HW_IF_VELOCITY, - &reference_interfaces_[1])); + reference_interfaces.emplace_back(get_node()->get_name(), + std::string("linear/y/") + HW_IF_VELOCITY, + &reference_interfaces_[1]); - reference_interfaces.push_back(hardware_interface::CommandInterface( - get_node()->get_name(), std::string("angular/z/") + HW_IF_VELOCITY, - &reference_interfaces_[2])); + reference_interfaces.emplace_back(get_node()->get_name(), + std::string("angular/z/") + HW_IF_VELOCITY, + &reference_interfaces_[2]); return reference_interfaces; } @@ -191,112 +189,126 @@ bool OmniChassisController::on_set_chained_mode(bool chained_mode) { return true || chained_mode; } -controller_interface::CallbackReturn OmniChassisController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +OmniChassisController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command - reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + reset_controller_reference_msg(*(ref_buf_.readFromRT()), get_node()); + reset_chassis_cmd_msg(*(chassis_buf_.readFromRT())); return controller_interface::CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn OmniChassisController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +OmniChassisController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { for (size_t i = 0; i < command_interfaces_.size(); ++i) { - command_interfaces_[i].set_value( - std::numeric_limits::quiet_NaN()); + command_interfaces_[i].set_value(NaN); } return controller_interface::CallbackReturn::SUCCESS; } controller_interface::return_type OmniChassisController::update_reference_from_subscribers() { - auto current_ref = - *(input_ref_.readFromRT()); // A shared_ptr must be allocated - // immediately to prevent dangling - const auto command_age = get_node()->now() - current_ref->header.stamp; + auto cur_ref = *(ref_buf_.readFromRT()); - // RCLCPP_INFO(get_node()->get_logger(), "Current reference age: %f", - // command_age.seconds()); - - if (command_age <= ref_timeout_ || + if (const auto command_age = get_node()->now() - cur_ref->header.stamp; + command_age <= ref_timeout_ || ref_timeout_ == rclcpp::Duration::from_seconds(0)) { - if (!std::isnan(current_ref->twist.linear.x) && - !std::isnan(current_ref->twist.linear.y) && - !std::isnan(current_ref->twist.angular.z)) { - reference_interfaces_[0] = current_ref->twist.linear.x; - reference_interfaces_[1] = current_ref->twist.linear.y; - reference_interfaces_[2] = current_ref->twist.angular.z; + if (!std::isnan(cur_ref->twist.linear.x) && + !std::isnan(cur_ref->twist.linear.y) && + !std::isnan(cur_ref->twist.angular.z)) { + reference_interfaces_[0] = cur_ref->twist.linear.x; + reference_interfaces_[1] = cur_ref->twist.linear.y; + reference_interfaces_[2] = cur_ref->twist.angular.z; } } else { - if (!std::isnan(current_ref->twist.linear.x) && - !std::isnan(current_ref->twist.linear.y) && - !std::isnan(current_ref->twist.angular.z)) { - reference_interfaces_[0] = 0.0; - reference_interfaces_[1] = 0.0; - reference_interfaces_[2] = 0.0; - current_ref->twist.linear.x = - std::numeric_limits::quiet_NaN(); - current_ref->twist.linear.y = - std::numeric_limits::quiet_NaN(); - current_ref->twist.angular.z = - std::numeric_limits::quiet_NaN(); - } + reference_interfaces_[0] = 0.0; + reference_interfaces_[1] = 0.0; + reference_interfaces_[2] = 0.0; } return controller_interface::return_type::OK; } controller_interface::return_type -OmniChassisController::update_and_write_commands( - const rclcpp::Time &time, const rclcpp::Duration &period) { +OmniChassisController::update_and_write_commands(const rclcpp::Time &time, + const rclcpp::Duration &period) { if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); } - if (!std::isnan(reference_interfaces_[0]) && - !std::isnan(reference_interfaces_[1]) && + auto cur_chassis = *(chassis_buf_.readFromRT()); + + if (!std::isnan(reference_interfaces_[0]) && !std::isnan(reference_interfaces_[1]) && !std::isnan(reference_interfaces_[2])) { Eigen::Vector3d twist; twist << reference_interfaces_[0], reference_interfaces_[1], reference_interfaces_[2]; - if (params_.control_mode == - static_cast(control_mode_type::CHASSIS_FOLLOW_GIMBAL)) { - double yaw_gimbal_joint_pos = state_interfaces_[0].get_value(); - double error = -angles::shortest_angular_distance( - yaw_gimbal_joint_pos, params_.follow_pid_target); - twist[2] = follow_pid_->computeCommand(error, period); - if (state_publisher_ && state_publisher_->trylock()) { - state_publisher_->msg_.header.stamp = time; - state_publisher_->msg_.set_point = 0.0; - state_publisher_->msg_.process_value = - state_interfaces_[0].get_value(); - state_publisher_->msg_.error = error; - state_publisher_->msg_.time_step = period.seconds(); - state_publisher_->msg_.command = twist[2]; - - state_publisher_->unlockAndPublish(); - } - } - if (params_.control_mode == - static_cast(control_mode_type::GIMBAL) || - params_.control_mode == - static_cast(control_mode_type::CHASSIS_FOLLOW_GIMBAL)) { - Eigen::MatrixXd rotation_mat(3, 3); - double yaw_gimbal_joint_pos = state_interfaces_[0].get_value(); - rotation_mat << cos(yaw_gimbal_joint_pos), - -sin(yaw_gimbal_joint_pos), 0, sin(yaw_gimbal_joint_pos), - cos(yaw_gimbal_joint_pos), 0, 0, 0, 1; - twist = rotation_mat * twist; + + switch (cur_chassis->mode) { + case behavior_interface::msg::Chassis::CHASSIS: + break; + case behavior_interface::msg::Chassis::GIMBAL: + gimbal_mode(twist); + break; + case behavior_interface::msg::Chassis::CHASSIS_FOLLOW: + chassis_follow_mode(twist, time, period); + break; + case behavior_interface::msg::Chassis::GYRO: + gyro_mode(twist); + break; + default: + RCLCPP_ERROR(get_node()->get_logger(), "Unkown chassis mode: %d", + cur_chassis->mode); + return controller_interface::return_type::ERROR; } - auto wheel_vels = omni_wheel_kinematics_->inverse(twist); + + Eigen::VectorXd wheel_vels = omni_wheel_kinematics_->inverse(twist); for (size_t i = 0; i < command_interfaces_.size(); i++) { - command_interfaces_[i].set_value( - wheel_vels[static_cast(i)]); + command_interfaces_[i].set_value(wheel_vels[static_cast(i)]); } } return controller_interface::return_type::OK; } +void OmniChassisController::chassis_follow_mode(Eigen::Vector3d &twist, + const rclcpp::Time &time, + const rclcpp::Duration &period) { + double yaw_gimbal_joint_pos = state_interfaces_[0].get_value(); + double error = -angles::shortest_angular_distance(yaw_gimbal_joint_pos, + params_.follow_pid_target); + twist[2] = follow_pid_->computeCommand(error, period); + + transform_twist_to_gimbal(twist, yaw_gimbal_joint_pos); + + if (state_publisher_ && state_publisher_->trylock()) { + state_publisher_->msg_.header.stamp = time; + state_publisher_->msg_.set_point = 0.0; + state_publisher_->msg_.process_value = state_interfaces_[0].get_value(); + state_publisher_->msg_.error = error; + state_publisher_->msg_.time_step = period.seconds(); + state_publisher_->msg_.command = twist[2]; + + state_publisher_->unlockAndPublish(); + } +} + +void OmniChassisController::gimbal_mode(Eigen::Vector3d &twist) { + transform_twist_to_gimbal(twist, state_interfaces_[0].get_value()); +} + +void OmniChassisController::gyro_mode(Eigen::Vector3d &twist) { + twist[2] = 4.0; + transform_twist_to_gimbal(twist, state_interfaces_[0].get_value()); +} + +void OmniChassisController::transform_twist_to_gimbal( + Eigen::Vector3d &twist, const double &yaw_gimbal_joint_pos) const { + Eigen::MatrixXd rotation_mat(3, 3); + rotation_mat << cos(yaw_gimbal_joint_pos), -sin(yaw_gimbal_joint_pos), 0, + sin(yaw_gimbal_joint_pos), cos(yaw_gimbal_joint_pos), 0, 0, 0, 1; + twist = rotation_mat * twist; +} + } // namespace meta_chassis_controller #include "pluginlib/class_list_macros.hpp" diff --git a/interfaces/behavior_interface/CMakeLists.txt b/interfaces/behavior_interface/CMakeLists.txt index 2037849..8f766ce 100644 --- a/interfaces/behavior_interface/CMakeLists.txt +++ b/interfaces/behavior_interface/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Aim.msg" + "msg/Chassis.msg" "msg/Move.msg" "msg/Shoot.msg" "msg/EndVel.msg" diff --git a/interfaces/behavior_interface/msg/Chassis.msg b/interfaces/behavior_interface/msg/Chassis.msg new file mode 100644 index 0000000..8e1df56 --- /dev/null +++ b/interfaces/behavior_interface/msg/Chassis.msg @@ -0,0 +1,9 @@ +# This message won't change too frequently and is therefore best sent with transient local setting + +uint8 CHASSIS=0 +uint8 GIMBAL=1 +uint8 CHASSIS_FOLLOW=2 +uint8 GYRO=3 + +float64 max_power +uint8 mode \ No newline at end of file From 2a5a9416fd5e74a68afbebf735c340d564f1edcd Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 16 Aug 2024 16:22:37 +0800 Subject: [PATCH 15/36] create dm interface header --- .../meta_hardware/dm_motor_interface.hpp | 94 ++++++++ .../meta_hardware/src/dm_motor_interface.cpp | 219 ++++++++++++------ 2 files changed, 237 insertions(+), 76 deletions(-) create mode 100644 execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp diff --git a/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp new file mode 100644 index 0000000..162f2db --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp @@ -0,0 +1,94 @@ +#ifndef META_HARDWARE__DM_MOTOR_INTERFACE_HPP_ +#define META_HARDWARE__DM_MOTOR_INTERFACE_HPP_ + +#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 "meta_hardware/motor_network/mi_motor_network.hpp" +#include "meta_hardware/visibility_control.h" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace meta_hardware { +constexpr double NaN = std::numeric_limits::quiet_NaN(); + +class MetaRobotDmMotorNetwork : public hardware_interface::SystemInterface { + public: + ~MetaRobotDmMotorNetwork() override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_init(const hardware_interface::HardwareInfo &info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector + export_state_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector + export_command_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type + read(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type + write(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + enum class DmMotorMode { + MIT, // Dynamic mode with all three commands + POSITION, // Position mode + VELOCITY, // Velocity mode + }; + + private: + class JointInterfaceData { + public: + double command_position = NaN; + double command_velocity = NaN; + double command_effort = NaN; + double state_position = NaN; + double state_velocity = NaN; + double state_effort = NaN; + }; + std::vector joint_interface_data_; + + class JointMotorInfo { + public: + std::string name; + double mechanical_reduction; + double offset; + bool command_pos; + bool command_vel; + bool command_eff; + DmMotorMode mode; + }; + std::vector + joint_motor_info_; // local cache of joint motor info + + MetaRobotDmMotorNetwork::DmMotorMode check_motor_mode(const std::string &mode, bool command_pos, + bool command_vel, bool command_eff); + + std::unique_ptr mi_motor_network_; +}; + +} // namespace meta_hardware + +#endif // META_HARDWARE__DM_MOTOR_INTERFACE_HPP_ diff --git a/execution/meta_hardware/src/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp index d6ffe45..9526c3d 100644 --- a/execution/meta_hardware/src/dm_motor_interface.cpp +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -1,12 +1,14 @@ #include #include #include +#include #include #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "meta_hardware/dji_motor_interface.hpp" -#include "meta_hardware/motor_network/dji_motor_network.hpp" +#include "meta_hardware/mi_motor_interface.hpp" +#include "meta_hardware/motor_network/mi_motor_network.hpp" #include "rclcpp/rclcpp.hpp" namespace meta_hardware { @@ -14,56 +16,84 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; -MetaRobotDjiMotorNetwork::~MetaRobotDjiMotorNetwork() { - on_deactivate(rclcpp_lifecycle::State()); -} +MetaRobotMiMotorNetwork::~MetaRobotMiMotorNetwork() = default; -hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_init( - const hardware_interface::HardwareInfo &info) { - if (hardware_interface::SystemInterface::on_init(info) != - CallbackReturn::SUCCESS) { +hardware_interface::CallbackReturn +MetaRobotMiMotorNetwork::on_init(const hardware_interface::HardwareInfo &info) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } joint_interface_data_.resize(info_.joints.size()); - joint_motors_info_.resize(info_.joints.size()); + joint_motor_info_.resize(info_.joints.size()); return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { +MetaRobotMiMotorNetwork::MiMotorMode +MetaRobotMiMotorNetwork::check_motor_mode(const std::string &mode, bool command_pos, + bool command_vel, bool command_eff) { + using enum meta_hardware::MetaRobotMiMotorNetwork::MiMotorMode; + if (mode == "dynamic") { + if (command_pos && command_vel && command_eff) { + return DYNAMIC; + } else if (command_pos && !command_vel && !command_eff) { + return DYNAMIC_POS; + } else if (!command_pos && command_vel && !command_eff) { + return DYNAMIC_VEL; + } else if (!command_pos && !command_vel && command_eff) { + return DYNAMIC_EFF; + } else if (command_pos && !command_vel && command_eff) { + return DYNAMIC_POS_FF; + } else if (!command_pos && command_vel && command_eff) { + return DYNAMIC_VEL_FF; + } else { + throw std::runtime_error("Invalid dynamic mode"); + } + } else if (mode == "position") { + return POSITION; + } else if (mode == "velocity") { + return VELOCITY; + } else { + throw std::runtime_error("Unknown control mode: " + mode); + } +} - std::vector> motor_params; +hardware_interface::CallbackReturn MetaRobotMiMotorNetwork::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + std::vector> joint_params; // Add the motors to the motor networks for (size_t i = 0; i < info_.joints.size(); ++i) { - const auto &joint_params = info_.joints[i].parameters; - motor_params.push_back(joint_params); - joint_motors_info_[i].joint_name = info_.joints[i].name; - joint_motors_info_[i].mechanical_reduction = - std::stod(joint_params.at("mechanical_reduction")); - joint_motors_info_[i].offset = std::stod(joint_params.at("offset")); + const auto &joint = info_.joints[i]; + const auto &joint_param = joint.parameters; + joint_motor_info_[i].name = joint.name; + joint_motor_info_[i].mechanical_reduction = + std::stod(joint_param.at("mechanical_reduction")); + joint_motor_info_[i].offset = std::stod(joint_param.at("offset")); + joint_motor_info_[i].mode = check_motor_mode( + joint_param.at("control_mode"), joint_motor_info_[i].command_pos, + joint_motor_info_[i].command_vel, joint_motor_info_[i].command_eff); + joint_params.emplace_back(joint_param); } - std::string can_interface = - info_.hardware_parameters.at("can_network_name"); - dji_motor_network_ = - std::make_unique(can_interface, motor_params); + std::string can_network_name = info_.hardware_parameters.at("can_network_name"); + mi_motor_network_ = + std::make_unique(can_network_name, 0x00, joint_params); return CallbackReturn::SUCCESS; } std::vector -MetaRobotDjiMotorNetwork::export_state_interfaces() { +MetaRobotMiMotorNetwork::export_state_interfaces() { std::vector state_interfaces; // Helper function to check if the interface exists auto contains_interface = [](const std::vector &interfaces, const std::string &interface_name) { - return std::find_if( - interfaces.begin(), interfaces.end(), + return std::ranges::find_if( + interfaces, [&interface_name]( const hardware_interface::InterfaceInfo &interface) { return interface.name == interface_name; @@ -73,19 +103,16 @@ MetaRobotDjiMotorNetwork::export_state_interfaces() { for (size_t i = 0; i < info_.joints.size(); ++i) { const auto &joint_state_interfaces = info_.joints[i].state_interfaces; if (contains_interface(joint_state_interfaces, "position")) { - state_interfaces.emplace_back( - info_.joints[i].name, HW_IF_POSITION, - &joint_interface_data_[i].state_position); + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_POSITION, + &joint_interface_data_[i].state_position); } if (contains_interface(joint_state_interfaces, "velocity")) { - state_interfaces.emplace_back( - info_.joints[i].name, HW_IF_VELOCITY, - &joint_interface_data_[i].state_velocity); + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_VELOCITY, + &joint_interface_data_[i].state_velocity); } if (contains_interface(joint_state_interfaces, "effort")) { - state_interfaces.emplace_back( - info_.joints[i].name, HW_IF_EFFORT, - &joint_interface_data_[i].state_effort); + state_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, + &joint_interface_data_[i].state_effort); } } @@ -93,15 +120,15 @@ MetaRobotDjiMotorNetwork::export_state_interfaces() { } std::vector -MetaRobotDjiMotorNetwork::export_command_interfaces() { +MetaRobotMiMotorNetwork::export_command_interfaces() { std::vector command_interfaces; // Helper function to check if the interface exists auto contains_interface = [](const std::vector &interfaces, const std::string &interface_name) { - return std::find_if( - interfaces.begin(), interfaces.end(), + return std::ranges::find_if( + interfaces, [&interface_name]( const hardware_interface::InterfaceInfo &interface) { return interface.name == interface_name; @@ -109,44 +136,51 @@ MetaRobotDjiMotorNetwork::export_command_interfaces() { }; for (size_t i = 0; i < info_.joints.size(); ++i) { - const auto &joint_command_interfaces = - info_.joints[i].command_interfaces; + const auto &joint_command_interfaces = info_.joints[i].command_interfaces; + if (contains_interface(joint_command_interfaces, "position")) { + command_interfaces.emplace_back(info_.joints[i].name, HW_IF_POSITION, + &joint_interface_data_[i].command_position); + joint_motor_info_[i].command_pos = true; + } + if (contains_interface(joint_command_interfaces, "velocity")) { + command_interfaces.emplace_back(info_.joints[i].name, HW_IF_VELOCITY, + &joint_interface_data_[i].command_velocity); + joint_motor_info_[i].command_vel = true; + } if (contains_interface(joint_command_interfaces, "effort")) { - command_interfaces.emplace_back( - info_.joints[i].name, HW_IF_EFFORT, - &joint_interface_data_[i].command_effort); + command_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, + &joint_interface_data_[i].command_effort); + joint_motor_info_[i].command_eff = true; } } return command_interfaces; } -hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { +hardware_interface::CallbackReturn +MetaRobotMiMotorNetwork::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn MetaRobotDjiMotorNetwork::on_deactivate( +hardware_interface::CallbackReturn MetaRobotMiMotorNetwork::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } hardware_interface::return_type -MetaRobotDjiMotorNetwork::read(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +MetaRobotMiMotorNetwork::read(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { - for (size_t i = 0; i < joint_motors_info_.size(); ++i) { - auto [position, velocity, effort] = dji_motor_network_->read(i); + for (size_t i = 0; i < joint_motor_info_.size(); ++i) { + auto [position, velocity, effort] = mi_motor_network_->read(i); - double reduction = joint_motors_info_[i].mechanical_reduction; - double offset = joint_motors_info_[i].offset; + double reduction = joint_motor_info_[i].mechanical_reduction; + double offset = joint_motor_info_[i].offset; position = position / reduction + offset; velocity /= reduction; - if (reduction < 0) { - effort = -effort; - } + effort *= reduction; joint_interface_data_[i].state_position = position; joint_interface_data_[i].state_velocity = velocity; @@ -157,32 +191,65 @@ MetaRobotDjiMotorNetwork::read(const rclcpp::Time & /*time*/, } hardware_interface::return_type -MetaRobotDjiMotorNetwork::write(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +MetaRobotMiMotorNetwork::write(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { - for (size_t i = 0; i < joint_motors_info_.size(); ++i) { + for (size_t i = 0; i < joint_motor_info_.size(); ++i) { + double position = joint_interface_data_[i].command_position; + double velocity = joint_interface_data_[i].command_velocity; double effort = joint_interface_data_[i].command_effort; - // Check if the command is valid - // If a command interface exists, the command must not be NaN - if (std::isnan(effort)) { - continue; + double reduction = joint_motor_info_[i].mechanical_reduction; + double offset = joint_motor_info_[i].offset; + position = (position - offset) * reduction; + velocity *= reduction; + effort /= reduction; + + using enum MetaRobotMiMotorNetwork::MiMotorMode; + switch (joint_motor_info_[i].mode) { + case DYNAMIC: + if (std::isnan(position) || std::isnan(velocity) || std::isnan(effort)) + continue; + mi_motor_network_->write_dyn(i, position, velocity, effort); + break; + case DYNAMIC_POS: + if (std::isnan(position)) + continue; + mi_motor_network_->write_dyn(i, position, 0.0, 0.0); + break; + case DYNAMIC_VEL: + if (std::isnan(velocity)) + continue; + mi_motor_network_->write_dyn(i, 0.0, velocity, 0.0); + break; + case DYNAMIC_EFF: + if (std::isnan(effort)) + continue; + mi_motor_network_->write_dyn(i, 0.0, 0.0, effort); + break; + case DYNAMIC_POS_FF: + if (std::isnan(position) || std::isnan(effort)) + continue; + mi_motor_network_->write_dyn(i, position, 0.0, effort); + break; + case DYNAMIC_VEL_FF: + if (std::isnan(velocity) || std::isnan(effort)) + continue; + mi_motor_network_->write_dyn(i, 0.0, velocity, effort); + break; + case POSITION: + if (std::isnan(position)) + continue; + mi_motor_network_->write_pos(i, position); + break; + case VELOCITY: + if (std::isnan(velocity)) + continue; + mi_motor_network_->write_vel(i, velocity); + break; } - - // Even though DJI motors receive proportional effort commands, - // the mechanical reduction can be negative, which means that the - // motor direction is inverted. In this case, the effort must be negated. - if (joint_motors_info_[i].mechanical_reduction < 0) { - effort = -effort; - } - - // Write the command to the motor network - dji_motor_network_->write(i, effort); } - // Some motor network implementations require a separate tx() call - dji_motor_network_->tx(); - return hardware_interface::return_type::OK; } @@ -190,5 +257,5 @@ MetaRobotDjiMotorNetwork::write(const rclcpp::Time & /*time*/, #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(meta_hardware::MetaRobotDjiMotorNetwork, +PLUGINLIB_EXPORT_CLASS(meta_hardware::MetaRobotMiMotorNetwork, hardware_interface::SystemInterface) From 1bddd3620d62ba5a9278ed435ac38fe7407ffa7c Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 16 Aug 2024 17:16:23 +0800 Subject: [PATCH 16/36] dm interface first version --- .../meta_hardware/dm_motor_interface.hpp | 4 +- .../motor_driver/dm_motor_driver.hpp | 3 +- .../meta_hardware/src/dm_motor_interface.cpp | 94 ++++++------------- .../src/motor_driver/dm_motor_driver.cpp | 23 +++-- 4 files changed, 43 insertions(+), 81 deletions(-) diff --git a/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp index 162f2db..497f102 100644 --- a/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp +++ b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp @@ -9,7 +9,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "meta_hardware/motor_network/mi_motor_network.hpp" +#include "meta_hardware/motor_network/dm_motor_network.hpp" #include "meta_hardware/visibility_control.h" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -86,7 +86,7 @@ class MetaRobotDmMotorNetwork : public hardware_interface::SystemInterface { MetaRobotDmMotorNetwork::DmMotorMode check_motor_mode(const std::string &mode, bool command_pos, bool command_vel, bool command_eff); - std::unique_ptr mi_motor_network_; + std::unique_ptr dm_motor_network_; }; } // namespace meta_hardware diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp index eed15e7..fd80b52 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -9,7 +9,6 @@ #include #include -#include "CanMessage.hpp" #include #include "angles/angles.h" @@ -43,7 +42,7 @@ class DmMotor { can_frame motor_pos_frame(double position, double velocity) const; can_frame motor_vel_frame(double velocity) const; - void set_motor_feedback(const sockcanpp::CanMessage &can_msg); + void set_motor_feedback(const can_frame &can_msg); std::tuple get_motor_feedback() const; RunMode get_run_mode() const { return run_mode_; } diff --git a/execution/meta_hardware/src/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp index 9526c3d..e600a99 100644 --- a/execution/meta_hardware/src/dm_motor_interface.cpp +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -7,8 +7,8 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "meta_hardware/mi_motor_interface.hpp" -#include "meta_hardware/motor_network/mi_motor_network.hpp" +#include "meta_hardware/dm_motor_interface.hpp" +#include "meta_hardware/motor_network/dm_motor_network.hpp" #include "rclcpp/rclcpp.hpp" namespace meta_hardware { @@ -16,10 +16,10 @@ using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; -MetaRobotMiMotorNetwork::~MetaRobotMiMotorNetwork() = default; +MetaRobotDmMotorNetwork::~MetaRobotDmMotorNetwork() = default; hardware_interface::CallbackReturn -MetaRobotMiMotorNetwork::on_init(const hardware_interface::HardwareInfo &info) { +MetaRobotDmMotorNetwork::on_init(const hardware_interface::HardwareInfo &info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -30,26 +30,12 @@ MetaRobotMiMotorNetwork::on_init(const hardware_interface::HardwareInfo &info) { return CallbackReturn::SUCCESS; } -MetaRobotMiMotorNetwork::MiMotorMode -MetaRobotMiMotorNetwork::check_motor_mode(const std::string &mode, bool command_pos, +MetaRobotDmMotorNetwork::DmMotorMode +MetaRobotDmMotorNetwork::check_motor_mode(const std::string &mode, bool command_pos, bool command_vel, bool command_eff) { - using enum meta_hardware::MetaRobotMiMotorNetwork::MiMotorMode; - if (mode == "dynamic") { - if (command_pos && command_vel && command_eff) { - return DYNAMIC; - } else if (command_pos && !command_vel && !command_eff) { - return DYNAMIC_POS; - } else if (!command_pos && command_vel && !command_eff) { - return DYNAMIC_VEL; - } else if (!command_pos && !command_vel && command_eff) { - return DYNAMIC_EFF; - } else if (command_pos && !command_vel && command_eff) { - return DYNAMIC_POS_FF; - } else if (!command_pos && command_vel && command_eff) { - return DYNAMIC_VEL_FF; - } else { - throw std::runtime_error("Invalid dynamic mode"); - } + using enum meta_hardware::MetaRobotDmMotorNetwork::DmMotorMode; + if (mode == "mit") { + return MIT; } else if (mode == "position") { return POSITION; } else if (mode == "velocity") { @@ -59,7 +45,7 @@ MetaRobotMiMotorNetwork::check_motor_mode(const std::string &mode, bool command_ } } -hardware_interface::CallbackReturn MetaRobotMiMotorNetwork::on_configure( +hardware_interface::CallbackReturn MetaRobotDmMotorNetwork::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { std::vector> joint_params; @@ -78,14 +64,17 @@ hardware_interface::CallbackReturn MetaRobotMiMotorNetwork::on_configure( } std::string can_network_name = info_.hardware_parameters.at("can_network_name"); - mi_motor_network_ = - std::make_unique(can_network_name, 0x00, joint_params); + // TODO: Add master ID to the hardware parameters, master ID is not 0x00 + + int master_id = std::stoi(info_.hardware_parameters.at("master_id")); + dm_motor_network_ = + std::make_unique(can_network_name, master_id, joint_params); return CallbackReturn::SUCCESS; } std::vector -MetaRobotMiMotorNetwork::export_state_interfaces() { +MetaRobotDmMotorNetwork::export_state_interfaces() { std::vector state_interfaces; // Helper function to check if the interface exists @@ -120,7 +109,7 @@ MetaRobotMiMotorNetwork::export_state_interfaces() { } std::vector -MetaRobotMiMotorNetwork::export_command_interfaces() { +MetaRobotDmMotorNetwork::export_command_interfaces() { std::vector command_interfaces; // Helper function to check if the interface exists @@ -158,23 +147,23 @@ MetaRobotMiMotorNetwork::export_command_interfaces() { } hardware_interface::CallbackReturn -MetaRobotMiMotorNetwork::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { +MetaRobotDmMotorNetwork::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn MetaRobotMiMotorNetwork::on_deactivate( +hardware_interface::CallbackReturn MetaRobotDmMotorNetwork::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } hardware_interface::return_type -MetaRobotMiMotorNetwork::read(const rclcpp::Time & /*time*/, +MetaRobotDmMotorNetwork::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { for (size_t i = 0; i < joint_motor_info_.size(); ++i) { - auto [position, velocity, effort] = mi_motor_network_->read(i); + auto [position, velocity, effort] = dm_motor_network_->read(i); double reduction = joint_motor_info_[i].mechanical_reduction; double offset = joint_motor_info_[i].offset; @@ -191,7 +180,7 @@ MetaRobotMiMotorNetwork::read(const rclcpp::Time & /*time*/, } hardware_interface::return_type -MetaRobotMiMotorNetwork::write(const rclcpp::Time & /*time*/, +MetaRobotDmMotorNetwork::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { for (size_t i = 0; i < joint_motor_info_.size(); ++i) { @@ -205,47 +194,22 @@ MetaRobotMiMotorNetwork::write(const rclcpp::Time & /*time*/, velocity *= reduction; effort /= reduction; - using enum MetaRobotMiMotorNetwork::MiMotorMode; + using enum MetaRobotDmMotorNetwork::DmMotorMode; switch (joint_motor_info_[i].mode) { - case DYNAMIC: + case MIT: if (std::isnan(position) || std::isnan(velocity) || std::isnan(effort)) continue; - mi_motor_network_->write_dyn(i, position, velocity, effort); - break; - case DYNAMIC_POS: - if (std::isnan(position)) - continue; - mi_motor_network_->write_dyn(i, position, 0.0, 0.0); - break; - case DYNAMIC_VEL: - if (std::isnan(velocity)) - continue; - mi_motor_network_->write_dyn(i, 0.0, velocity, 0.0); - break; - case DYNAMIC_EFF: - if (std::isnan(effort)) - continue; - mi_motor_network_->write_dyn(i, 0.0, 0.0, effort); - break; - case DYNAMIC_POS_FF: - if (std::isnan(position) || std::isnan(effort)) - continue; - mi_motor_network_->write_dyn(i, position, 0.0, effort); - break; - case DYNAMIC_VEL_FF: - if (std::isnan(velocity) || std::isnan(effort)) - continue; - mi_motor_network_->write_dyn(i, 0.0, velocity, effort); + dm_motor_network_->write_mit(i, position, velocity, effort); break; case POSITION: - if (std::isnan(position)) + if (std::isnan(position) || std::isnan(velocity)) continue; - mi_motor_network_->write_pos(i, position); + dm_motor_network_->write_pos(i, position, velocity); break; case VELOCITY: if (std::isnan(velocity)) continue; - mi_motor_network_->write_vel(i, velocity); + dm_motor_network_->write_vel(i, velocity); break; } } @@ -257,5 +221,5 @@ MetaRobotMiMotorNetwork::write(const rclcpp::Time & /*time*/, #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(meta_hardware::MetaRobotMiMotorNetwork, +PLUGINLIB_EXPORT_CLASS(meta_hardware::MetaRobotDmMotorNetwork, hardware_interface::SystemInterface) diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index c07a3b3..d03061f 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -7,7 +7,6 @@ #include #include -#include "CanMessage.hpp" #include "angles/angles.h" #include "meta_hardware/motor_driver/dm_motor_driver.hpp" @@ -147,35 +146,35 @@ can_frame DmMotor::motor_vel_frame(double velocity) const{ return frame; } -void DmMotor::set_motor_feedback(const sockcanpp::CanMessage &can_msg){ +void DmMotor::set_motor_feedback(const can_frame &can_msg){ auto error_code = static_cast - (static_cast(can_msg.getRawFrame().data[0]) >> 4); + (static_cast(can_msg.data[0]) >> 4); auto id = static_cast - ((static_cast(can_msg.getRawFrame().data[0]) & 0x0F)); + ((static_cast(can_msg.data[0]) & 0x0F)); auto position_raw = static_cast - (((static_cast(can_msg.getRawFrame().data[1]) & 0xFF) << 8) | - (static_cast(can_msg.getRawFrame().data[2]) & 0xFF) + (((static_cast(can_msg.data[1]) & 0xFF) << 8) | + (static_cast(can_msg.data[2]) & 0xFF) ); auto velocity_raw = static_cast - (((static_cast(can_msg.getRawFrame().data[3]) & 0xFF) << 8) | - ((static_cast(can_msg.getRawFrame().data[4]) & 0xC0) >> 12) + (((static_cast(can_msg.data[3]) & 0xFF) << 8) | + ((static_cast(can_msg.data[4]) & 0xC0) >> 12) ); auto effort_raw = static_cast - (((static_cast(can_msg.getRawFrame().data[4]) & 0x3F) << 4) | - (static_cast(can_msg.getRawFrame().data[5]) & 0xFF) + (((static_cast(can_msg.data[4]) & 0x3F) << 4) | + (static_cast(can_msg.data[5]) & 0xFF) ); auto temperature_mos = static_cast - (static_cast(can_msg.getRawFrame().data[6])); + (static_cast(can_msg.data[6])); auto temperature_rotor = static_cast - (static_cast(can_msg.getRawFrame().data[7])); + (static_cast(can_msg.data[7])); double position = static_cast(position_raw) / ((1 << 16) - 1) * 2 * max_pos_ - max_pos_; From 279985bc33a0e1f8e26eb6f150d6ddc4f06170ca Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 16 Aug 2024 18:01:55 +0800 Subject: [PATCH 17/36] dm motor interface can be compiled --- execution/meta_hardware/CMakeLists.txt | 3 +++ execution/meta_hardware/src/dm_motor_interface.cpp | 2 -- execution/meta_hardware/src/motor_network/dm_motor_network.cpp | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/execution/meta_hardware/CMakeLists.txt b/execution/meta_hardware/CMakeLists.txt index 3296ae7..ecda4c5 100644 --- a/execution/meta_hardware/CMakeLists.txt +++ b/execution/meta_hardware/CMakeLists.txt @@ -24,6 +24,9 @@ add_library( src/mi_motor_interface.cpp src/motor_network/mi_motor_network.cpp src/motor_driver/mi_motor_driver.cpp + src/dm_motor_interface.cpp + src/motor_network/dm_motor_network.cpp + src/motor_driver/dm_motor_driver.cpp src/brt_encoder_interface.cpp src/motor_network/brt_encoder_network.cpp src/motor_driver/brt_encoder_driver.cpp diff --git a/execution/meta_hardware/src/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp index e600a99..bafd883 100644 --- a/execution/meta_hardware/src/dm_motor_interface.cpp +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -64,8 +64,6 @@ hardware_interface::CallbackReturn MetaRobotDmMotorNetwork::on_configure( } std::string can_network_name = info_.hardware_parameters.at("can_network_name"); - // TODO: Add master ID to the hardware parameters, master ID is not 0x00 - int master_id = std::stoi(info_.hardware_parameters.at("master_id")); dm_motor_network_ = std::make_unique(can_network_name, master_id, joint_params); diff --git a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp index 45ad4d2..f80c977 100644 --- a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -25,7 +25,7 @@ DmMotorNetwork::DmMotorNetwork(const std::string &can_network_name, uint8_t mast std::string motor_mode = joint_param.at("motor_mode"); auto dm_motor = - std::make_shared(joint_params, master_id); + std::make_shared(joint_param, master_id); dm_motors_.emplace_back(dm_motor); motor_id2motor_[dm_motor_id] = dm_motor; From 3d4b420debc4286b66f1c37dbe58c4b2bbe3a871 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Mon, 19 Aug 2024 19:48:03 +0800 Subject: [PATCH 18/36] launch file for dm motor test --- .../urdf/playground/dm_motor.xacro | 72 ++++++++++ .../meta_hardware/dm_motor_interface.hpp | 3 +- execution/meta_hardware/meta_hardware.xml | 7 + .../meta_hardware/src/dm_motor_interface.cpp | 6 +- .../src/motor_driver/dm_motor_driver.cpp | 2 +- .../src/motor_network/dm_motor_network.cpp | 2 +- meta_bringup/config/dm_motor.yaml | 34 +++++ meta_bringup/launch/dm_motor.launch.py | 130 ++++++++++++++++++ 8 files changed, 248 insertions(+), 8 deletions(-) create mode 100644 decomposition/metav_description/urdf/playground/dm_motor.xacro create mode 100644 meta_bringup/config/dm_motor.yaml create mode 100644 meta_bringup/launch/dm_motor.launch.py diff --git a/decomposition/metav_description/urdf/playground/dm_motor.xacro b/decomposition/metav_description/urdf/playground/dm_motor.xacro new file mode 100644 index 0000000..a6a1984 --- /dev/null +++ b/decomposition/metav_description/urdf/playground/dm_motor.xacro @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + meta_hardware/MetaRobotDmMotorNetwork + cagi n0 + 1 + + + + + + 4310 + 1 + -1.0 + 12.5 + 45 + 18 + 0.0 + mit + 0.0 + 1.0 + 0.0 + + + + + + + + + $(find meta_bringup)/config/dm_motor.yaml + + + + \ No newline at end of file diff --git a/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp index 497f102..a2b0e36 100644 --- a/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp +++ b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp @@ -83,8 +83,7 @@ class MetaRobotDmMotorNetwork : public hardware_interface::SystemInterface { std::vector joint_motor_info_; // local cache of joint motor info - MetaRobotDmMotorNetwork::DmMotorMode check_motor_mode(const std::string &mode, bool command_pos, - bool command_vel, bool command_eff); + MetaRobotDmMotorNetwork::DmMotorMode check_motor_mode(const std::string &mode); std::unique_ptr dm_motor_network_; }; diff --git a/execution/meta_hardware/meta_hardware.xml b/execution/meta_hardware/meta_hardware.xml index 8a36524..5f9b833 100644 --- a/execution/meta_hardware/meta_hardware.xml +++ b/execution/meta_hardware/meta_hardware.xml @@ -6,6 +6,13 @@ DJI motor network hardware interface. + + + DM motor network hardware interface. + + diff --git a/execution/meta_hardware/src/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp index bafd883..fe3d8bf 100644 --- a/execution/meta_hardware/src/dm_motor_interface.cpp +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -31,8 +31,7 @@ MetaRobotDmMotorNetwork::on_init(const hardware_interface::HardwareInfo &info) { } MetaRobotDmMotorNetwork::DmMotorMode -MetaRobotDmMotorNetwork::check_motor_mode(const std::string &mode, bool command_pos, - bool command_vel, bool command_eff) { +MetaRobotDmMotorNetwork::check_motor_mode(const std::string &mode) { using enum meta_hardware::MetaRobotDmMotorNetwork::DmMotorMode; if (mode == "mit") { return MIT; @@ -58,8 +57,7 @@ hardware_interface::CallbackReturn MetaRobotDmMotorNetwork::on_configure( std::stod(joint_param.at("mechanical_reduction")); joint_motor_info_[i].offset = std::stod(joint_param.at("offset")); joint_motor_info_[i].mode = check_motor_mode( - joint_param.at("control_mode"), joint_motor_info_[i].command_pos, - joint_motor_info_[i].command_vel, joint_motor_info_[i].command_eff); + joint_param.at("control_mode")); joint_params.emplace_back(joint_param); } diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index d03061f..503fed1 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -34,7 +34,7 @@ DmMotor::DmMotor(const std::unordered_map &motor_param std::string control_mode = motor_param.at("control_mode"); uint32_t id_offeset = 0; - if (control_mode == "MIT") { + if (control_mode == "mit") { Kp_ = std::stod(motor_param.at("Kp")); Kd_ = std::stod(motor_param.at("Kd")); Tff_ = std::stod(motor_param.at("Tff")); diff --git a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp index f80c977..a09b09b 100644 --- a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -22,7 +22,7 @@ DmMotorNetwork::DmMotorNetwork(const std::string &can_network_name, uint8_t mast for (const auto &joint_param : joint_params) { std::string motor_model = joint_param.at("motor_model"); uint32_t dm_motor_id = std::stoi(joint_param.at("motor_id")); - std::string motor_mode = joint_param.at("motor_mode"); + std::string motor_mode = joint_param.at("control_mode"); auto dm_motor = std::make_shared(joint_param, master_id); diff --git a/meta_bringup/config/dm_motor.yaml b/meta_bringup/config/dm_motor.yaml new file mode 100644 index 0000000..1d7e288 --- /dev/null +++ b/meta_bringup/config/dm_motor.yaml @@ -0,0 +1,34 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + motor_pos2vel_pid_controller: + type: pid_controller/PidController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +forward_position_controller: + ros__parameters: + joints: + - motor_pos2vel_pid_controller/motor_joint + interface_name: position + +motor_pos2vel_pid_controller: + ros__parameters: + dof_names: + - motor_joint + + reference_and_state_dof_names: + - motor_joint + + command_interface: velocity + + reference_and_state_interfaces: ["position"] + + gains: + motor_joint: + { p: 10.0, i: 0.0, d: 0.0} diff --git a/meta_bringup/launch/dm_motor.launch.py b/meta_bringup/launch/dm_motor.launch.py new file mode 100644 index 0000000..9f94215 --- /dev/null +++ b/meta_bringup/launch/dm_motor.launch.py @@ -0,0 +1,130 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +# Necessary dirty work that lets us import modules from the meta_bringup package +import os +import sys +from ament_index_python.packages import get_package_share_directory +sys.path.append(os.path.join(get_package_share_directory('meta_bringup'), 'launch')) + +from launch_utils import load_controller, register_loading_order, register_sequential_loading + +ARGUMENTS = [ + DeclareLaunchArgument( + 'enable_simulation', + default_value='false', + description='If true, the simulation will be started'), +] + +def generate_launch_description(): + # Launch Arguments + enable_simulation = LaunchConfiguration('enable_simulation') + + # Get URDF via xacro + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'dm_motor.xacro']), + ' ', + 'is_simulation:=', enable_simulation, + ]) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[ + {'use_sim_time': enable_simulation}, + {'robot_description': robot_description_content},], + output='both', + emulate_tty=True + ) + + # Gazebo related launch + world_sdf = PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'worlds', 'empty_world.sdf']) + bridge_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'ros_gz_bridge.yaml']) + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'launch', 'meta_gazebo.launch.py'])], + ), + launch_arguments=[ + ('world_sdf', world_sdf), + ('robot_name', 'dm_motor'), + ('bridge_config_file', bridge_config), + ], + condition=IfCondition(enable_simulation) + ) + + # ROS2 Control related launch + robot_controller_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'dm_motor.yaml']) + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controller_config], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output='both', + emulate_tty=True, + condition=UnlessCondition(enable_simulation) + ) + + load_joint_state_broadcaster = load_controller('joint_state_broadcaster') + + # List of controllers to be loaded sequentially + # Order in this list is IMPORTANT + load_controllers = [ + load_controller('motor_pos2vel_pid_controller'), + load_controller('forward_position_controller'), + ] + + motor_tester_node = Node( + package='motor_tester', + executable='motor_tester', + name='motor_tester_node', + parameters=[robot_controller_config], + ) + + dbus_control_node = Node( + package='dbus_control', + executable='dbus_control_node', + name='dbus_control_node', + parameters=[robot_controller_config], + ) + + return LaunchDescription([ + # Launch Arguments + *ARGUMENTS, + # Launch Gazebo and ROS2 bridge and spawn robot in Gazebo (also start controller manager) + gazebo_launch, + # Load robot state publisher + node_robot_state_publisher, + # Launch controller manager (if not in simulation) + controller_manager, + # Load joint state broadcaster + load_joint_state_broadcaster, + # Load controllers + *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), + motor_tester_node, + dbus_control_node, + ]) From 5b47ed3089e26919ffad23d1cc8c754291baad4b Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Mon, 19 Aug 2024 20:02:18 +0800 Subject: [PATCH 19/36] bug fix in xcro --- decomposition/metav_description/urdf/playground/dm_motor.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/decomposition/metav_description/urdf/playground/dm_motor.xacro b/decomposition/metav_description/urdf/playground/dm_motor.xacro index a6a1984..35ac3de 100644 --- a/decomposition/metav_description/urdf/playground/dm_motor.xacro +++ b/decomposition/metav_description/urdf/playground/dm_motor.xacro @@ -37,7 +37,7 @@ name="single_motor_control" type="system"> meta_hardware/MetaRobotDmMotorNetwork - cagi n0 + can0 1 From 6c2e0bf31d257b29e358d6eb31637e681b9b457e Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Wed, 21 Aug 2024 19:23:42 +0800 Subject: [PATCH 20/36] launch file modified --- .../urdf/playground/dm_motor.xacro | 4 ++-- .../src/motor_driver/dm_motor_driver.cpp | 2 +- meta_bringup/config/dm_motor.yaml | 21 +------------------ meta_bringup/launch/dm_motor.launch.py | 1 - 4 files changed, 4 insertions(+), 24 deletions(-) diff --git a/decomposition/metav_description/urdf/playground/dm_motor.xacro b/decomposition/metav_description/urdf/playground/dm_motor.xacro index 35ac3de..e4360fc 100644 --- a/decomposition/metav_description/urdf/playground/dm_motor.xacro +++ b/decomposition/metav_description/urdf/playground/dm_motor.xacro @@ -26,7 +26,7 @@ - + @@ -51,7 +51,7 @@ 45 18 0.0 - mit + position 0.0 1.0 0.0 diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index 503fed1..45bfd29 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -44,7 +44,7 @@ DmMotor::DmMotor(const std::unordered_map &motor_param run_mode_ = RunMode::MIT; - } else if (control_mode == "postition") { + } else if (control_mode == "position") { run_mode_ = RunMode::POSITION; id_offeset = 0x100; } else if (control_mode == "velocity") { diff --git a/meta_bringup/config/dm_motor.yaml b/meta_bringup/config/dm_motor.yaml index 1d7e288..a0e1417 100644 --- a/meta_bringup/config/dm_motor.yaml +++ b/meta_bringup/config/dm_motor.yaml @@ -5,30 +5,11 @@ controller_manager: forward_position_controller: type: forward_command_controller/ForwardCommandController - motor_pos2vel_pid_controller: - type: pid_controller/PidController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster forward_position_controller: ros__parameters: joints: - - motor_pos2vel_pid_controller/motor_joint - interface_name: position - -motor_pos2vel_pid_controller: - ros__parameters: - dof_names: - - motor_joint - - reference_and_state_dof_names: - motor_joint - - command_interface: velocity - - reference_and_state_interfaces: ["position"] - - gains: - motor_joint: - { p: 10.0, i: 0.0, d: 0.0} + interface_name: velocity \ No newline at end of file diff --git a/meta_bringup/launch/dm_motor.launch.py b/meta_bringup/launch/dm_motor.launch.py index 9f94215..6c03c5f 100644 --- a/meta_bringup/launch/dm_motor.launch.py +++ b/meta_bringup/launch/dm_motor.launch.py @@ -94,7 +94,6 @@ def generate_launch_description(): # List of controllers to be loaded sequentially # Order in this list is IMPORTANT load_controllers = [ - load_controller('motor_pos2vel_pid_controller'), load_controller('forward_position_controller'), ] From abedc01df04fc3f0ace8a5b78ff73ce813e48e28 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 23 Aug 2024 00:29:49 +0800 Subject: [PATCH 21/36] MIT Position Mode Complete --- .../urdf/playground/dm_motor.xacro | 11 ++--- .../meta_hardware/dm_motor_interface.hpp | 8 ++- .../motor_driver/dm_motor_driver.hpp | 5 -- .../meta_hardware/src/dm_motor_interface.cpp | 49 +++++++++++++++++-- .../src/motor_driver/dm_motor_driver.cpp | 9 ++-- .../src/motor_network/dm_motor_network.cpp | 2 +- meta_bringup/config/dm_motor.yaml | 2 +- 7 files changed, 64 insertions(+), 22 deletions(-) diff --git a/decomposition/metav_description/urdf/playground/dm_motor.xacro b/decomposition/metav_description/urdf/playground/dm_motor.xacro index e4360fc..5a9a51c 100644 --- a/decomposition/metav_description/urdf/playground/dm_motor.xacro +++ b/decomposition/metav_description/urdf/playground/dm_motor.xacro @@ -38,23 +38,22 @@ meta_hardware/MetaRobotDmMotorNetwork can0 - 1 + 0 - + 4310 1 - -1.0 + 1.0 12.5 45 18 0.0 - position - 0.0 + mit + 50.0 1.0 - 0.0 diff --git a/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp index a2b0e36..1419c3b 100644 --- a/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp +++ b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp @@ -54,6 +54,11 @@ class MetaRobotDmMotorNetwork : public hardware_interface::SystemInterface { enum class DmMotorMode { MIT, // Dynamic mode with all three commands + MIT_POS, + MIT_VEL, + MIT_EFF, + MIT_POS_FF, + MIT_VEL_FF, POSITION, // Position mode VELOCITY, // Velocity mode }; @@ -83,7 +88,8 @@ class MetaRobotDmMotorNetwork : public hardware_interface::SystemInterface { std::vector joint_motor_info_; // local cache of joint motor info - MetaRobotDmMotorNetwork::DmMotorMode check_motor_mode(const std::string &mode); + MetaRobotDmMotorNetwork::DmMotorMode check_motor_mode(const std::string &mode,bool command_pos, + bool command_vel, bool command_eff); std::unique_ptr dm_motor_network_; }; diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp index fd80b52..8726fd6 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -16,9 +16,6 @@ namespace meta_hardware { class DmMotor { public: - // DmMotor(const std::string &motor_model, uint32_t dm_motor_id,std::string mode, - // double max_vel, double max_pos, double max_effort, uint32_t Kp, uint32_t Kd, - // uint32_t Tff); DmMotor(const std::unordered_map &motor_param, uint8_t master_id); @@ -66,10 +63,8 @@ class DmMotor { double Kp_; double Kd_; - uint32_t Tff_; uint8_t Kp_raw_; uint16_t Kd_raw_; - uint16_t Tff_raw_; // Motor feedback uint8_t error_code_{0}; diff --git a/execution/meta_hardware/src/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp index fe3d8bf..47f01e3 100644 --- a/execution/meta_hardware/src/dm_motor_interface.cpp +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -31,10 +31,25 @@ MetaRobotDmMotorNetwork::on_init(const hardware_interface::HardwareInfo &info) { } MetaRobotDmMotorNetwork::DmMotorMode -MetaRobotDmMotorNetwork::check_motor_mode(const std::string &mode) { +MetaRobotDmMotorNetwork::check_motor_mode(const std::string &mode,bool command_pos, + bool command_vel, bool command_eff) { using enum meta_hardware::MetaRobotDmMotorNetwork::DmMotorMode; if (mode == "mit") { - return MIT; + if (command_pos && command_vel && command_eff) { + return MIT; + } else if (command_pos && !command_vel && !command_eff) { + return MIT_POS; + } else if (!command_pos && command_vel && !command_eff) { + return MIT_VEL; + } else if (!command_pos && !command_vel && command_eff) { + return MIT_EFF; + } else if (command_pos && !command_vel && command_eff) { + return MIT_POS_FF; + } else if (!command_pos && command_vel && command_eff) { + return MIT_VEL_FF; + } else { + throw std::runtime_error("Invalid dynamic mode"); + } } else if (mode == "position") { return POSITION; } else if (mode == "velocity") { @@ -57,7 +72,8 @@ hardware_interface::CallbackReturn MetaRobotDmMotorNetwork::on_configure( std::stod(joint_param.at("mechanical_reduction")); joint_motor_info_[i].offset = std::stod(joint_param.at("offset")); joint_motor_info_[i].mode = check_motor_mode( - joint_param.at("control_mode")); + joint_param.at("control_mode"),joint_motor_info_[i].command_pos, + joint_motor_info_[i].command_vel, joint_motor_info_[i].command_eff); joint_params.emplace_back(joint_param); } @@ -197,6 +213,33 @@ MetaRobotDmMotorNetwork::write(const rclcpp::Time & /*time*/, continue; dm_motor_network_->write_mit(i, position, velocity, effort); break; + + case MIT_POS: + if (std::isnan(position)) + continue; + dm_motor_network_->write_mit(i, position, 0.0, 0.0); + break; + case MIT_VEL: + if (std::isnan(velocity)) + continue; + std::cout << "Writing velocity" << std::endl; + dm_motor_network_->write_mit(i, 0.0, velocity, 0.0); + break; + case MIT_EFF: + if (std::isnan(effort)) + continue; + dm_motor_network_->write_mit(i, 0.0, 0.0, effort); + break; + case MIT_POS_FF: + if (std::isnan(position) || std::isnan(effort)) + continue; + dm_motor_network_->write_mit(i, position, 0.0, effort); + break; + case MIT_VEL_FF: + if (std::isnan(velocity) || std::isnan(effort)) + continue; + dm_motor_network_->write_mit(i, 0.0, velocity, effort); + break; case POSITION: if (std::isnan(position) || std::isnan(velocity)) continue; diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index 45bfd29..1b21870 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -37,10 +37,9 @@ DmMotor::DmMotor(const std::unordered_map &motor_param if (control_mode == "mit") { Kp_ = std::stod(motor_param.at("Kp")); Kd_ = std::stod(motor_param.at("Kd")); - Tff_ = std::stod(motor_param.at("Tff")); - Kp_raw_ = static_cast((Kp_ - MAX_KP)/(MAX_KP - MIN_KP)) * ( (1 << 8) - 1); - Kd_raw_ = static_cast((Kd_ - MAX_KD)/(MAX_KD - MIN_KD)) * ( (1 << 12) - 1); + Kp_raw_ = static_cast((Kp_ - MIN_KP)/(MAX_KP - MIN_KP) * ( (1 << 8) - 1)); + Kd_raw_ = static_cast((Kd_ - MIN_KD)/(MAX_KD - MIN_KD) * ( (1 << 12) - 1)); run_mode_ = RunMode::MIT; @@ -97,9 +96,9 @@ can_frame DmMotor::motor_clear_error_frame() const{ } can_frame DmMotor::motor_mit_frame(double position, double velocity, double effort) const{ - uint16_t velocity_raw = double_to_raw(velocity, max_vel_, -max_vel_, 16); + uint16_t velocity_raw = double_to_raw(velocity, max_vel_, -max_vel_, 12); uint16_t position_raw = double_to_raw(position, max_pos_, -max_pos_, 16); - uint16_t effort_raw = double_to_raw(effort, max_effort_, -max_effort_, 16); + uint16_t effort_raw = double_to_raw(effort, max_effort_, -max_effort_, 12); can_frame frame = { .can_id = tx_can_id_, .can_dlc = 8, diff --git a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp index a09b09b..eeb94f2 100644 --- a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -100,7 +100,7 @@ void DmMotorNetwork::rx_loop(std::stop_token stop_token) { while (!stop_token.stop_requested()) { try { auto can_msg = can_driver_->read(2000); - const auto &motor = motor_id2motor_.at(can_msg.can_id); + const auto &motor = motor_id2motor_.at(can_msg.data[0] & 0x0F); motor->set_motor_feedback(can_msg); } catch (CanIOTimedOutException & /*e*/) { std::cerr << "Timed out waiting for DM motor feedback." << std::endl; diff --git a/meta_bringup/config/dm_motor.yaml b/meta_bringup/config/dm_motor.yaml index a0e1417..bc57074 100644 --- a/meta_bringup/config/dm_motor.yaml +++ b/meta_bringup/config/dm_motor.yaml @@ -12,4 +12,4 @@ forward_position_controller: ros__parameters: joints: - motor_joint - interface_name: velocity \ No newline at end of file + interface_name: position \ No newline at end of file From a811e6ef1c88a84af4d85821cfbbfd5ff5b645e5 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Fri, 23 Aug 2024 12:05:12 +0800 Subject: [PATCH 22/36] Update MI motor params --- .../urdf/playground/single_motor_ext_encoder.xacro | 2 +- execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp | 4 ++++ meta_bringup/config/single_motor_ext_encoder.yaml | 5 ++--- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro b/decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro index 78f7418..c407c6b 100644 --- a/decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro +++ b/decomposition/metav_description/urdf/playground/single_motor_ext_encoder.xacro @@ -40,7 +40,7 @@ can0 - + CyberGear diff --git a/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp index 48587ee..533f30d 100644 --- a/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/mi_motor_driver.cpp @@ -131,10 +131,14 @@ can_frame MiMotor::motor_dyn_frame(double position, double velocity, } can_frame MiMotor::motor_pos_frame(double position) const { + position = std::clamp(position, -MAX_ABS_POSITION, MAX_ABS_POSITION); + return motor_wr_param_frame(0x7016, static_cast(position)); } can_frame MiMotor::motor_vel_frame(double velocity) const { + velocity = std::clamp(velocity, -MAX_ABS_VELOCITY, MAX_ABS_VELOCITY); + return motor_wr_param_frame(0x700A, static_cast(velocity)); } diff --git a/meta_bringup/config/single_motor_ext_encoder.yaml b/meta_bringup/config/single_motor_ext_encoder.yaml index 1d7e288..34727e3 100644 --- a/meta_bringup/config/single_motor_ext_encoder.yaml +++ b/meta_bringup/config/single_motor_ext_encoder.yaml @@ -25,10 +25,9 @@ motor_pos2vel_pid_controller: reference_and_state_dof_names: - motor_joint - command_interface: velocity + command_interface: effort reference_and_state_interfaces: ["position"] gains: - motor_joint: - { p: 10.0, i: 0.0, d: 0.0} + motor_joint: { p: 12.0, i: 0.0, d: 0.0, angle_wraparound: true } From 08a1c61f75df17af55a5db25bb9bef97975efed2 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 23 Aug 2024 15:59:46 +0800 Subject: [PATCH 23/36] dm motor first stage test complete --- .../metav_description/urdf/playground/dm_motor.xacro | 3 ++- .../meta_hardware/src/motor_driver/dm_motor_driver.cpp | 6 +++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/decomposition/metav_description/urdf/playground/dm_motor.xacro b/decomposition/metav_description/urdf/playground/dm_motor.xacro index 5a9a51c..a5ecb78 100644 --- a/decomposition/metav_description/urdf/playground/dm_motor.xacro +++ b/decomposition/metav_description/urdf/playground/dm_motor.xacro @@ -42,10 +42,11 @@ + 4310 - 1 + 2 1.0 12.5 45 diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index 1b21870..252229f 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -159,12 +159,12 @@ void DmMotor::set_motor_feedback(const can_frame &can_msg){ ); auto velocity_raw = static_cast - (((static_cast(can_msg.data[3]) & 0xFF) << 8) | - ((static_cast(can_msg.data[4]) & 0xC0) >> 12) + (((static_cast(can_msg.data[3]) & 0xFF) << 4) | + ((static_cast(can_msg.data[4]) & 0xF0) >> 4) ); auto effort_raw = static_cast - (((static_cast(can_msg.data[4]) & 0x3F) << 4) | + (((static_cast(can_msg.data[4]) & 0x0F) << 8) | (static_cast(can_msg.data[5]) & 0xFF) ); From 997b1c4741031b179567cd5acbe57e01fc397028 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Sun, 25 Aug 2024 01:25:45 +0800 Subject: [PATCH 24/36] Remove transform from gimbal controller as they are not needed --- .../gimbal_controller/gimbal_controller.hpp | 8 ---- .../src/gimbal_controller.cpp | 41 +++---------------- 2 files changed, 5 insertions(+), 44 deletions(-) diff --git a/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp index 35e18ba..049730b 100644 --- a/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp +++ b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp @@ -16,10 +16,6 @@ #include "realtime_tools/realtime_buffer.h" #include "realtime_tools/realtime_publisher.h" #include "std_srvs/srv/set_bool.hpp" -#include "tf2/tf2/LinearMath/Matrix3x3.h" -#include "tf2/tf2/LinearMath/Quaternion.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_listener.h" #include "behavior_interface/msg/aim.hpp" #include "control_msgs/msg/multi_dof_state_stamped.hpp" @@ -77,10 +73,6 @@ class GimbalController : public controller_interface::ChainableControllerInterfa realtime_tools::RealtimeBuffer> input_feedback_; - std::unique_ptr tf_buffer_; - std::unique_ptr tf_listener_; - tf2::Quaternion rot_imu2gimbal_; - std::shared_ptr yaw_pos2vel_pid_; std::shared_ptr pitch_pos2vel_pid_; std::shared_ptr yaw_vel2eff_pid_; diff --git a/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp b/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp index 4d384da..209d59e 100644 --- a/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp +++ b/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp @@ -5,10 +5,8 @@ #include #include #include -#include -#include +#include #include -#include #include #include "angles/angles.h" @@ -118,10 +116,6 @@ GimbalController::on_configure(const rclcpp_lifecycle::State & /*previous_state* auto feedback_msg = std::make_shared(); reset_controller_feedback_msg(feedback_msg, get_node()); input_feedback_.writeFromNonRT(feedback_msg); - - tf_buffer_ = std::make_unique(get_node()->get_clock()); - tf_listener_ = std::make_unique(*tf_buffer_); - try { // State publisher s_publisher_ = get_node()->create_publisher( @@ -213,23 +207,6 @@ GimbalController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/ reference_interfaces_.assign(reference_interfaces_.size(), NaN); - // Get transform from IMU to gimbal - geometry_msgs::msg::TransformStamped trans; - - try { - trans = - tf_buffer_->lookupTransform("gimbal_imu", "pitch_gimbal", tf2::TimePointZero, - tf2::durationFromSec(10.0)); // wait for 10s - } catch (tf2::LookupException &e) { - RCLCPP_ERROR(get_node()->get_logger(), "Failed to lookup transform: %s", - e.what()); - return controller_interface::CallbackReturn::ERROR; - } - - rot_imu2gimbal_ = - tf2::Quaternion(trans.transform.rotation.x, trans.transform.rotation.y, - trans.transform.rotation.z, trans.transform.rotation.w); - return controller_interface::CallbackReturn::SUCCESS; } @@ -259,21 +236,13 @@ GimbalController::update_and_write_commands(const rclcpp::Time &time, // Collect IMU feedback auto current_feedback = *(input_feedback_.readFromRT()); - // Transform IMU feedback to gimbal frame + double yaw_pos_fb, pitch_pos_fb, roll_pos_fb; tf2::Quaternion q_imu; fromMsg(current_feedback->orientation, q_imu); - tf2::Quaternion q_gimbal = q_imu * rot_imu2gimbal_; - - // Transform angular velocity feedback to gimbal frame - tf2::Vector3 v_imu; - fromMsg(current_feedback->angular_velocity, v_imu); - tf2::Vector3 v_gimbal = tf2::quatRotate(rot_imu2gimbal_.inverse(), v_imu); - - double yaw_pos_fb, pitch_pos_fb, roll_pos_fb; - tf2::Matrix3x3(q_gimbal).getRPY(roll_pos_fb, pitch_pos_fb, yaw_pos_fb); + tf2::Matrix3x3(q_imu).getRPY(roll_pos_fb, pitch_pos_fb, yaw_pos_fb); - double yaw_vel_fb = v_gimbal.z(); - double pitch_vel_fb = v_gimbal.y(); + double yaw_vel_fb = current_feedback->angular_velocity.z; + double pitch_vel_fb = current_feedback->angular_velocity.y; double yaw_pos_ref = NaN, pitch_pos_ref = NaN; double yaw_pos_err = NaN, pitch_pos_err = NaN; From 8f22648bc3c77cee9987d275f26e8fd1c350fad7 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Sun, 25 Aug 2024 01:29:08 +0800 Subject: [PATCH 25/36] Revisit IMU position in xacro file --- decomposition/metav_description/urdf/sentry/sentry.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/decomposition/metav_description/urdf/sentry/sentry.xacro b/decomposition/metav_description/urdf/sentry/sentry.xacro index 810c6d5..516b605 100644 --- a/decomposition/metav_description/urdf/sentry/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry/sentry.xacro @@ -26,7 +26,7 @@ + rpy="0 0 0" /> Date: Sun, 25 Aug 2024 19:24:44 +0800 Subject: [PATCH 26/36] position mode complete --- .../urdf/playground/dm_motor.xacro | 12 +++---- .../motor_driver/dm_motor_driver.hpp | 2 +- .../motor_network/dm_motor_network.hpp | 2 +- .../meta_hardware/src/dm_motor_interface.cpp | 5 +-- .../src/motor_driver/dm_motor_driver.cpp | 34 ++++++++++--------- .../src/motor_network/dm_motor_network.cpp | 4 +-- 6 files changed, 31 insertions(+), 28 deletions(-) diff --git a/decomposition/metav_description/urdf/playground/dm_motor.xacro b/decomposition/metav_description/urdf/playground/dm_motor.xacro index a5ecb78..23d7dc7 100644 --- a/decomposition/metav_description/urdf/playground/dm_motor.xacro +++ b/decomposition/metav_description/urdf/playground/dm_motor.xacro @@ -48,13 +48,13 @@ 4310 2 1.0 - 12.5 - 45 - 18 + 3.141593 + 30 + 10 0.0 - mit - 50.0 - 1.0 + position + 150.0 + 0.5 diff --git a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp index 8726fd6..5208e5b 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -36,7 +36,7 @@ class DmMotor { can_frame motor_save_initial_frame() const; can_frame motor_clear_error_frame() const; can_frame motor_mit_frame(double position, double velocity, double effort) const; - can_frame motor_pos_frame(double position, double velocity) const; + can_frame motor_pos_frame(double position) const; can_frame motor_vel_frame(double velocity) const; void set_motor_feedback(const can_frame &can_msg); diff --git a/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp index 1aaa29a..97416a4 100644 --- a/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp +++ b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp @@ -37,7 +37,7 @@ class DmMotorNetwork { * @param effort The effort to write */ void write_mit(uint32_t joint_id, double position, double velocity, double effort); - void write_pos(uint32_t joint_id, double position, double velocity); + void write_pos(uint32_t joint_id, double position); void write_vel(uint32_t joint_id, double velocity); private: diff --git a/execution/meta_hardware/src/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp index 47f01e3..fdefc5c 100644 --- a/execution/meta_hardware/src/dm_motor_interface.cpp +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -241,9 +241,10 @@ MetaRobotDmMotorNetwork::write(const rclcpp::Time & /*time*/, dm_motor_network_->write_mit(i, 0.0, velocity, effort); break; case POSITION: - if (std::isnan(position) || std::isnan(velocity)) + if (std::isnan(position)){ continue; - dm_motor_network_->write_pos(i, position, velocity); + } + dm_motor_network_->write_pos(i, position); break; case VELOCITY: if (std::isnan(velocity)) diff --git a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp index 252229f..882ca15 100644 --- a/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -114,33 +115,34 @@ can_frame DmMotor::motor_mit_frame(double position, double velocity, double effo return frame; } -can_frame DmMotor::motor_pos_frame(double position, double velocity) const{ - uint32_t velocity_raw = double_to_raw(velocity, max_vel_, -max_vel_, 32); - uint32_t position_raw = double_to_raw(position, max_pos_, -max_pos_, 32); +can_frame DmMotor::motor_pos_frame(double position) const{ + double velocity = max_vel_; + uint32_t velocity_raw = std::bit_cast(static_cast(velocity)); + uint32_t position_raw = std::bit_cast(static_cast(position)); can_frame frame = { .can_id = tx_can_id_, .can_dlc = 8, - .data = { static_cast((position_raw & 0xFF000000) >> 24), - static_cast((position_raw & 0x00FF0000) >> 16), - static_cast((position_raw & 0x0000FF00) >> 8), - static_cast(position_raw & 0x000000FF), - static_cast((velocity_raw & 0xFF000000) >> 24), - static_cast((velocity_raw & 0x00FF0000) >> 16), - static_cast((velocity_raw & 0x0000FF00) >> 8), - static_cast(velocity_raw & 0x000000FF)} + .data = { static_cast((position_raw & 0x000000FF)), + static_cast((position_raw & 0x0000FF00) >> 8), + static_cast((position_raw & 0x00FF0000) >> 16), + static_cast((position_raw & 0xFF000000) >> 24), + static_cast((velocity_raw & 0x000000FF)), + static_cast((velocity_raw & 0x0000FF00) >> 8), + static_cast((velocity_raw & 0x00FF0000) >> 16), + static_cast((velocity_raw & 0xFF000000) >> 24)} }; return frame; } can_frame DmMotor::motor_vel_frame(double velocity) const{ - uint32_t velocity_raw = double_to_raw(velocity, max_vel_, -max_vel_, 32); + uint32_t velocity_raw = std::bit_cast(static_cast(velocity)); can_frame frame = { .can_id = tx_can_id_, .can_dlc = 4, - .data = { static_cast((velocity_raw & 0xFF000000) >> 24), - static_cast((velocity_raw & 0x00FF0000) >> 16), - static_cast((velocity_raw & 0x0000FF00) >> 8), - static_cast(velocity_raw & 0x000000FF)} + .data = { static_cast((velocity_raw & 0x000000FF)), + static_cast((velocity_raw & 0x0000FF00) >> 8), + static_cast((velocity_raw & 0x00FF0000) >> 16), + static_cast((velocity_raw & 0xFF000000) >> 24)} }; return frame; } diff --git a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp index eeb94f2..94bd225 100644 --- a/execution/meta_hardware/src/motor_network/dm_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -76,10 +76,10 @@ void DmMotorNetwork::write_mit(uint32_t joint_id, double position, double veloci } } -void DmMotorNetwork::write_pos(uint32_t joint_id, double position, double velocity){ +void DmMotorNetwork::write_pos(uint32_t joint_id, double position){ const auto &motor = dm_motors_[joint_id]; try { - can_driver_->write(motor->motor_pos_frame(position, velocity)); + can_driver_->write(motor->motor_pos_frame(position)); } catch (CanIOException &e) { std::cerr << "Error writing DM motor command CAN message: " << e.what() << std::endl; From d3153616f7c737a29fca20d8dddd3df8fd73d4c5 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Mon, 26 Aug 2024 19:34:57 +0800 Subject: [PATCH 27/36] create modbus rtu driver --- execution/meta_hardware/CMakeLists.txt | 1 + .../meta_hardware/im1253c_interface.hpp | 60 +++++++ .../modbus_rtu_driver/modbus_rtu_driver.hpp | 46 +++++ .../meta_hardware/src/im1253c_interface.cpp | 162 ++++++++++++++++++ .../modbus_rtu_driver/modbus_rtu_driver.cpp | 118 +++++++++++++ 5 files changed, 387 insertions(+) create mode 100644 execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp create mode 100644 execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp create mode 100644 execution/meta_hardware/src/im1253c_interface.cpp create mode 100644 execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp diff --git a/execution/meta_hardware/CMakeLists.txt b/execution/meta_hardware/CMakeLists.txt index ecda4c5..1af7c4a 100644 --- a/execution/meta_hardware/CMakeLists.txt +++ b/execution/meta_hardware/CMakeLists.txt @@ -30,6 +30,7 @@ add_library( src/brt_encoder_interface.cpp src/motor_network/brt_encoder_network.cpp src/motor_driver/brt_encoder_driver.cpp + src/im1253c_interface.cpp ) target_include_directories( meta_hardware diff --git a/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp b/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp new file mode 100644 index 0000000..0e5f15b --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp @@ -0,0 +1,60 @@ +#ifndef META_HARDWARE__IM1253C_INTERFACE_HPP_ +#define META_HARDWARE__IM1253C_INTERFACE_HPP_ + +#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 "meta_hardware/visibility_control.h" +#include "rclcpp/macros.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace meta_hardware { +constexpr double NaN = std::numeric_limits::quiet_NaN(); + +class MetaRobotIm1253cManager : public hardware_interface::SystemInterface { + public: + ~MetaRobotIm1253cManager() override; + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_init(const hardware_interface::HardwareInfo &info) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector + export_state_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + std::vector + export_command_interfaces() override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type + read(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC + hardware_interface::return_type + write(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + private: + +}; + +} // namespace meta_hardware + +#endif // META_HARDWARE__IM1253C_INTERFACE_HPP_ diff --git a/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp b/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp new file mode 100644 index 0000000..5802a59 --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp @@ -0,0 +1,46 @@ +#ifndef META_HARDWARE__MODBUS_RTU_DRIVER_HPP_ +#define META_HARDWARE__MODBUS_RTU_DRIVER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + + +class ModbusRtuDriver +{ +public: + explicit ModbusRtuDriver(std::unordered_map serial_params); + ~ModbusRtuDriver(); + + void set_command(int addr, int func, int reg, int len); + void get_response(std::vector &data); + +private: + std::unique_ptr owned_ctx_; + std::string device_name_; + std::unique_ptr device_config_; + std::unique_ptr serial_driver_; + std::unique_ptr receive_thread_; + + int baud_rate_; + drivers::serial_driver::FlowControl flow_control_; + drivers::serial_driver::Parity parity_; + drivers::serial_driver::StopBits stop_bits_; + + std::vector command_; + std::vector response_; + + void rx_tx_loop(std::stop_token stop_token); + auto awake_time(); + void read_params(std::unordered_map serial_params); + void send_command(std::vector data); + + + +}; +#endif // META_HARDWARE__MODBUS_RTU_DRIVER_HPP_ \ No newline at end of file diff --git a/execution/meta_hardware/src/im1253c_interface.cpp b/execution/meta_hardware/src/im1253c_interface.cpp new file mode 100644 index 0000000..4283669 --- /dev/null +++ b/execution/meta_hardware/src/im1253c_interface.cpp @@ -0,0 +1,162 @@ +#include +#include +#include +#include +#include + +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "meta_hardware/im1253c_interface.hpp" +#include "meta_hardware/motor_network/mi_motor_network.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace meta_hardware { +using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; + +MetaRobotIm1253cManager::~MetaRobotIm1253cManager() = default; + +hardware_interface::CallbackReturn +MetaRobotIm1253cManager::on_init(const hardware_interface::HardwareInfo &info) { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + + // joint_interface_data_.resize(info_.joints.size()); + // joint_motor_info_.resize(info_.joints.size()); + + return CallbackReturn::SUCCESS; +} + + +hardware_interface::CallbackReturn MetaRobotIm1253cManager::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + + std::vector> joint_params; + // Add the motors to the motor networks + // for (size_t i = 0; i < info_.joints.size(); ++i) { + // const auto &joint = info_.joints[i]; + // const auto &joint_param = joint.parameters; + // joint_motor_info_[i].name = joint.name; + // joint_motor_info_[i].mechanical_reduction = + // std::stod(joint_param.at("mechanical_reduction")); + // joint_motor_info_[i].offset = std::stod(joint_param.at("offset")); + // joint_motor_info_[i].mode = check_motor_mode( + // joint_param.at("control_mode"), joint_motor_info_[i].command_pos, + // joint_motor_info_[i].command_vel, joint_motor_info_[i].command_eff); + // joint_params.emplace_back(joint_param); + // } + + // std::string can_network_name = info_.hardware_parameters.at("can_network_name"); + // mi_motor_network_ = + // std::make_unique(can_network_name, 0x00, joint_params); + + return CallbackReturn::SUCCESS; +} + +std::vector +MetaRobotIm1253cManager::export_state_interfaces() { + std::vector state_interfaces; + + // Helper function to check if the interface exists + auto contains_interface = + [](const std::vector &interfaces, + const std::string &interface_name) { + return std::ranges::find_if( + interfaces, + [&interface_name]( + const hardware_interface::InterfaceInfo &interface) { + return interface.name == interface_name; + }) != interfaces.end(); + }; + + // for (size_t i = 0; i < info_.joints.size(); ++i) { + // const auto &joint_state_interfaces = info_.joints[i].state_interfaces; + // if (contains_interface(joint_state_interfaces, "position")) { + // state_interfaces.emplace_back(info_.joints[i].name, HW_IF_POSITION, + // &joint_interface_data_[i].state_position); + // } + // if (contains_interface(joint_state_interfaces, "velocity")) { + // state_interfaces.emplace_back(info_.joints[i].name, HW_IF_VELOCITY, + // &joint_interface_data_[i].state_velocity); + // } + // if (contains_interface(joint_state_interfaces, "effort")) { + // state_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, + // &joint_interface_data_[i].state_effort); + // } + // } + + return state_interfaces; +} + +std::vector +MetaRobotIm1253cManager::export_command_interfaces() { + std::vector command_interfaces; + + // Helper function to check if the interface exists + // auto contains_interface = + // [](const std::vector &interfaces, + // const std::string &interface_name) { + // return std::ranges::find_if( + // interfaces, + // [&interface_name]( + // const hardware_interface::InterfaceInfo &interface) { + // return interface.name == interface_name; + // }) != interfaces.end(); + // }; + + // for (size_t i = 0; i < info_.joints.size(); ++i) { + // const auto &joint_command_interfaces = info_.joints[i].command_interfaces; + // if (contains_interface(joint_command_interfaces, "position")) { + // command_interfaces.emplace_back(info_.joints[i].name, HW_IF_POSITION, + // &joint_interface_data_[i].command_position); + // joint_motor_info_[i].command_pos = true; + // } + // if (contains_interface(joint_command_interfaces, "velocity")) { + // command_interfaces.emplace_back(info_.joints[i].name, HW_IF_VELOCITY, + // &joint_interface_data_[i].command_velocity); + // joint_motor_info_[i].command_vel = true; + // } + // if (contains_interface(joint_command_interfaces, "effort")) { + // command_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, + // &joint_interface_data_[i].command_effort); + // joint_motor_info_[i].command_eff = true; + // } + // } + + return command_interfaces; +} + +hardware_interface::CallbackReturn +MetaRobotIm1253cManager::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MetaRobotIm1253cManager::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type +MetaRobotIm1253cManager::read(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type +MetaRobotIm1253cManager::write(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + return hardware_interface::return_type::OK; +} + +} // namespace meta_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(meta_hardware::MetaRobotIm1253cManager, + hardware_interface::SystemInterface) diff --git a/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp b/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp new file mode 100644 index 0000000..ac81321 --- /dev/null +++ b/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp @@ -0,0 +1,118 @@ + +#include "meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp" +#include +#include +#include +#include + +ModbusRtuDriver::ModbusRtuDriver(std::unordered_map serial_params): + owned_ctx_{new IoContext(2)}, + serial_driver_{new drivers::serial_driver::SerialDriver(*owned_ctx_)} + { + + command_.resize(8); + + read_params(serial_params); + device_config_ = std::make_unique + (baud_rate_, flow_control_, parity_, stop_bits_); + + try { + serial_driver_->init_port(device_name_, *device_config_); + if (!serial_driver_->port()->is_open()) { + serial_driver_->port()->open(); + receive_thread_ = std::make_unique([this](std::stop_token s) { rx_tx_loop(s); }); + } + } catch (const std::exception & ex) { + throw ex; + } + +} + +void ModbusRtuDriver::read_params(std::unordered_map serial_params){ + + baud_rate_ = std::stoi(serial_params.at("baud_rate")); + + std::string fc = serial_params.at("flow_control"); + if (fc == "none") { + flow_control_ = drivers::serial_driver::FlowControl::NONE; + } else if (fc == "hardware") { + flow_control_ = drivers::serial_driver::FlowControl::HARDWARE; + } else if (fc == "software") { + flow_control_ = drivers::serial_driver::FlowControl::SOFTWARE; + } else { + throw std::runtime_error("Unknown flow control: " + fc); + } + + std::string pt = serial_params.at("parity"); + if (pt == "none") { + parity_ = drivers::serial_driver::Parity::NONE; + } else if (pt == "odd") { + parity_ = drivers::serial_driver::Parity::ODD; + } else if (pt == "even") { + parity_ = drivers::serial_driver::Parity::EVEN; + } else { + throw std::runtime_error("Unknown parity: " + pt); + } + + float sb = std::stof(serial_params.at("stop_bits")); + if (sb == 1.0) { + stop_bits_ = drivers::serial_driver::StopBits::ONE; + } else if (sb == 1.5) { + stop_bits_ = drivers::serial_driver::StopBits::ONE_POINT_FIVE; + } else if (sb == 2.0) { + stop_bits_ = drivers::serial_driver::StopBits::TWO; + } else { + throw std::runtime_error("Unknown stop bits(Please Use Floating Point Notation): " + std::to_string(sb)); + } + + std::string device_name = serial_params.at("device_name"); +}; + + +auto ModbusRtuDriver::awake_time() +{ + using std::chrono::operator""ms; + return std::chrono::steady_clock::now() + 2ms; +} + +void ModbusRtuDriver::set_command(int addr, int func, int reg, int len){ + command_[0] = static_cast(addr); + command_[1] = static_cast(func); + command_[2] = static_cast(reg >> 8); + command_[3] = static_cast(reg & 0xFF); + command_[4] = static_cast(len >> 8); + command_[5] = static_cast(len & 0xFF); + boost::crc_16_type crc_16; + crc_16.process_bytes(&command_, 6); + int crc = crc_16.checksum(); + command_[6] = static_cast(crc & 0xFF); + command_[7] = static_cast(crc >> 8); + +} + +void ModbusRtuDriver::rx_tx_loop(std::stop_token stop_token){ + while (!stop_token.stop_requested()) { + std::this_thread::sleep_until(awake_time()); + + // Send Command with blocking + serial_driver_->port()->send(command_); + // Receive Response with blocking + response_.clear(); + serial_driver_->port()->receive(response_); + } +} + + +ModbusRtuDriver::~ModbusRtuDriver(){ + if (receive_thread_->joinable()) { + receive_thread_->join(); + } + + if (serial_driver_->port()->is_open()) { + serial_driver_->port()->close(); + } + + if (owned_ctx_) { + owned_ctx_->waitForExit(); + } + } From b1c7a87e78a1d780d137efdef276ede3e894f169 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Tue, 27 Aug 2024 18:27:47 +0800 Subject: [PATCH 28/36] im1253c first version finished --- execution/meta_hardware/CMakeLists.txt | 3 + .../meta_hardware/im1253c_interface.hpp | 11 +- .../modbus_rtu_driver/modbus_rtu_driver.hpp | 4 +- .../meta_hardware/src/im1253c_interface.cpp | 111 ++++++------------ .../modbus_rtu_driver/modbus_rtu_driver.cpp | 55 ++++++++- 5 files changed, 101 insertions(+), 83 deletions(-) diff --git a/execution/meta_hardware/CMakeLists.txt b/execution/meta_hardware/CMakeLists.txt index 1af7c4a..1e0de52 100644 --- a/execution/meta_hardware/CMakeLists.txt +++ b/execution/meta_hardware/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(angles REQUIRED) +find_package(Boost REQUIRED) add_library( meta_hardware @@ -31,6 +32,7 @@ add_library( src/motor_network/brt_encoder_network.cpp src/motor_driver/brt_encoder_driver.cpp src/im1253c_interface.cpp + src/modbus_rtu_driver/modbus_rtu_driver.cpp ) target_include_directories( meta_hardware @@ -43,6 +45,7 @@ ament_target_dependencies( rclcpp rclcpp_lifecycle angles + Boost ) # prevent pluginlib from using boost target_compile_definitions(meta_hardware PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp b/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp index 0e5f15b..4677540 100644 --- a/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp +++ b/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp @@ -2,6 +2,7 @@ #define META_HARDWARE__IM1253C_INTERFACE_HPP_ #include +#include #include #include @@ -9,6 +10,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp" #include "meta_hardware/visibility_control.h" #include "rclcpp/macros.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -52,7 +54,14 @@ class MetaRobotIm1253cManager : public hardware_interface::SystemInterface { write(const rclcpp::Time &time, const rclcpp::Duration &period) override; private: - + std::vector> device_; + class SensorInterfaceData { + public: + double state_voltage = NaN; + double state_current = NaN; + double state_power = NaN; + }; + std::vector sensor_interface_data_; }; } // namespace meta_hardware diff --git a/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp b/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp index 5802a59..b290c22 100644 --- a/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp @@ -18,7 +18,7 @@ class ModbusRtuDriver ~ModbusRtuDriver(); void set_command(int addr, int func, int reg, int len); - void get_response(std::vector &data); + std::vector get_reg_data(){return reg_data_;}; private: std::unique_ptr owned_ctx_; @@ -33,7 +33,7 @@ class ModbusRtuDriver drivers::serial_driver::StopBits stop_bits_; std::vector command_; - std::vector response_; + std::vector reg_data_; void rx_tx_loop(std::stop_token stop_token); auto awake_time(); diff --git a/execution/meta_hardware/src/im1253c_interface.cpp b/execution/meta_hardware/src/im1253c_interface.cpp index 4283669..922427c 100644 --- a/execution/meta_hardware/src/im1253c_interface.cpp +++ b/execution/meta_hardware/src/im1253c_interface.cpp @@ -1,20 +1,26 @@ +#include #include #include #include #include +#include #include #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "meta_hardware/im1253c_interface.hpp" +#include "meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp" #include "meta_hardware/motor_network/mi_motor_network.hpp" #include "rclcpp/rclcpp.hpp" + +constexpr int DEVICE_ADDRESS = 0x01; +constexpr int FUNCTION_CODE_READ = 0x03; +constexpr int REGISTER_ADDRESS = 0x48; +constexpr int REGISTER_NUM = 3; // 3 registers for voltage, current and power + namespace meta_hardware { -using hardware_interface::HW_IF_EFFORT; -using hardware_interface::HW_IF_POSITION; -using hardware_interface::HW_IF_VELOCITY; MetaRobotIm1253cManager::~MetaRobotIm1253cManager() = default; @@ -23,36 +29,16 @@ MetaRobotIm1253cManager::on_init(const hardware_interface::HardwareInfo &info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } - - // joint_interface_data_.resize(info_.joints.size()); - // joint_motor_info_.resize(info_.joints.size()); - return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn MetaRobotIm1253cManager::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { - - std::vector> joint_params; - // Add the motors to the motor networks - // for (size_t i = 0; i < info_.joints.size(); ++i) { - // const auto &joint = info_.joints[i]; - // const auto &joint_param = joint.parameters; - // joint_motor_info_[i].name = joint.name; - // joint_motor_info_[i].mechanical_reduction = - // std::stod(joint_param.at("mechanical_reduction")); - // joint_motor_info_[i].offset = std::stod(joint_param.at("offset")); - // joint_motor_info_[i].mode = check_motor_mode( - // joint_param.at("control_mode"), joint_motor_info_[i].command_pos, - // joint_motor_info_[i].command_vel, joint_motor_info_[i].command_eff); - // joint_params.emplace_back(joint_param); - // } - - // std::string can_network_name = info_.hardware_parameters.at("can_network_name"); - // mi_motor_network_ = - // std::make_unique(can_network_name, 0x00, joint_params); - + for (size_t i = 0; i < info_.sensors.size(); ++i) { + device_.emplace_back(std::make_unique(info_.hardware_parameters)); + device_[i]->set_command(DEVICE_ADDRESS, FUNCTION_CODE_READ, REGISTER_ADDRESS, REGISTER_NUM); + } return CallbackReturn::SUCCESS; } @@ -72,21 +58,21 @@ MetaRobotIm1253cManager::export_state_interfaces() { }) != interfaces.end(); }; - // for (size_t i = 0; i < info_.joints.size(); ++i) { - // const auto &joint_state_interfaces = info_.joints[i].state_interfaces; - // if (contains_interface(joint_state_interfaces, "position")) { - // state_interfaces.emplace_back(info_.joints[i].name, HW_IF_POSITION, - // &joint_interface_data_[i].state_position); - // } - // if (contains_interface(joint_state_interfaces, "velocity")) { - // state_interfaces.emplace_back(info_.joints[i].name, HW_IF_VELOCITY, - // &joint_interface_data_[i].state_velocity); - // } - // if (contains_interface(joint_state_interfaces, "effort")) { - // state_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, - // &joint_interface_data_[i].state_effort); - // } - // } + for (size_t i = 0; i < info_.sensors.size(); ++i) { + const auto &sensors_state_interfaces = info_.sensors[i].state_interfaces; + if (contains_interface(sensors_state_interfaces, "voltage")) { + state_interfaces.emplace_back(info_.sensors[i].name, std::string("voltage"), + &sensor_interface_data_[i].state_voltage); + } + if (contains_interface(sensors_state_interfaces, "current")) { + state_interfaces.emplace_back(info_.sensors[i].name, std::string("current"), + &sensor_interface_data_[i].state_current); + } + if (contains_interface(sensors_state_interfaces, "power")) { + state_interfaces.emplace_back(info_.sensors[i].name, std::string("power"), + &sensor_interface_data_[i].state_power); + } + } return state_interfaces; } @@ -94,58 +80,29 @@ MetaRobotIm1253cManager::export_state_interfaces() { std::vector MetaRobotIm1253cManager::export_command_interfaces() { std::vector command_interfaces; - - // Helper function to check if the interface exists - // auto contains_interface = - // [](const std::vector &interfaces, - // const std::string &interface_name) { - // return std::ranges::find_if( - // interfaces, - // [&interface_name]( - // const hardware_interface::InterfaceInfo &interface) { - // return interface.name == interface_name; - // }) != interfaces.end(); - // }; - - // for (size_t i = 0; i < info_.joints.size(); ++i) { - // const auto &joint_command_interfaces = info_.joints[i].command_interfaces; - // if (contains_interface(joint_command_interfaces, "position")) { - // command_interfaces.emplace_back(info_.joints[i].name, HW_IF_POSITION, - // &joint_interface_data_[i].command_position); - // joint_motor_info_[i].command_pos = true; - // } - // if (contains_interface(joint_command_interfaces, "velocity")) { - // command_interfaces.emplace_back(info_.joints[i].name, HW_IF_VELOCITY, - // &joint_interface_data_[i].command_velocity); - // joint_motor_info_[i].command_vel = true; - // } - // if (contains_interface(joint_command_interfaces, "effort")) { - // command_interfaces.emplace_back(info_.joints[i].name, HW_IF_EFFORT, - // &joint_interface_data_[i].command_effort); - // joint_motor_info_[i].command_eff = true; - // } - // } - return command_interfaces; } hardware_interface::CallbackReturn MetaRobotIm1253cManager::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn MetaRobotIm1253cManager::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; } hardware_interface::return_type MetaRobotIm1253cManager::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - return hardware_interface::return_type::OK; + for(size_t i = 0; i < info_.sensors.size(); i++){ + std::vector sensor_data_raw = device_[i]->get_reg_data(); + sensor_interface_data_[i].state_voltage = static_cast(sensor_data_raw[0])/100.0; + sensor_interface_data_[i].state_current = static_cast(sensor_data_raw[1])/100.0; + sensor_interface_data_[i].state_power = static_cast(sensor_data_raw[2]); + } } hardware_interface::return_type diff --git a/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp b/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp index ac81321..8719e01 100644 --- a/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp +++ b/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp @@ -4,6 +4,9 @@ #include #include #include +#include +#include +#include ModbusRtuDriver::ModbusRtuDriver(std::unordered_map serial_params): owned_ctx_{new IoContext(2)}, @@ -83,7 +86,7 @@ void ModbusRtuDriver::set_command(int addr, int func, int reg, int len){ command_[4] = static_cast(len >> 8); command_[5] = static_cast(len & 0xFF); boost::crc_16_type crc_16; - crc_16.process_bytes(&command_, 6); + crc_16.process_bytes(command_.data(), 6); int crc = crc_16.checksum(); command_[6] = static_cast(crc & 0xFF); command_[7] = static_cast(crc >> 8); @@ -91,14 +94,60 @@ void ModbusRtuDriver::set_command(int addr, int func, int reg, int len){ } void ModbusRtuDriver::rx_tx_loop(std::stop_token stop_token){ + std::vector address(1); + std::vector response(2); + std::vector data; + std::vector response_crc(2); + std::vector response_without_crc; while (!stop_token.stop_requested()) { std::this_thread::sleep_until(awake_time()); // Send Command with blocking serial_driver_->port()->send(command_); // Receive Response with blocking - response_.clear(); - serial_driver_->port()->receive(response_); + + response_without_crc.clear(); + serial_driver_->port()->receive(address); + response_without_crc.emplace_back(address[0]); + if( address[0] == command_[0]){ + serial_driver_->port()->receive(address); + serial_driver_->port()->receive(response); + uint8_t func_code = response[0]; + if(func_code != command_[1]){ + std::cout << " Function Code Mismatch. Expected: " + << command_[1] << " Received: " << func_code << std::endl; + continue; + }else if(func_code == 0x03){ + uint8_t byte_count = response[1]; + data.resize(byte_count); + + serial_driver_->port()->receive(data); + + response_without_crc.emplace_back(func_code); + response_without_crc.emplace_back(byte_count); + for(uint8_t i = 0; i < data.size(); i++){ + response_without_crc.emplace_back(data[i]); + } + + boost::crc_16_type crc_16; + crc_16.process_bytes(response_without_crc.data(), response_without_crc.size()); + int calc_crc = crc_16.checksum(); + serial_driver_->port()->receive(response_crc); + if((static_cast(response_crc[0]) + (static_cast(response_crc[1]) << 8))== calc_crc){ + reg_data_ = data; + }else{ + std::cout << " CRC Mismatch. " << std::endl; + } + }else if(func_code == 0x10){ + std::vector write_response(6); + // get write response + serial_driver_->port()->receive(write_response); + // TODO: Check if the response is the same as the command + } + + }else{ + std::cout << " Device Address Not Found. Device Address: " << address[0] << std::endl; + } } } From 9816723197fd2c4f3212ca0c77a49c5574635c3c Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Wed, 28 Aug 2024 19:20:46 +0800 Subject: [PATCH 29/36] new controller added --- decomposition/power_limit/CMakeLists.txt | 44 ++++++ .../include/power_limit/power_limit.hpp | 86 ++++++++++++ decomposition/power_limit/package.xml | 37 +++++ .../power_limit/power_limit_controller.xml | 29 ++++ decomposition/power_limit/src/power_limit.cpp | 128 +++++++++++++++++ .../power_limit/src/power_limit.yaml | 17 +++ .../device_interface/msg/MotorPower.msg | 5 + meta_bringup/config/im1253c.yaml | 3 + meta_bringup/launch/im1253c.launch.py | 129 ++++++++++++++++++ 9 files changed, 478 insertions(+) create mode 100644 decomposition/power_limit/CMakeLists.txt create mode 100644 decomposition/power_limit/include/power_limit/power_limit.hpp create mode 100644 decomposition/power_limit/package.xml create mode 100644 decomposition/power_limit/power_limit_controller.xml create mode 100644 decomposition/power_limit/src/power_limit.cpp create mode 100644 decomposition/power_limit/src/power_limit.yaml create mode 100644 interfaces/device_interface/msg/MotorPower.msg create mode 100644 meta_bringup/config/im1253c.yaml create mode 100644 meta_bringup/launch/im1253c.launch.py diff --git a/decomposition/power_limit/CMakeLists.txt b/decomposition/power_limit/CMakeLists.txt new file mode 100644 index 0000000..ff5bb7b --- /dev/null +++ b/decomposition/power_limit/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(power_limit) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +generate_parameter_library(power_limit_parameters + src/power_limit.yaml +) + +ament_auto_add_library( + ${PROJECT_NAME} + SHARED + src/power_limit.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") +target_link_libraries(power_limit + power_limit_parameters) + +pluginlib_export_plugin_description_file( + controller_interface power_limit_controller.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/decomposition/power_limit/include/power_limit/power_limit.hpp b/decomposition/power_limit/include/power_limit/power_limit.hpp new file mode 100644 index 0000000..ea2d300 --- /dev/null +++ b/decomposition/power_limit/include/power_limit/power_limit.hpp @@ -0,0 +1,86 @@ +#ifndef POWER_LIMIT__POWER_LIMIT_HPP_ +#define POWER_LIMIT__POWER_LIMIT_HPP_ + +#include +#include +#include + +#include "device_interface/msg/motor_power.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" +#include "power_limit_parameters.hpp" +// #include "" + +#include "control_msgs/msg/joint_controller_state.hpp" + +namespace power_limit { + +enum class control_mode_type : std::uint8_t { + CHASSIS = 0, + GIMBAL = 1, + CHASSIS_FOLLOW_GIMBAL = 2, +}; + +class PowerLimitController : public controller_interface::ChainableControllerInterface { + public: + PowerLimitController(); + + controller_interface::CallbackReturn on_init() override; + + controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + controller_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::return_type update_reference_from_subscribers() override; + + controller_interface::return_type + update_and_write_commands(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + // using ControllerStateMsg = control_msgs::msg::JointControllerState; + using MotorPowerMsg = device_interface::msg::MotorPower; + + private: + std::shared_ptr param_listener_; + power_limit::Params params_; + + + rclcpp::Duration ref_timeout_ = rclcpp::Duration(0, 0); + // rclcpp::Subscription::SharedPtr twist_sub_; + // realtime_tools::RealtimeBuffer> ref_buf_; + + // rclcpp::Subscription::SharedPtr chassis_sub_; + // realtime_tools::RealtimeBuffer> + // chassis_buf_; + + using MotorPowerPublisher = + realtime_tools::RealtimePublisher; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // override methods from ChainableControllerInterface + std::vector + on_export_reference_interfaces() override; + + bool on_set_chained_mode(bool chained_mode) override; +}; + +} // namespace power_limit + +#endif // POWER_LIMIT__POWER_LIMIT_HPP_ \ No newline at end of file diff --git a/decomposition/power_limit/package.xml b/decomposition/power_limit/package.xml new file mode 100644 index 0000000..ae4c333 --- /dev/null +++ b/decomposition/power_limit/package.xml @@ -0,0 +1,37 @@ + + + + power_limit + 0.0.0 + A controller to access im1253c power measuring component. + Feiyang Wu + MIT + + ament_cmake + + generate_parameter_library + + device_interface + + control_toolbox + + control_msgs + + controller_interface + + hardware_interface + + pluginlib + + rclcpp + + rclcpp_lifecycle + + realtime_tools + + std_srvs + + + ament_cmake + + \ No newline at end of file diff --git a/decomposition/power_limit/power_limit_controller.xml b/decomposition/power_limit/power_limit_controller.xml new file mode 100644 index 0000000..bb61863 --- /dev/null +++ b/decomposition/power_limit/power_limit_controller.xml @@ -0,0 +1,29 @@ + + + + + + The controller to read the power limit from im1253c. + + + \ No newline at end of file diff --git a/decomposition/power_limit/src/power_limit.cpp b/decomposition/power_limit/src/power_limit.cpp new file mode 100644 index 0000000..e2d83b3 --- /dev/null +++ b/decomposition/power_limit/src/power_limit.cpp @@ -0,0 +1,128 @@ +#include "power_limit/power_limit.hpp" + +#include +#include +#include +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" + +namespace power_limit { + +PowerLimitController::PowerLimitController() + : controller_interface::ChainableControllerInterface() {} + +controller_interface::CallbackReturn PowerLimitController::on_init() { + + try { + param_listener_ = + std::make_shared(get_node()); + } catch (const std::exception &e) { + fprintf(stderr, "Exception thrown during controller's init with message: %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +PowerLimitController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + params_ = param_listener_->get_params(); + + try { + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } catch (const std::exception &e) { + fprintf(stderr, + "Exception thrown during publisher creation at configure stage " + "with message : %s \n", + e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +PowerLimitController::command_interface_configuration() const { + controller_interface::InterfaceConfiguration command_interfaces_config; + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration +PowerLimitController::state_interface_configuration() const { + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = + controller_interface::interface_configuration_type::INDIVIDUAL; + + state_interfaces_config.names.reserve(3); + state_interfaces_config.names.push_back(params_.name + "/voltage"); + state_interfaces_config.names.push_back(params_.name + "/current"); + state_interfaces_config.names.push_back(params_.name + "/power"); + state_interfaces_config.names.push_back(params_.motor_joint + "/" + hardware_interface::HW_IF_VELOCITY); + state_interfaces_config.names.push_back(params_.motor_joint + "/" + hardware_interface::HW_IF_EFFORT); + return state_interfaces_config; +} + +std::vector +PowerLimitController::on_export_reference_interfaces() { + std::vector reference_interfaces; + return reference_interfaces; +} + +bool PowerLimitController::on_set_chained_mode(bool chained_mode) { + // Always accept switch to/from chained mode + return true || chained_mode; +} + +controller_interface::CallbackReturn +PowerLimitController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + // Set default value in command + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +PowerLimitController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type +PowerLimitController::update_reference_from_subscribers() { + return controller_interface::return_type::OK; +} + +controller_interface::return_type +PowerLimitController::update_and_write_commands(const rclcpp::Time &/*time*/, + const rclcpp::Duration &/*period*/) { + if (param_listener_->is_old(params_)) { + params_ = param_listener_->get_params(); + } + MotorPowerMsg msg; + + if (state_publisher_ && state_publisher_->trylock()) { + state_publisher_->msg_.voltage = state_interfaces_[0].get_value(); + state_publisher_->msg_.current = state_interfaces_[1].get_value(); + state_publisher_->msg_.power = state_interfaces_[2].get_value(); + state_publisher_->msg_.motor_velocity = state_interfaces_[3].get_value(); + state_publisher_->msg_.motor_effort = state_interfaces_[4].get_value(); + state_publisher_->unlockAndPublish(); + } + + + return controller_interface::return_type::OK; +} + + + +} // namespace meta_chassis_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(power_limit::PowerLimitController, + controller_interface::ChainableControllerInterface) diff --git a/decomposition/power_limit/src/power_limit.yaml b/decomposition/power_limit/src/power_limit.yaml new file mode 100644 index 0000000..1e1af0c --- /dev/null +++ b/decomposition/power_limit/src/power_limit.yaml @@ -0,0 +1,17 @@ +power_limit: + name: + { + type: string, + default_value: "", + description: "", + read_only: true, + validation: { not_empty<>: null }, + } + motor_joint: + { + type: string, + default_value: "", + description: "Specifies the the motor power to measure.", + read_only: true, + validation: { not_empty<>: null }, + } \ No newline at end of file diff --git a/interfaces/device_interface/msg/MotorPower.msg b/interfaces/device_interface/msg/MotorPower.msg new file mode 100644 index 0000000..66e79fd --- /dev/null +++ b/interfaces/device_interface/msg/MotorPower.msg @@ -0,0 +1,5 @@ +float64 voltage +float64 current +float64 power +float64 motor_velocity +float64 motor_effort diff --git a/meta_bringup/config/im1253c.yaml b/meta_bringup/config/im1253c.yaml new file mode 100644 index 0000000..db829b6 --- /dev/null +++ b/meta_bringup/config/im1253c.yaml @@ -0,0 +1,3 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz \ No newline at end of file diff --git a/meta_bringup/launch/im1253c.launch.py b/meta_bringup/launch/im1253c.launch.py new file mode 100644 index 0000000..d2d61d1 --- /dev/null +++ b/meta_bringup/launch/im1253c.launch.py @@ -0,0 +1,129 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +# Necessary dirty work that lets us import modules from the meta_bringup package +import os +import sys +from ament_index_python.packages import get_package_share_directory +sys.path.append(os.path.join(get_package_share_directory('meta_bringup'), 'launch')) + +from launch_utils import load_controller, register_loading_order, register_sequential_loading + +ARGUMENTS = [ + DeclareLaunchArgument( + 'enable_simulation', + default_value='false', + description='If true, the simulation will be started'), +] + +def generate_launch_description(): + # Launch Arguments + enable_simulation = LaunchConfiguration('enable_simulation') + + # Get URDF via xacro + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'im1253c.xacro']), + ' ', + 'is_simulation:=', enable_simulation, + ]) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[ + {'use_sim_time': enable_simulation}, + {'robot_description': robot_description_content},], + output='both', + emulate_tty=True + ) + + # Gazebo related launch + world_sdf = PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'worlds', 'empty_world.sdf']) + bridge_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'ros_gz_bridge.yaml']) + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'launch', 'meta_gazebo.launch.py'])], + ), + launch_arguments=[ + ('world_sdf', world_sdf), + ('robot_name', 'dm_motor'), + ('bridge_config_file', bridge_config), + ], + condition=IfCondition(enable_simulation) + ) + + # ROS2 Control related launch + robot_controller_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'im1253c.yaml']) + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controller_config], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output='both', + emulate_tty=True, + condition=UnlessCondition(enable_simulation) + ) + + load_joint_state_broadcaster = load_controller('joint_state_broadcaster') + + # List of controllers to be loaded sequentially + # Order in this list is IMPORTANT + load_controllers = [ + load_controller('forward_position_controller'), + ] + + motor_tester_node = Node( + package='motor_tester', + executable='motor_tester', + name='motor_tester_node', + parameters=[robot_controller_config], + ) + + dbus_control_node = Node( + package='dbus_control', + executable='dbus_control_node', + name='dbus_control_node', + parameters=[robot_controller_config], + ) + + return LaunchDescription([ + # Launch Arguments + *ARGUMENTS, + # Launch Gazebo and ROS2 bridge and spawn robot in Gazebo (also start controller manager) + gazebo_launch, + # Load robot state publisher + node_robot_state_publisher, + # Launch controller manager (if not in simulation) + controller_manager, + # Load joint state broadcaster + load_joint_state_broadcaster, + # Load controllers + *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), + motor_tester_node, + dbus_control_node, + ]) From a45db25b9fe45de9c9b775e294e3ebebf75a347b Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 30 Aug 2024 19:57:05 +0800 Subject: [PATCH 30/36] im1253c working --- .../urdf/playground/power_test.xacro | 78 ++++++++++++++++ decomposition/power_limit/CMakeLists.txt | 4 +- .../include/power_limit/power_limit.hpp | 14 +-- ...r_limit_controller.xml => power_limit.xml} | 8 +- decomposition/power_limit/src/power_limit.cpp | 35 +++----- execution/meta_hardware/CMakeLists.txt | 2 + .../modbus_rtu_driver/modbus_rtu_driver.hpp | 4 +- execution/meta_hardware/meta_hardware.xml | 7 ++ execution/meta_hardware/package.xml | 2 + .../meta_hardware/src/im1253c_interface.cpp | 11 ++- .../modbus_rtu_driver/modbus_rtu_driver.cpp | 90 +++++++++---------- interfaces/device_interface/CMakeLists.txt | 1 + meta_bringup/config/im1253c.yaml | 64 ++++++++++++- meta_bringup/launch/im1253c.launch.py | 16 ++-- 14 files changed, 229 insertions(+), 107 deletions(-) create mode 100644 decomposition/metav_description/urdf/playground/power_test.xacro rename decomposition/power_limit/{power_limit_controller.xml => power_limit.xml} (81%) diff --git a/decomposition/metav_description/urdf/playground/power_test.xacro b/decomposition/metav_description/urdf/playground/power_test.xacro new file mode 100644 index 0000000..cb331b0 --- /dev/null +++ b/decomposition/metav_description/urdf/playground/power_test.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + + + + meta_hardware/MetaRobotIm1253cManager + 4800 + none + none + 1.0 + /dev/ttyUSB0 + + + + + + + + + \ No newline at end of file diff --git a/decomposition/power_limit/CMakeLists.txt b/decomposition/power_limit/CMakeLists.txt index ff5bb7b..10c3d8d 100644 --- a/decomposition/power_limit/CMakeLists.txt +++ b/decomposition/power_limit/CMakeLists.txt @@ -27,7 +27,7 @@ target_link_libraries(power_limit power_limit_parameters) pluginlib_export_plugin_description_file( - controller_interface power_limit_controller.xml) + controller_interface power_limit.xml) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -41,4 +41,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_auto_package() diff --git a/decomposition/power_limit/include/power_limit/power_limit.hpp b/decomposition/power_limit/include/power_limit/power_limit.hpp index ea2d300..5763544 100644 --- a/decomposition/power_limit/include/power_limit/power_limit.hpp +++ b/decomposition/power_limit/include/power_limit/power_limit.hpp @@ -6,7 +6,8 @@ #include #include "device_interface/msg/motor_power.hpp" -#include "controller_interface/chainable_controller_interface.hpp" +// #include "controller_interface/chainable_controller_interface.hpp" +#include "controller_interface/controller_interface.hpp" #include "rclcpp/duration.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" @@ -25,7 +26,7 @@ enum class control_mode_type : std::uint8_t { CHASSIS_FOLLOW_GIMBAL = 2, }; -class PowerLimitController : public controller_interface::ChainableControllerInterface { +class PowerLimitController : public controller_interface::ControllerInterface { public: PowerLimitController(); @@ -46,10 +47,8 @@ class PowerLimitController : public controller_interface::ChainableControllerInt controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; - controller_interface::return_type update_reference_from_subscribers() override; - controller_interface::return_type - update_and_write_commands(const rclcpp::Time &time, + update(const rclcpp::Time &time, const rclcpp::Duration &period) override; // using ControllerStateMsg = control_msgs::msg::JointControllerState; @@ -74,11 +73,6 @@ class PowerLimitController : public controller_interface::ChainableControllerInt rclcpp::Publisher::SharedPtr s_publisher_; std::unique_ptr state_publisher_; - // override methods from ChainableControllerInterface - std::vector - on_export_reference_interfaces() override; - - bool on_set_chained_mode(bool chained_mode) override; }; } // namespace power_limit diff --git a/decomposition/power_limit/power_limit_controller.xml b/decomposition/power_limit/power_limit.xml similarity index 81% rename from decomposition/power_limit/power_limit_controller.xml rename to decomposition/power_limit/power_limit.xml index bb61863..7e89f4e 100644 --- a/decomposition/power_limit/power_limit_controller.xml +++ b/decomposition/power_limit/power_limit.xml @@ -18,10 +18,10 @@ Source of this file are templates in [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. --> - - + + The controller to read the power limit from im1253c. diff --git a/decomposition/power_limit/src/power_limit.cpp b/decomposition/power_limit/src/power_limit.cpp index e2d83b3..0fe9d12 100644 --- a/decomposition/power_limit/src/power_limit.cpp +++ b/decomposition/power_limit/src/power_limit.cpp @@ -1,5 +1,6 @@ #include "power_limit/power_limit.hpp" +#include #include #include #include @@ -12,7 +13,7 @@ namespace power_limit { PowerLimitController::PowerLimitController() - : controller_interface::ChainableControllerInterface() {} + : controller_interface::ControllerInterface() {} controller_interface::CallbackReturn PowerLimitController::on_init() { @@ -65,22 +66,11 @@ PowerLimitController::state_interface_configuration() const { state_interfaces_config.names.push_back(params_.name + "/voltage"); state_interfaces_config.names.push_back(params_.name + "/current"); state_interfaces_config.names.push_back(params_.name + "/power"); - state_interfaces_config.names.push_back(params_.motor_joint + "/" + hardware_interface::HW_IF_VELOCITY); - state_interfaces_config.names.push_back(params_.motor_joint + "/" + hardware_interface::HW_IF_EFFORT); + // state_interfaces_config.names.push_back(params_.motor_joint + "/" + hardware_interface::HW_IF_VELOCITY); + // state_interfaces_config.names.push_back(params_.motor_joint + "/" + hardware_interface::HW_IF_EFFORT); return state_interfaces_config; } -std::vector -PowerLimitController::on_export_reference_interfaces() { - std::vector reference_interfaces; - return reference_interfaces; -} - -bool PowerLimitController::on_set_chained_mode(bool chained_mode) { - // Always accept switch to/from chained mode - return true || chained_mode; -} - controller_interface::CallbackReturn PowerLimitController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { // Set default value in command @@ -93,28 +83,23 @@ PowerLimitController::on_deactivate(const rclcpp_lifecycle::State & /*previous_s } controller_interface::return_type -PowerLimitController::update_reference_from_subscribers() { - return controller_interface::return_type::OK; -} - -controller_interface::return_type -PowerLimitController::update_and_write_commands(const rclcpp::Time &/*time*/, +PowerLimitController::update(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) { if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); } - MotorPowerMsg msg; if (state_publisher_ && state_publisher_->trylock()) { state_publisher_->msg_.voltage = state_interfaces_[0].get_value(); state_publisher_->msg_.current = state_interfaces_[1].get_value(); state_publisher_->msg_.power = state_interfaces_[2].get_value(); - state_publisher_->msg_.motor_velocity = state_interfaces_[3].get_value(); - state_publisher_->msg_.motor_effort = state_interfaces_[4].get_value(); + // state_publisher_->msg_.motor_velocity = state_interfaces_[3].get_value(); + // state_publisher_->msg_.motor_effort = state_interfaces_[4].get_value(); + state_publisher_->msg_.motor_velocity = 0.0; + state_publisher_->msg_.motor_effort = 0.0; state_publisher_->unlockAndPublish(); } - return controller_interface::return_type::OK; } @@ -125,4 +110,4 @@ PowerLimitController::update_and_write_commands(const rclcpp::Time &/*time*/, #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(power_limit::PowerLimitController, - controller_interface::ChainableControllerInterface) + controller_interface::ControllerInterface) diff --git a/execution/meta_hardware/CMakeLists.txt b/execution/meta_hardware/CMakeLists.txt index 1e0de52..efa57b6 100644 --- a/execution/meta_hardware/CMakeLists.txt +++ b/execution/meta_hardware/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(angles REQUIRED) find_package(Boost REQUIRED) +find_package(serial_driver REQUIRED) add_library( meta_hardware @@ -46,6 +47,7 @@ ament_target_dependencies( rclcpp_lifecycle angles Boost + serial_driver ) # prevent pluginlib from using boost target_compile_definitions(meta_hardware PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp b/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp index b290c22..fc970d0 100644 --- a/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp +++ b/execution/meta_hardware/include/meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp @@ -18,7 +18,7 @@ class ModbusRtuDriver ~ModbusRtuDriver(); void set_command(int addr, int func, int reg, int len); - std::vector get_reg_data(){return reg_data_;}; + std::vector get_reg_data(){return reg_data_;}; private: std::unique_ptr owned_ctx_; @@ -33,7 +33,7 @@ class ModbusRtuDriver drivers::serial_driver::StopBits stop_bits_; std::vector command_; - std::vector reg_data_; + std::vector reg_data_; void rx_tx_loop(std::stop_token stop_token); auto awake_time(); diff --git a/execution/meta_hardware/meta_hardware.xml b/execution/meta_hardware/meta_hardware.xml index 5f9b833..d9c3eef 100644 --- a/execution/meta_hardware/meta_hardware.xml +++ b/execution/meta_hardware/meta_hardware.xml @@ -27,4 +27,11 @@ BRT encoder network hardware interface. + + + IM1253C manager hardware interface. + + \ No newline at end of file diff --git a/execution/meta_hardware/package.xml b/execution/meta_hardware/package.xml index bc5683f..8d43358 100644 --- a/execution/meta_hardware/package.xml +++ b/execution/meta_hardware/package.xml @@ -19,6 +19,8 @@ angles + serial_driver + ament_lint_auto ament_lint_common ament_cmake_gmock diff --git a/execution/meta_hardware/src/im1253c_interface.cpp b/execution/meta_hardware/src/im1253c_interface.cpp index 922427c..ef6e055 100644 --- a/execution/meta_hardware/src/im1253c_interface.cpp +++ b/execution/meta_hardware/src/im1253c_interface.cpp @@ -1,9 +1,11 @@ #include +#include #include #include #include #include #include +#include #include #include "hardware_interface/hardware_info.hpp" @@ -29,6 +31,7 @@ MetaRobotIm1253cManager::on_init(const hardware_interface::HardwareInfo &info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } + sensor_interface_data_.resize(info_.sensors.size()); return CallbackReturn::SUCCESS; } @@ -96,13 +99,17 @@ hardware_interface::CallbackReturn MetaRobotIm1253cManager::on_deactivate( hardware_interface::return_type MetaRobotIm1253cManager::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - return hardware_interface::return_type::OK; for(size_t i = 0; i < info_.sensors.size(); i++){ - std::vector sensor_data_raw = device_[i]->get_reg_data(); + std::vector sensor_data_raw = device_[i]->get_reg_data(); sensor_interface_data_[i].state_voltage = static_cast(sensor_data_raw[0])/100.0; sensor_interface_data_[i].state_current = static_cast(sensor_data_raw[1])/100.0; sensor_interface_data_[i].state_power = static_cast(sensor_data_raw[2]); + // std::cout << "Voltage: " << sensor_interface_data_[i].state_voltage << std::endl; + // std::cout << "Current: " << sensor_interface_data_[i].state_current << std::endl; + // std::cout << "Power: " << sensor_interface_data_[i].state_power << std::endl; } + + return hardware_interface::return_type::OK; } hardware_interface::return_type diff --git a/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp b/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp index 8719e01..fb38e4c 100644 --- a/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp +++ b/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp @@ -1,11 +1,14 @@ #include "meta_hardware/modbus_rtu_driver/modbus_rtu_driver.hpp" +#include #include +#include #include #include #include #include #include +#include #include ModbusRtuDriver::ModbusRtuDriver(std::unordered_map serial_params): @@ -26,6 +29,7 @@ ModbusRtuDriver::ModbusRtuDriver(std::unordered_map se receive_thread_ = std::make_unique([this](std::stop_token s) { rx_tx_loop(s); }); } } catch (const std::exception & ex) { + std::cerr << "Error creating serial port: " << device_name_ << " - " << ex.what() << std::endl; throw ex; } @@ -68,14 +72,14 @@ void ModbusRtuDriver::read_params(std::unordered_map s throw std::runtime_error("Unknown stop bits(Please Use Floating Point Notation): " + std::to_string(sb)); } - std::string device_name = serial_params.at("device_name"); + device_name_ = serial_params.at("device_name"); }; auto ModbusRtuDriver::awake_time() { using std::chrono::operator""ms; - return std::chrono::steady_clock::now() + 2ms; + return std::chrono::steady_clock::now() + 10ms; } void ModbusRtuDriver::set_command(int addr, int func, int reg, int len){ @@ -85,69 +89,55 @@ void ModbusRtuDriver::set_command(int addr, int func, int reg, int len){ command_[3] = static_cast(reg & 0xFF); command_[4] = static_cast(len >> 8); command_[5] = static_cast(len & 0xFF); - boost::crc_16_type crc_16; + boost::crc_optimal<16, 0x8005, 0xFFFF, 0, true, true> crc_16; crc_16.process_bytes(command_.data(), 6); int crc = crc_16.checksum(); command_[6] = static_cast(crc & 0xFF); command_[7] = static_cast(crc >> 8); + reg_data_.resize(len); } void ModbusRtuDriver::rx_tx_loop(std::stop_token stop_token){ - std::vector address(1); - std::vector response(2); - std::vector data; - std::vector response_crc(2); - std::vector response_without_crc; + int reg_num = static_cast(command_[4] << 8 | command_[5]); + int response_size = reg_num * 2 + 5; + + std::cout << "Reponse Size:" << response_size << std::endl; + while (!stop_token.stop_requested()) { - std::this_thread::sleep_until(awake_time()); + auto start_time = std::chrono::steady_clock::now(); + // std::cout << "Sending Command..." << std::endl; // Send Command with blocking serial_driver_->port()->send(command_); - // Receive Response with blocking - - response_without_crc.clear(); - serial_driver_->port()->receive(address); - response_without_crc.emplace_back(address[0]); - if( address[0] == command_[0]){ - serial_driver_->port()->receive(address); - serial_driver_->port()->receive(response); - uint8_t func_code = response[0]; - if(func_code != command_[1]){ - std::cout << " Function Code Mismatch. Expected: " - << command_[1] << " Received: " << func_code << std::endl; - continue; - }else if(func_code == 0x03){ - uint8_t byte_count = response[1]; - data.resize(byte_count); - - serial_driver_->port()->receive(data); - - response_without_crc.emplace_back(func_code); - response_without_crc.emplace_back(byte_count); - for(uint8_t i = 0; i < data.size(); i++){ - response_without_crc.emplace_back(data[i]); - } - - boost::crc_16_type crc_16; - crc_16.process_bytes(response_without_crc.data(), response_without_crc.size()); - int calc_crc = crc_16.checksum(); - serial_driver_->port()->receive(response_crc); - if((static_cast(response_crc[0]) + (static_cast(response_crc[1]) << 8))== calc_crc){ - reg_data_ = data; - }else{ - std::cout << " CRC Mismatch. " << std::endl; - } - }else if(func_code == 0x10){ - std::vector write_response(6); - // get write response - serial_driver_->port()->receive(write_response); - // TODO: Check if the response is the same as the command - } + + + using std::chrono::operator""ms; + std::this_thread::sleep_for(100ms); + std::vector response(response_size); + serial_driver_->port()->receive(response); + + if(response[0] == command_[0] && response[1] == command_[1] && response[2] == (uint8_t)reg_num*2){ + // TODO: Only Implementing Reading Mode; Wrting Mode Not Implemented. + for(int i = 0; i < reg_num; i++){ + reg_data_[i] = static_cast(response[3 + i * 2] << 8 | response[4 + i * 2]); + } + boost::crc_optimal<16, 0x8005, 0xFFFF, 0, true, true> crc_16; + crc_16.process_bytes(response.data(), response_size - 2); + int crc = crc_16.checksum(); + if((crc & 0xFF) != response[response_size - 2] || (crc >> 8) != response[response_size - 1]){ + std::cout << "CRC Checked Failure. " << std::endl; + } }else{ - std::cout << " Device Address Not Found. Device Address: " << address[0] << std::endl; + std::cout << "Feedback Format Error." << std::endl; } + // for(size_t i = 0; i < response.size(); i++){ + // std::cout << std::hex << static_cast(response[i]) << " "; + // } + // std::cout << std::endl; + + std::this_thread::sleep_until(start_time + 250ms); } } diff --git a/interfaces/device_interface/CMakeLists.txt b/interfaces/device_interface/CMakeLists.txt index 46c2b75..1c187ed 100644 --- a/interfaces/device_interface/CMakeLists.txt +++ b/interfaces/device_interface/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorGoal.msg" "msg/MotorState.msg" + "msg/MotorPower.msg" "msg/AngularPosition.msg" "msg/Relay.msg" "srv/CapacitorState.srv" diff --git a/meta_bringup/config/im1253c.yaml b/meta_bringup/config/im1253c.yaml index db829b6..bf278ec 100644 --- a/meta_bringup/config/im1253c.yaml +++ b/meta_bringup/config/im1253c.yaml @@ -1,3 +1,65 @@ controller_manager: ros__parameters: - update_rate: 250 # Hz \ No newline at end of file + update_rate: 250 # Hz + + power_limit: + type: power_limit/PowerLimitController + + # forward_position_controller: + # type: forward_command_controller/ForwardCommandController + + # motor_pos2vel_pid_controller: + # type: pid_controller/PidController + + # motor_vel2eff_pid_controller: + # type: pid_controller/PidController + + # joint_state_broadcaster: + # type: joint_state_broadcaster/JointStateBroadcaster + +power_limit: + ros__parameters: + name: im1253c + motor_joint: motor_joint + +# forward_position_controller: +# ros__parameters: +# joints: +# - motor_pos2vel_pid_controller/motor_joint +# interface_name: position + +# motor_pos2vel_pid_controller: +# ros__parameters: +# dof_names: +# - motor_vel2eff_pid_controller/motor_joint + +# reference_and_state_dof_names: +# - motor_joint + +# command_interface: velocity + +# reference_and_state_interfaces: ["position"] + +# gains: +# motor_vel2eff_pid_controller/motor_joint: +# { p: 10.0, i: 0.0, d: 0.0, angle_wraparound: true } + +# motor_vel2eff_pid_controller: +# ros__parameters: +# dof_names: +# - motor_joint + +# command_interface: effort + +# reference_and_state_interfaces: ["velocity"] + +# gains: +# motor_joint: +# { +# p: 1.0e-2, +# i: 3.0e-1, +# d: 0.0, +# i_clamp_max: 1.0, +# i_clamp_min: -1.0, +# antiwindup: true, +# } \ No newline at end of file diff --git a/meta_bringup/launch/im1253c.launch.py b/meta_bringup/launch/im1253c.launch.py index d2d61d1..55e756b 100644 --- a/meta_bringup/launch/im1253c.launch.py +++ b/meta_bringup/launch/im1253c.launch.py @@ -45,7 +45,7 @@ def generate_launch_description(): robot_description_content = Command([ PathJoinSubstitution([FindExecutable(name='xacro')]), ' ', - PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'im1253c.xacro']), + PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'power_test.xacro']), ' ', 'is_simulation:=', enable_simulation, ]) @@ -69,7 +69,7 @@ def generate_launch_description(): ), launch_arguments=[ ('world_sdf', world_sdf), - ('robot_name', 'dm_motor'), + ('robot_name', 'im1253c_test'), ('bridge_config_file', bridge_config), ], condition=IfCondition(enable_simulation) @@ -94,7 +94,8 @@ def generate_launch_description(): # List of controllers to be loaded sequentially # Order in this list is IMPORTANT load_controllers = [ - load_controller('forward_position_controller'), + load_controller('power_limit'), + # load_controller('forward_position_controller'), ] motor_tester_node = Node( @@ -104,13 +105,6 @@ def generate_launch_description(): parameters=[robot_controller_config], ) - dbus_control_node = Node( - package='dbus_control', - executable='dbus_control_node', - name='dbus_control_node', - parameters=[robot_controller_config], - ) - return LaunchDescription([ # Launch Arguments *ARGUMENTS, @@ -125,5 +119,5 @@ def generate_launch_description(): # Load controllers *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), motor_tester_node, - dbus_control_node, + # dbus_control_node, ]) From ffcd59aa41eed53eeaf4b630e86b3f1c6f7f1b30 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Thu, 5 Sep 2024 18:57:58 +0800 Subject: [PATCH 31/36] shoot controller added --- decomposition/shoot_controller/CMakeLists.txt | 35 +++++++++ decomposition/shoot_controller/LICENSE | 17 +++++ .../shoot_controller/shoot_controller.hpp | 72 +++++++++++++++++++ decomposition/shoot_controller/package.xml | 29 ++++++++ .../shoot_controller/shoot_controller.xml | 29 ++++++++ .../shoot_controller/src/shoot_controller.cpp | 42 +++++++++++ interfaces/behavior_interface/msg/Shoot.msg | 3 +- 7 files changed, 226 insertions(+), 1 deletion(-) create mode 100644 decomposition/shoot_controller/CMakeLists.txt create mode 100644 decomposition/shoot_controller/LICENSE create mode 100644 decomposition/shoot_controller/include/shoot_controller/shoot_controller.hpp create mode 100644 decomposition/shoot_controller/package.xml create mode 100644 decomposition/shoot_controller/shoot_controller.xml create mode 100644 decomposition/shoot_controller/src/shoot_controller.cpp diff --git a/decomposition/shoot_controller/CMakeLists.txt b/decomposition/shoot_controller/CMakeLists.txt new file mode 100644 index 0000000..86073a4 --- /dev/null +++ b/decomposition/shoot_controller/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(shoot_controller) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library( + ${PROJECT_NAME} + SHARED + src/shoot_controller.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/decomposition/shoot_controller/LICENSE b/decomposition/shoot_controller/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/decomposition/shoot_controller/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/decomposition/shoot_controller/include/shoot_controller/shoot_controller.hpp b/decomposition/shoot_controller/include/shoot_controller/shoot_controller.hpp new file mode 100644 index 0000000..9477ed1 --- /dev/null +++ b/decomposition/shoot_controller/include/shoot_controller/shoot_controller.hpp @@ -0,0 +1,72 @@ +#ifndef SHOOT_CONTROLLER__SHOOT_CONTROLLER_HPP_ +#define SHOOT_CONTROLLER__SHOOT_CONTROLLER_HPP_ + +#include +#include + +#include "control_toolbox/pid_ros.hpp" +#include "controller_interface/chainable_controller_interface.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_publisher.h" + + +#include "control_msgs/msg/joint_controller_state.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "behavior_interface/msg/shoot.hpp" +#include +#include + +namespace meta_shoot_controller { + class ShootController : public controller_interface::ChainableControllerInterface { + public: + ShootController(); + + controller_interface::CallbackReturn + on_init() override; + + controller_interface::InterfaceConfiguration + command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration + state_interface_configuration() const override; + + controller_interface::CallbackReturn + on_configure(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_activate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + + controller_interface::return_type update_reference_from_subscribers() override; + + controller_interface::return_type + update_and_write_commands(const rclcpp::Time &time, + const rclcpp::Duration &period) override; + + protected: + // override methods from ChainableControllerInterface + std::vector + on_export_reference_interfaces() override; + bool on_set_chained_mode(bool chained_mode) override; + + private: + std::shared_ptr fric1_vel_pid_; + std::shared_ptr fric2_vel_pid_; + std::shared_ptr load_vel_pid_; + + using ControllerReferenceMsg = behavior_interface::msg::Shoot; + + rclcpp::Subscription::SharedPtr ref_subscriber_; + realtime_tools::RealtimeBuffer> ref_buf_; + + void reference_callback(const std::shared_ptr msg); + void feedback_callback(const std::shared_ptr motor_feedback); + }; +} +#endif // SHOOT_CONTROLLER__SHOOT_CONTROLLER_HPP_ \ No newline at end of file diff --git a/decomposition/shoot_controller/package.xml b/decomposition/shoot_controller/package.xml new file mode 100644 index 0000000..4afe0e3 --- /dev/null +++ b/decomposition/shoot_controller/package.xml @@ -0,0 +1,29 @@ + + + + shoot_controller + 0.0.0 + A ros2-control compatible shooting controller for Meta-Team robots gimbal shooting functionality. + Feiyang Wu + MIT + + ament_cmake + generate_parameter_library + + behavior_interface + control_toolbox + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/decomposition/shoot_controller/shoot_controller.xml b/decomposition/shoot_controller/shoot_controller.xml new file mode 100644 index 0000000..9468d30 --- /dev/null +++ b/decomposition/shoot_controller/shoot_controller.xml @@ -0,0 +1,29 @@ + + + + + + Shoot Controller ros2_control controller. + + + \ No newline at end of file diff --git a/decomposition/shoot_controller/src/shoot_controller.cpp b/decomposition/shoot_controller/src/shoot_controller.cpp new file mode 100644 index 0000000..488af44 --- /dev/null +++ b/decomposition/shoot_controller/src/shoot_controller.cpp @@ -0,0 +1,42 @@ +#include "shoot_controller/shoot_controller.hpp" +#include + + +using hardware_interface::HW_IF_VELOCITY; + +namespace meta_shoot_controller { + ShootController::ShootController() { + // Empty constructor + } + + + controller_interface::CallbackReturn + ShootController::on_init() { + // Empty on_init + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ShootController::on_configure(const rclcpp_lifecycle::State &previous_state){ + // Empty on_configure + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ShootController::on_activate(const rclcpp_lifecycle::State &previous_state){ + // Empty on_activate + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ShootController::on_deactivate(const rclcpp_lifecycle::State &previous_state){ + // Empty on_deactivate + return controller_interface::CallbackReturn::SUCCESS; + } + + // controller_interface::InterfaceConfiguration + // ShootController::command_interface_configuration() const{ + // // return controller_interface::InterfaceConfiguration(); + // } + +} \ No newline at end of file diff --git a/interfaces/behavior_interface/msg/Shoot.msg b/interfaces/behavior_interface/msg/Shoot.msg index c7141fc..c3ed3ca 100644 --- a/interfaces/behavior_interface/msg/Shoot.msg +++ b/interfaces/behavior_interface/msg/Shoot.msg @@ -1,2 +1,3 @@ bool fric_state # on or off -bool feed_state # on or off \ No newline at end of file +bool feed_state # on or off +float64 feed_speed # feed speed \ No newline at end of file From 017261948a546d2cca91507a319a0b9c4983abd5 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 6 Sep 2024 19:51:23 +0800 Subject: [PATCH 32/36] first version of the shoot controller finished --- .../CMakeLists.txt | 9 +- .../LICENSE | 0 .../shoot_controller/shoot_controller.hpp | 40 ++- .../package.xml | 3 +- .../shoot_controller.xml | 0 .../src/meta_shoot_controller.yaml | 21 ++ .../src/shoot_controller.cpp | 249 ++++++++++++++++++ .../shoot_controller/src/shoot_controller.cpp | 42 --- 8 files changed, 311 insertions(+), 53 deletions(-) rename decomposition/{shoot_controller => meta_shoot_controller}/CMakeLists.txt (84%) rename decomposition/{shoot_controller => meta_shoot_controller}/LICENSE (100%) rename decomposition/{shoot_controller => meta_shoot_controller}/include/shoot_controller/shoot_controller.hpp (72%) rename decomposition/{shoot_controller => meta_shoot_controller}/package.xml (91%) rename decomposition/{shoot_controller => meta_shoot_controller}/shoot_controller.xml (100%) create mode 100644 decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml create mode 100644 decomposition/meta_shoot_controller/src/shoot_controller.cpp delete mode 100644 decomposition/shoot_controller/src/shoot_controller.cpp diff --git a/decomposition/shoot_controller/CMakeLists.txt b/decomposition/meta_shoot_controller/CMakeLists.txt similarity index 84% rename from decomposition/shoot_controller/CMakeLists.txt rename to decomposition/meta_shoot_controller/CMakeLists.txt index 86073a4..58e2766 100644 --- a/decomposition/shoot_controller/CMakeLists.txt +++ b/decomposition/meta_shoot_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(shoot_controller) +project(meta_shoot_controller) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") add_compile_options(-Wall -Wextra -Werror=conversion -Werror=unused-but-set-variable -Werror=return-type -Werror=shadow) @@ -10,6 +10,10 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +generate_parameter_library(${PROJECT_NAME}_parameters + src/meta_shoot_controller.yaml +) + ament_auto_add_library( ${PROJECT_NAME} SHARED @@ -19,6 +23,7 @@ ament_auto_add_library( target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_parameters) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -32,4 +37,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_auto_package() diff --git a/decomposition/shoot_controller/LICENSE b/decomposition/meta_shoot_controller/LICENSE similarity index 100% rename from decomposition/shoot_controller/LICENSE rename to decomposition/meta_shoot_controller/LICENSE diff --git a/decomposition/shoot_controller/include/shoot_controller/shoot_controller.hpp b/decomposition/meta_shoot_controller/include/shoot_controller/shoot_controller.hpp similarity index 72% rename from decomposition/shoot_controller/include/shoot_controller/shoot_controller.hpp rename to decomposition/meta_shoot_controller/include/shoot_controller/shoot_controller.hpp index 9477ed1..5a16a38 100644 --- a/decomposition/shoot_controller/include/shoot_controller/shoot_controller.hpp +++ b/decomposition/meta_shoot_controller/include/shoot_controller/shoot_controller.hpp @@ -3,6 +3,8 @@ #include #include +#include +#include #include "control_toolbox/pid_ros.hpp" #include "controller_interface/chainable_controller_interface.hpp" @@ -14,16 +16,18 @@ #include "control_msgs/msg/joint_controller_state.hpp" +#include "control_msgs/msg/multi_dof_state_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "behavior_interface/msg/shoot.hpp" -#include -#include +#include "meta_shoot_controller_parameters.hpp" -namespace meta_shoot_controller { + + +namespace shoot_controller { class ShootController : public controller_interface::ChainableControllerInterface { public: - ShootController(); + ShootController()=default; controller_interface::CallbackReturn on_init() override; @@ -48,25 +52,45 @@ namespace meta_shoot_controller { controller_interface::return_type update_and_write_commands(const rclcpp::Time &time, const rclcpp::Duration &period) override; - + protected: // override methods from ChainableControllerInterface std::vector - on_export_reference_interfaces() override; + on_export_reference_interfaces() override; bool on_set_chained_mode(bool chained_mode) override; + private: + using ControllerReferenceMsg = behavior_interface::msg::Shoot; + using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped; + using ControllerStatePublisher = realtime_tools::RealtimePublisher; + + double friction_wheel_speed_; + bool friction_wheel_on_; + bool bullet_loader_on_; + std::shared_ptr fric1_vel_pid_; std::shared_ptr fric2_vel_pid_; std::shared_ptr load_vel_pid_; + + std::shared_ptr param_listener_; + shoot_controller::Params params_; - using ControllerReferenceMsg = behavior_interface::msg::Shoot; + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + // Command subscribers rclcpp::Subscription::SharedPtr ref_subscriber_; - realtime_tools::RealtimeBuffer> ref_buf_; + realtime_tools::RealtimeBuffer> input_ref_; void reference_callback(const std::shared_ptr msg); void feedback_callback(const std::shared_ptr motor_feedback); + + void reset_controller_reference_msg(const std::shared_ptr &msg, + const std::shared_ptr & node); + + + }; } #endif // SHOOT_CONTROLLER__SHOOT_CONTROLLER_HPP_ \ No newline at end of file diff --git a/decomposition/shoot_controller/package.xml b/decomposition/meta_shoot_controller/package.xml similarity index 91% rename from decomposition/shoot_controller/package.xml rename to decomposition/meta_shoot_controller/package.xml index 4afe0e3..bbc601a 100644 --- a/decomposition/shoot_controller/package.xml +++ b/decomposition/meta_shoot_controller/package.xml @@ -1,13 +1,14 @@ - shoot_controller + meta_shoot_controller 0.0.0 A ros2-control compatible shooting controller for Meta-Team robots gimbal shooting functionality. Feiyang Wu MIT ament_cmake + ament_cmake_auto generate_parameter_library behavior_interface diff --git a/decomposition/shoot_controller/shoot_controller.xml b/decomposition/meta_shoot_controller/shoot_controller.xml similarity index 100% rename from decomposition/shoot_controller/shoot_controller.xml rename to decomposition/meta_shoot_controller/shoot_controller.xml diff --git a/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml b/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml new file mode 100644 index 0000000..6d49bcc --- /dev/null +++ b/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml @@ -0,0 +1,21 @@ +shoot_controller: + friction_wheel: + velocity: + { + type: double, + default_value: 0.0, + description: "Specifies the velocity of the friction wheel set.", + } + inverse: + { + type: bool, + default_value: false, + description: "Specifies whether the friction wheel set is inverse.", + } + bullet_loader: + velocity: + { + type: double, + default_value: 0.0, + description: "Specifies the velocity of the friction wheel.", + } diff --git a/decomposition/meta_shoot_controller/src/shoot_controller.cpp b/decomposition/meta_shoot_controller/src/shoot_controller.cpp new file mode 100644 index 0000000..d20bd43 --- /dev/null +++ b/decomposition/meta_shoot_controller/src/shoot_controller.cpp @@ -0,0 +1,249 @@ +#include "shoot_controller/shoot_controller.hpp" +#include +#include +#include + + +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::HW_IF_EFFORT; + +constexpr double NaN = std::numeric_limits::quiet_NaN(); + + + +namespace shoot_controller { + + controller_interface::CallbackReturn + ShootController::on_init() { + friction_wheel_on_ = false; + bullet_loader_on_ = false; + try { + param_listener_ = std::make_shared(get_node()); + } catch (const std::exception &e) { + std::cerr << "Exception thrown during controller's init with message: " + << e.what() << std::endl; + return controller_interface::CallbackReturn::ERROR; + } + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ShootController::on_configure(const rclcpp_lifecycle::State &previous_state){ + params_ = param_listener_->get_params(); + friction_wheel_speed_ = params_.friction_wheel.velocity; + + fric1_vel_pid_ = std::make_shared( + get_node(), "gains.friction_wheel1_vel", true + ); + fric2_vel_pid_ = std::make_shared( + get_node(), "gains.friction_wheel2_vel", true + ); + load_vel_pid_ = std::make_shared( + get_node(), "gains.load_vel", true + ); + + if (!fric1_vel_pid_->initPid()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for friction wheel 1"); + return controller_interface::CallbackReturn::FAILURE; + } + if (!fric2_vel_pid_->initPid()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for friction wheel 2"); + return controller_interface::CallbackReturn::FAILURE; + } + if (!load_vel_pid_->initPid()) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to initialize PID for bullet load"); + return controller_interface::CallbackReturn::FAILURE; + } + + // topics QoS + auto subscribers_qos = rclcpp::SystemDefaultsQoS(); + subscribers_qos.keep_last(1); + subscribers_qos.best_effort(); + + // Reference Subscriber + ref_subscriber_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&ShootController::reference_callback, this, std::placeholders::_1)); + + auto msg = std::make_shared(); + reset_controller_reference_msg(msg, get_node()); + input_ref_.writeFromNonRT(msg); + + try { + // State publisher + s_publisher_ = get_node()->create_publisher( + "~/controller_state", rclcpp::SystemDefaultsQoS()); + state_publisher_ = std::make_unique(s_publisher_); + } catch (const std::exception &e) { + std::cerr << "Exception thrown during publisher creation at configure " + "stage with message: " + << e.what() << std::endl; + return controller_interface::CallbackReturn::ERROR; + } + + state_publisher_->lock(); + state_publisher_->msg_.dof_states.resize(3); + state_publisher_->msg_.dof_states[0].name = "friction1_vel"; + state_publisher_->msg_.dof_states[1].name = "friction2_vel"; + state_publisher_->msg_.dof_states[2].name = "load_vel"; + state_publisher_->unlock(); + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ShootController::on_activate(const rclcpp_lifecycle::State &previous_state){ + reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); + + reference_interfaces_.assign(reference_interfaces_.size(), NaN); + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn + ShootController::on_deactivate(const rclcpp_lifecycle::State &previous_state){ + return controller_interface::CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration + ShootController::command_interface_configuration() const{ + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = + controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.reserve(3); + command_interfaces_config.names.push_back("friction_wheel1/" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back("friction_wheel2/" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back( "bullet_loader" + std::string(HW_IF_EFFORT)); + + return command_interfaces_config; + } + + controller_interface::InterfaceConfiguration + ShootController::state_interface_configuration() const{ + + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = + controller_interface::interface_configuration_type::INDIVIDUAL; + + // Joint position state of yaw gimbal is required + state_interfaces_config.names.reserve(3); + state_interfaces_config.names.push_back("friction_wheel1/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("friction_wheel2/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("bullet_loader/" + std::string(HW_IF_VELOCITY)); + + return state_interfaces_config; + } + + void ShootController::reset_controller_reference_msg( + const std::shared_ptr &msg, + const std::shared_ptr & /*node*/) { + msg->fric_state = false; + msg->fric_state = false; + msg->feed_speed = 0.0; + } + + controller_interface::return_type ShootController::update_reference_from_subscribers(){ + auto current_ref = *(input_ref_.readFromRT()); // A shared_ptr must be allocated + // immediately to prevent dangling + if (!std::isnan(current_ref->feed_speed)) { + if(current_ref->fric_state){ + reference_interfaces_[0] = friction_wheel_speed_; + friction_wheel_on_ = true; + }else{ + reference_interfaces_[0] = 0.0; + friction_wheel_on_ = false; + } + + if(current_ref->feed_state){ + reference_interfaces_[1] = current_ref->feed_speed; + bullet_loader_on_ = true; + }else{ + reference_interfaces_[1] = 0.0; + bullet_loader_on_ = false; + } + } + return controller_interface::return_type::OK; + } + + controller_interface::return_type + ShootController::update_and_write_commands(const rclcpp::Time & time , + const rclcpp::Duration & period ){ + + double fric1_vel_ref = NaN, fric2_vel_ref = NaN, load_vel_ref = NaN; + double fric1_vel_fb, fric2_vel_fb, load_vel_fb; + double fric1_vel_err = NaN, fric2_vel_err = NaN, load_vel_err = NaN; + double fric1_eff_cmd = NaN, fric2_eff_cmd = NaN, load_eff_cmd = NaN; + + fric1_vel_ref = reference_interfaces_[0]; + fric2_vel_ref = reference_interfaces_[0]; + load_vel_ref = reference_interfaces_[1]; + + fric1_vel_fb = state_interfaces_[0].get_value(); + fric2_vel_fb = state_interfaces_[1].get_value(); + load_vel_fb = state_interfaces_[2].get_value(); + + fric1_vel_err = fric1_vel_ref - fric1_vel_fb; + fric1_vel_err = fric2_vel_ref - fric2_vel_fb; + load_vel_err = load_vel_ref - load_vel_fb; + + fric1_eff_cmd = fric1_vel_pid_->computeCommand(fric1_vel_err, period); + fric2_eff_cmd = fric2_vel_pid_->computeCommand(fric2_vel_err, period); + load_eff_cmd = load_vel_pid_->computeCommand(load_vel_err, period); + + command_interfaces_[0].set_value(fric1_vel_pid_->computeCommand(fric1_vel_err, period)); + command_interfaces_[1].set_value(fric2_vel_pid_->computeCommand(fric2_vel_err, period)); + command_interfaces_[2].set_value(load_vel_pid_->computeCommand(load_vel_err, period)); + + // Publish state + if (state_publisher_ && state_publisher_->trylock()) { + state_publisher_->msg_.header.stamp = time; + if (friction_wheel_on_) { + state_publisher_->msg_.dof_states[0].reference = fric1_vel_ref; + state_publisher_->msg_.dof_states[0].feedback = fric1_vel_fb; + state_publisher_->msg_.dof_states[0].error = fric1_vel_err; + state_publisher_->msg_.dof_states[0].time_step = period.seconds(); + state_publisher_->msg_.dof_states[0].output = fric1_eff_cmd; + + state_publisher_->msg_.dof_states[1].reference = fric2_vel_ref; + state_publisher_->msg_.dof_states[1].feedback = fric2_vel_fb; + state_publisher_->msg_.dof_states[1].error = fric2_vel_err; + state_publisher_->msg_.dof_states[1].time_step = period.seconds(); + state_publisher_->msg_.dof_states[1].output = fric2_eff_cmd; + } + + if (bullet_loader_on_) { + state_publisher_->msg_.dof_states[2].reference = load_vel_ref; + state_publisher_->msg_.dof_states[2].feedback = load_vel_fb; + state_publisher_->msg_.dof_states[2].error = load_vel_err; + state_publisher_->msg_.dof_states[2].time_step = period.seconds(); + state_publisher_->msg_.dof_states[2].output = load_eff_cmd; + } + + state_publisher_->unlockAndPublish(); + } + return controller_interface::return_type::OK; + } + + std::vector + ShootController::on_export_reference_interfaces() { + reference_interfaces_.resize(3, NaN); + + std::vector reference_interfaces; + reference_interfaces.reserve(reference_interfaces_.size()); + + reference_interfaces.emplace_back(get_node()->get_name(), + std::string("friction_wheel/") + HW_IF_EFFORT, + &reference_interfaces_[0]); + + reference_interfaces.emplace_back(get_node()->get_name(), + std::string("bullet_loader/") + HW_IF_EFFORT, + &reference_interfaces_[1]); + return reference_interfaces; + } + + void ShootController::reference_callback( + const std::shared_ptr msg) { + input_ref_.writeFromNonRT(msg); + } +} \ No newline at end of file diff --git a/decomposition/shoot_controller/src/shoot_controller.cpp b/decomposition/shoot_controller/src/shoot_controller.cpp deleted file mode 100644 index 488af44..0000000 --- a/decomposition/shoot_controller/src/shoot_controller.cpp +++ /dev/null @@ -1,42 +0,0 @@ -#include "shoot_controller/shoot_controller.hpp" -#include - - -using hardware_interface::HW_IF_VELOCITY; - -namespace meta_shoot_controller { - ShootController::ShootController() { - // Empty constructor - } - - - controller_interface::CallbackReturn - ShootController::on_init() { - // Empty on_init - return controller_interface::CallbackReturn::SUCCESS; - } - - controller_interface::CallbackReturn - ShootController::on_configure(const rclcpp_lifecycle::State &previous_state){ - // Empty on_configure - return controller_interface::CallbackReturn::SUCCESS; - } - - controller_interface::CallbackReturn - ShootController::on_activate(const rclcpp_lifecycle::State &previous_state){ - // Empty on_activate - return controller_interface::CallbackReturn::SUCCESS; - } - - controller_interface::CallbackReturn - ShootController::on_deactivate(const rclcpp_lifecycle::State &previous_state){ - // Empty on_deactivate - return controller_interface::CallbackReturn::SUCCESS; - } - - // controller_interface::InterfaceConfiguration - // ShootController::command_interface_configuration() const{ - // // return controller_interface::InterfaceConfiguration(); - // } - -} \ No newline at end of file From 1b0eb62e4ce81c863307886fa37fec7a1e56ecb1 Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Wed, 11 Sep 2024 12:00:34 +0800 Subject: [PATCH 33/36] launch file created for shooter --- .../src/shoot_controller.cpp | 7 +- .../urdf/playground/shooter.xacro | 78 +++++++++++ meta_bringup/config/shooter.yaml | 19 +++ meta_bringup/launch/shooter.launch.py | 121 ++++++++++++++++++ 4 files changed, 224 insertions(+), 1 deletion(-) create mode 100644 decomposition/metav_description/urdf/playground/shooter.xacro create mode 100644 meta_bringup/config/shooter.yaml create mode 100644 meta_bringup/launch/shooter.launch.py diff --git a/decomposition/meta_shoot_controller/src/shoot_controller.cpp b/decomposition/meta_shoot_controller/src/shoot_controller.cpp index d20bd43..651adf1 100644 --- a/decomposition/meta_shoot_controller/src/shoot_controller.cpp +++ b/decomposition/meta_shoot_controller/src/shoot_controller.cpp @@ -246,4 +246,9 @@ namespace shoot_controller { const std::shared_ptr msg) { input_ref_.writeFromNonRT(msg); } -} \ No newline at end of file +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(shoot_controller::ShootController, + controller_interface::ChainableControllerInterface) \ No newline at end of file diff --git a/decomposition/metav_description/urdf/playground/shooter.xacro b/decomposition/metav_description/urdf/playground/shooter.xacro new file mode 100644 index 0000000..9084c15 --- /dev/null +++ b/decomposition/metav_description/urdf/playground/shooter.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + meta_hardware/MetaRobotDjiMotorNetwork + can0 + + + + + + + + M3508 + 1 + 1.0 + 0.0 + + + + + + + M3508 + 2 + 1.0 + 0.0 + + + + + + + M2006 + 3 + 1.0 + 0.0 + + + + + + + $(find meta_bringup)/config/single_motor.yaml + + + + \ No newline at end of file diff --git a/meta_bringup/config/shooter.yaml b/meta_bringup/config/shooter.yaml new file mode 100644 index 0000000..451b529 --- /dev/null +++ b/meta_bringup/config/shooter.yaml @@ -0,0 +1,19 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + + shoot_controller: + type: shoot_controller/ShootController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +shoot_controller: + ros__parameters: + friction_wheel: + velocity: 0.1 + inverse: false + + bullet_loader: + velocity: 0.1 + diff --git a/meta_bringup/launch/shooter.launch.py b/meta_bringup/launch/shooter.launch.py new file mode 100644 index 0000000..a3dec89 --- /dev/null +++ b/meta_bringup/launch/shooter.launch.py @@ -0,0 +1,121 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +# Necessary dirty work that lets us import modules from the meta_bringup package +import os +import sys +from ament_index_python.packages import get_package_share_directory +sys.path.append(os.path.join(get_package_share_directory('meta_bringup'), 'launch')) + +from launch_utils import load_controller, register_loading_order, register_sequential_loading + +ARGUMENTS = [ + DeclareLaunchArgument( + 'enable_simulation', + default_value='false', + description='If true, the simulation will be started'), +] + +def generate_launch_description(): + # Launch Arguments + enable_simulation = LaunchConfiguration('enable_simulation') + + # Get URDF via xacro + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([FindPackageShare('metav_description'), 'urdf', 'playground', 'shooter.xacro']), + ' ', + 'is_simulation:=', enable_simulation, + ]) + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[ + {'use_sim_time': enable_simulation}, + {'robot_description': robot_description_content},], + output='both', + emulate_tty=True + ) + + # Gazebo related launch + world_sdf = PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'worlds', 'empty_world.sdf']) + bridge_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'ros_gz_bridge.yaml']) + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('metav_gazebo'), 'launch', 'meta_gazebo.launch.py'])], + ), + launch_arguments=[ + ('world_sdf', world_sdf), + ('robot_name', 'shooter'), + ('bridge_config_file', bridge_config), + ], + condition=IfCondition(enable_simulation) + ) + + # ROS2 Control related launch + robot_controller_config = PathJoinSubstitution([FindPackageShare('meta_bringup'), 'config', 'shooter.yaml']) + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controller_config], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output='both', + emulate_tty=True, + condition=UnlessCondition(enable_simulation) + ) + + load_joint_state_broadcaster = load_controller('joint_state_broadcaster') + + # List of controllers to be loaded sequentially + # Order in this list is IMPORTANT + load_controllers = [ + load_controller('shoot_controller') + ] + + dbus_control_node = Node( + package='dbus_control', + executable='dbus_control_node', + name='dbus_control_node', + parameters=[robot_controller_config], + ) + + return LaunchDescription([ + # Launch Arguments + *ARGUMENTS, + # Launch Gazebo and ROS2 bridge and spawn robot in Gazebo (also start controller manager) + gazebo_launch, + # Load robot state publisher + node_robot_state_publisher, + # Launch controller manager (if not in simulation) + controller_manager, + # Load joint state broadcaster + load_joint_state_broadcaster, + # Load controllers + *register_sequential_loading(load_joint_state_broadcaster, *load_controllers), + dbus_control_node, + ]) From ccdcd6d12579c5fd862f3ecd41b583354336eb1f Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 13 Sep 2024 15:49:50 +0800 Subject: [PATCH 34/36] config files modification --- .../src/meta_shoot_controller.yaml | 2 ++ .../meta_shoot_controller/src/shoot_controller.cpp | 12 ++++++------ meta_bringup/config/shooter.yaml | 8 ++++++++ 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml b/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml index 6d49bcc..118d954 100644 --- a/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml +++ b/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml @@ -19,3 +19,5 @@ shoot_controller: default_value: 0.0, description: "Specifies the velocity of the friction wheel.", } + + diff --git a/decomposition/meta_shoot_controller/src/shoot_controller.cpp b/decomposition/meta_shoot_controller/src/shoot_controller.cpp index 651adf1..2b67bbd 100644 --- a/decomposition/meta_shoot_controller/src/shoot_controller.cpp +++ b/decomposition/meta_shoot_controller/src/shoot_controller.cpp @@ -28,18 +28,18 @@ namespace shoot_controller { } controller_interface::CallbackReturn - ShootController::on_configure(const rclcpp_lifecycle::State &previous_state){ + ShootController::on_configure(const rclcpp_lifecycle::State &/*previous_state*/){ params_ = param_listener_->get_params(); friction_wheel_speed_ = params_.friction_wheel.velocity; fric1_vel_pid_ = std::make_shared( - get_node(), "gains.friction_wheel1_vel", true + get_node(), "gains.friction_wheel1_joint_vel2eff", true ); fric2_vel_pid_ = std::make_shared( - get_node(), "gains.friction_wheel2_vel", true + get_node(), "gains.friction_wheel2_joint_vel2eff", true ); load_vel_pid_ = std::make_shared( - get_node(), "gains.load_vel", true + get_node(), "gains.bullet_loader_joint_vel2eff", true ); if (!fric1_vel_pid_->initPid()) { @@ -93,7 +93,7 @@ namespace shoot_controller { } controller_interface::CallbackReturn - ShootController::on_activate(const rclcpp_lifecycle::State &previous_state){ + ShootController::on_activate(const rclcpp_lifecycle::State &/*previous_state*/){ reset_controller_reference_msg(*(input_ref_.readFromRT()), get_node()); reference_interfaces_.assign(reference_interfaces_.size(), NaN); @@ -101,7 +101,7 @@ namespace shoot_controller { } controller_interface::CallbackReturn - ShootController::on_deactivate(const rclcpp_lifecycle::State &previous_state){ + ShootController::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/){ return controller_interface::CallbackReturn::SUCCESS; } diff --git a/meta_bringup/config/shooter.yaml b/meta_bringup/config/shooter.yaml index 451b529..1cc4aaa 100644 --- a/meta_bringup/config/shooter.yaml +++ b/meta_bringup/config/shooter.yaml @@ -17,3 +17,11 @@ shoot_controller: bullet_loader: velocity: 0.1 + gains: + friction_wheel1_joint_vel2eff: + { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + friction_wheel2_joint_vel2eff: + { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + bullet_loader_joint_vel2eff: + { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + From a3ee81f22b870c5c3c5d976aacdfd3e55eda6d1c Mon Sep 17 00:00:00 2001 From: Feiyang Wu Date: Fri, 13 Sep 2024 23:24:28 +0800 Subject: [PATCH 35/36] shoot controller test --- .../meta_shoot_controller/CMakeLists.txt | 3 + .../shoot_controller.xml | 4 +- .../src/shoot_controller.cpp | 86 +++++++++++-------- .../urdf/playground/shooter.xacro | 4 +- meta_bringup/config/shooter.yaml | 10 +-- 5 files changed, 60 insertions(+), 47 deletions(-) diff --git a/decomposition/meta_shoot_controller/CMakeLists.txt b/decomposition/meta_shoot_controller/CMakeLists.txt index 58e2766..623db3d 100644 --- a/decomposition/meta_shoot_controller/CMakeLists.txt +++ b/decomposition/meta_shoot_controller/CMakeLists.txt @@ -20,6 +20,9 @@ ament_auto_add_library( src/shoot_controller.cpp ) +pluginlib_export_plugin_description_file( + controller_interface shoot_controller.xml) + target_include_directories(${PROJECT_NAME} PUBLIC "$" "$") diff --git a/decomposition/meta_shoot_controller/shoot_controller.xml b/decomposition/meta_shoot_controller/shoot_controller.xml index 9468d30..403be86 100644 --- a/decomposition/meta_shoot_controller/shoot_controller.xml +++ b/decomposition/meta_shoot_controller/shoot_controller.xml @@ -18,8 +18,8 @@ Source of this file are templates in [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. --> - - + diff --git a/decomposition/meta_shoot_controller/src/shoot_controller.cpp b/decomposition/meta_shoot_controller/src/shoot_controller.cpp index 2b67bbd..62e381a 100644 --- a/decomposition/meta_shoot_controller/src/shoot_controller.cpp +++ b/decomposition/meta_shoot_controller/src/shoot_controller.cpp @@ -65,6 +65,7 @@ namespace shoot_controller { "~/reference", subscribers_qos, std::bind(&ShootController::reference_callback, this, std::placeholders::_1)); + auto msg = std::make_shared(); reset_controller_reference_msg(msg, get_node()); input_ref_.writeFromNonRT(msg); @@ -112,9 +113,9 @@ namespace shoot_controller { controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(3); - command_interfaces_config.names.push_back("friction_wheel1/" + std::string(HW_IF_EFFORT)); - command_interfaces_config.names.push_back("friction_wheel2/" + std::string(HW_IF_EFFORT)); - command_interfaces_config.names.push_back( "bullet_loader" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back("friction_wheel1_joint/" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back("friction_wheel2_joint/" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back( "bullet_loader_joint/" + std::string(HW_IF_EFFORT)); return command_interfaces_config; } @@ -128,9 +129,9 @@ namespace shoot_controller { // Joint position state of yaw gimbal is required state_interfaces_config.names.reserve(3); - state_interfaces_config.names.push_back("friction_wheel1/" + std::string(HW_IF_VELOCITY)); - state_interfaces_config.names.push_back("friction_wheel2/" + std::string(HW_IF_VELOCITY)); - state_interfaces_config.names.push_back("bullet_loader/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("friction_wheel1_joint/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("friction_wheel2_joint/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("bullet_loader_joint/" + std::string(HW_IF_VELOCITY)); return state_interfaces_config; } @@ -140,7 +141,7 @@ namespace shoot_controller { const std::shared_ptr & /*node*/) { msg->fric_state = false; msg->fric_state = false; - msg->feed_speed = 0.0; + msg->feed_speed = NaN; } controller_interface::return_type ShootController::update_reference_from_subscribers(){ @@ -171,10 +172,11 @@ namespace shoot_controller { const rclcpp::Duration & period ){ double fric1_vel_ref = NaN, fric2_vel_ref = NaN, load_vel_ref = NaN; - double fric1_vel_fb, fric2_vel_fb, load_vel_fb; + double fric1_vel_fb = NaN, fric2_vel_fb = NaN, load_vel_fb = NaN; double fric1_vel_err = NaN, fric2_vel_err = NaN, load_vel_err = NaN; double fric1_eff_cmd = NaN, fric2_eff_cmd = NaN, load_eff_cmd = NaN; + fric1_vel_ref = reference_interfaces_[0]; fric2_vel_ref = reference_interfaces_[0]; load_vel_ref = reference_interfaces_[1]; @@ -183,42 +185,46 @@ namespace shoot_controller { fric2_vel_fb = state_interfaces_[1].get_value(); load_vel_fb = state_interfaces_[2].get_value(); - fric1_vel_err = fric1_vel_ref - fric1_vel_fb; - fric1_vel_err = fric2_vel_ref - fric2_vel_fb; - load_vel_err = load_vel_ref - load_vel_fb; + if(!std::isnan(fric1_vel_ref) && !std::isnan(fric1_vel_fb) && + !std::isnan(fric2_vel_ref) && !std::isnan(fric2_vel_fb) && + !std::isnan(load_vel_ref) && !std::isnan(load_vel_fb)){ + + fric1_vel_err = fric1_vel_ref - fric1_vel_fb; + fric2_vel_err = fric2_vel_ref - fric2_vel_fb; + load_vel_err = load_vel_ref - load_vel_fb; + + fric1_eff_cmd = fric1_vel_pid_->computeCommand(fric1_vel_err, period); + fric2_eff_cmd = fric2_vel_pid_->computeCommand(fric2_vel_err, period); + load_eff_cmd = load_vel_pid_->computeCommand(load_vel_err, period); - fric1_eff_cmd = fric1_vel_pid_->computeCommand(fric1_vel_err, period); - fric2_eff_cmd = fric2_vel_pid_->computeCommand(fric2_vel_err, period); - load_eff_cmd = load_vel_pid_->computeCommand(load_vel_err, period); + command_interfaces_[0].set_value(fric1_eff_cmd); + command_interfaces_[1].set_value(fric2_eff_cmd); + command_interfaces_[2].set_value(load_eff_cmd); + } - command_interfaces_[0].set_value(fric1_vel_pid_->computeCommand(fric1_vel_err, period)); - command_interfaces_[1].set_value(fric2_vel_pid_->computeCommand(fric2_vel_err, period)); - command_interfaces_[2].set_value(load_vel_pid_->computeCommand(load_vel_err, period)); + // Publish state if (state_publisher_ && state_publisher_->trylock()) { state_publisher_->msg_.header.stamp = time; - if (friction_wheel_on_) { - state_publisher_->msg_.dof_states[0].reference = fric1_vel_ref; - state_publisher_->msg_.dof_states[0].feedback = fric1_vel_fb; - state_publisher_->msg_.dof_states[0].error = fric1_vel_err; - state_publisher_->msg_.dof_states[0].time_step = period.seconds(); - state_publisher_->msg_.dof_states[0].output = fric1_eff_cmd; - - state_publisher_->msg_.dof_states[1].reference = fric2_vel_ref; - state_publisher_->msg_.dof_states[1].feedback = fric2_vel_fb; - state_publisher_->msg_.dof_states[1].error = fric2_vel_err; - state_publisher_->msg_.dof_states[1].time_step = period.seconds(); - state_publisher_->msg_.dof_states[1].output = fric2_eff_cmd; - } - if (bullet_loader_on_) { - state_publisher_->msg_.dof_states[2].reference = load_vel_ref; - state_publisher_->msg_.dof_states[2].feedback = load_vel_fb; - state_publisher_->msg_.dof_states[2].error = load_vel_err; - state_publisher_->msg_.dof_states[2].time_step = period.seconds(); - state_publisher_->msg_.dof_states[2].output = load_eff_cmd; - } + state_publisher_->msg_.dof_states[0].reference = fric1_vel_ref; + state_publisher_->msg_.dof_states[0].feedback = fric1_vel_fb; + state_publisher_->msg_.dof_states[0].error = fric1_vel_err; + state_publisher_->msg_.dof_states[0].time_step = period.seconds(); + state_publisher_->msg_.dof_states[0].output = fric1_eff_cmd; + + state_publisher_->msg_.dof_states[1].reference = fric2_vel_ref; + state_publisher_->msg_.dof_states[1].feedback = fric2_vel_fb; + state_publisher_->msg_.dof_states[1].error = fric2_vel_err; + state_publisher_->msg_.dof_states[1].time_step = period.seconds(); + state_publisher_->msg_.dof_states[1].output = fric2_eff_cmd; + + state_publisher_->msg_.dof_states[2].reference = load_vel_ref; + state_publisher_->msg_.dof_states[2].feedback = load_vel_fb; + state_publisher_->msg_.dof_states[2].error = load_vel_err; + state_publisher_->msg_.dof_states[2].time_step = period.seconds(); + state_publisher_->msg_.dof_states[2].output = load_eff_cmd; state_publisher_->unlockAndPublish(); } @@ -227,7 +233,7 @@ namespace shoot_controller { std::vector ShootController::on_export_reference_interfaces() { - reference_interfaces_.resize(3, NaN); + reference_interfaces_.resize(2, NaN); std::vector reference_interfaces; reference_interfaces.reserve(reference_interfaces_.size()); @@ -246,6 +252,10 @@ namespace shoot_controller { const std::shared_ptr msg) { input_ref_.writeFromNonRT(msg); } + + bool ShootController::on_set_chained_mode(bool chained_mode){ + return true || chained_mode; + } } #include "pluginlib/class_list_macros.hpp" diff --git a/decomposition/metav_description/urdf/playground/shooter.xacro b/decomposition/metav_description/urdf/playground/shooter.xacro index 9084c15..b0bfdd4 100644 --- a/decomposition/metav_description/urdf/playground/shooter.xacro +++ b/decomposition/metav_description/urdf/playground/shooter.xacro @@ -34,7 +34,7 @@ can0 - + @@ -44,7 +44,7 @@ 1.0 0.0 - + diff --git a/meta_bringup/config/shooter.yaml b/meta_bringup/config/shooter.yaml index 1cc4aaa..a415471 100644 --- a/meta_bringup/config/shooter.yaml +++ b/meta_bringup/config/shooter.yaml @@ -3,7 +3,7 @@ controller_manager: update_rate: 250 # Hz shoot_controller: - type: shoot_controller/ShootController + type: meta_shoot_controller/ShootController joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster @@ -11,7 +11,7 @@ controller_manager: shoot_controller: ros__parameters: friction_wheel: - velocity: 0.1 + velocity: 50.0 inverse: false bullet_loader: @@ -19,9 +19,9 @@ shoot_controller: gains: friction_wheel1_joint_vel2eff: - { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + { p: 6.0e-3, i: 0.0, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } friction_wheel2_joint_vel2eff: - { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + { p: 0.0e-3, i: 0.0, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } bullet_loader_joint_vel2eff: - { p: 2.0e-2, i: 1.0e-4, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + { p: 2.0e-3, i: 0.0, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } From 1a57a6d61394252d6003424570dd8d96243bd942 Mon Sep 17 00:00:00 2001 From: Chiming Ni Date: Tue, 31 Dec 2024 17:58:59 +0800 Subject: [PATCH 36/36] Shooter functional on sentry --- .../dbus_vehicle/src/dbus_interpreter.cpp | 3 + decision/dbus_vehicle/src/dbus_vehicle.cpp | 3 +- .../src/shoot_controller.cpp | 12 +- .../urdf/sentry/sentry.shooter.xacro | 105 ++++++++++++++++++ .../urdf/sentry/sentry.xacro | 9 ++ meta_bringup/config/sentry.yaml | 21 ++++ meta_bringup/launch/sentry.launch.py | 1 + 7 files changed, 147 insertions(+), 7 deletions(-) create mode 100644 decomposition/metav_description/urdf/sentry/sentry.shooter.xacro diff --git a/decision/dbus_vehicle/src/dbus_interpreter.cpp b/decision/dbus_vehicle/src/dbus_interpreter.cpp index 8870211..35e482e 100644 --- a/decision/dbus_vehicle/src/dbus_interpreter.cpp +++ b/decision/dbus_vehicle/src/dbus_interpreter.cpp @@ -58,16 +58,19 @@ void DbusInterpreter::update() { shoot_->fric_state = false; shoot_->feed_state = false; + shoot_->feed_speed = 0; } else if (rsw == "MID") { shoot_->fric_state = true; shoot_->feed_state = false; + shoot_->feed_speed = 0; } else if (rsw == "DOWN") { shoot_->fric_state = true; shoot_->feed_state = true; + shoot_->feed_speed = -4.0; } } diff --git a/decision/dbus_vehicle/src/dbus_vehicle.cpp b/decision/dbus_vehicle/src/dbus_vehicle.cpp index 06f8858..491c1e5 100644 --- a/decision/dbus_vehicle/src/dbus_vehicle.cpp +++ b/decision/dbus_vehicle/src/dbus_vehicle.cpp @@ -16,6 +16,7 @@ class DbusVehicle : public rclcpp::Node double aim_sens = this->declare_parameter("control.stick_sens", 1.57); double deadzone = this->declare_parameter("control.deadzone", 0.05); std::string aim_topic = this->declare_parameter("aim_topic", "aim"); + std::string shoot_topic = this->declare_parameter("shoot_topic", "shoot"); RCLCPP_INFO(this->get_logger(), "max_vel: %f, max_omega: %f, aim_sens: %f, deadzone: %f", max_vel, max_omega, aim_sens, deadzone); enable_ros2_control_ = this->declare_parameter("enable_ros2_control", false); @@ -29,7 +30,7 @@ class DbusVehicle : public rclcpp::Node } else { move_pub_ = this->create_publisher("move", 10); } - shoot_pub_ = this->create_publisher("shoot", 10); + shoot_pub_ = this->create_publisher(shoot_topic, 10); aim_pub_ = this->create_publisher(aim_topic, 10); dbus_sub_ = this->create_subscription( "dbus_control", 10, diff --git a/decomposition/meta_shoot_controller/src/shoot_controller.cpp b/decomposition/meta_shoot_controller/src/shoot_controller.cpp index 62e381a..4ffa411 100644 --- a/decomposition/meta_shoot_controller/src/shoot_controller.cpp +++ b/decomposition/meta_shoot_controller/src/shoot_controller.cpp @@ -113,9 +113,9 @@ namespace shoot_controller { controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(3); - command_interfaces_config.names.push_back("friction_wheel1_joint/" + std::string(HW_IF_EFFORT)); - command_interfaces_config.names.push_back("friction_wheel2_joint/" + std::string(HW_IF_EFFORT)); - command_interfaces_config.names.push_back( "bullet_loader_joint/" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back("fric1_shooter_joint/" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back("fric2_shooter_joint/" + std::string(HW_IF_EFFORT)); + command_interfaces_config.names.push_back( "loader_shooter_joint/" + std::string(HW_IF_EFFORT)); return command_interfaces_config; } @@ -129,9 +129,9 @@ namespace shoot_controller { // Joint position state of yaw gimbal is required state_interfaces_config.names.reserve(3); - state_interfaces_config.names.push_back("friction_wheel1_joint/" + std::string(HW_IF_VELOCITY)); - state_interfaces_config.names.push_back("friction_wheel2_joint/" + std::string(HW_IF_VELOCITY)); - state_interfaces_config.names.push_back("bullet_loader_joint/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("fric1_shooter_joint/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("fric2_shooter_joint/" + std::string(HW_IF_VELOCITY)); + state_interfaces_config.names.push_back("loader_shooter_joint/" + std::string(HW_IF_VELOCITY)); return state_interfaces_config; } diff --git a/decomposition/metav_description/urdf/sentry/sentry.shooter.xacro b/decomposition/metav_description/urdf/sentry/sentry.shooter.xacro new file mode 100644 index 0000000..cfcecd0 --- /dev/null +++ b/decomposition/metav_description/urdf/sentry/sentry.shooter.xacro @@ -0,0 +1,105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + M2006 + ${motor_id} + ${mechanical_reduction} + ${offset} + + + + \ No newline at end of file diff --git a/decomposition/metav_description/urdf/sentry/sentry.xacro b/decomposition/metav_description/urdf/sentry/sentry.xacro index 516b605..ef57615 100644 --- a/decomposition/metav_description/urdf/sentry/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry/sentry.xacro @@ -24,6 +24,9 @@ + + + @@ -46,6 +49,12 @@ motor_id="5" /> + + + diff --git a/meta_bringup/config/sentry.yaml b/meta_bringup/config/sentry.yaml index e6a4b4e..5ee528e 100644 --- a/meta_bringup/config/sentry.yaml +++ b/meta_bringup/config/sentry.yaml @@ -14,6 +14,9 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + shoot_controller: + type: meta_shoot_controller/ShootController + omni_chassis_controller: ros__parameters: omni_wheel_joints: @@ -84,6 +87,23 @@ gimbal_controller: pitch_gimbal_joint_vel2eff: { p: 1.0e-1, i: 2.0e-2, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } +shoot_controller: + ros__parameters: + friction_wheel: + velocity: -500.0 + inverse: false + + bullet_loader: + velocity: 0.1 + + gains: + friction_wheel1_joint_vel2eff: + { p: 6.0e-3, i: 0.0, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + friction_wheel2_joint_vel2eff: + { p: 6.0e-3, i: 0.0, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + bullet_loader_joint_vel2eff: + { p: 1.0e-1, i: 5.0e-1, d: 0.0e-2, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + dbus_control: ros__parameters: dbus_port: "dbus_control" @@ -92,6 +112,7 @@ dbus_vehicle: ros__parameters: enable_ros2_control: true aim_topic: "/gimbal_controller/reference" + shoot_topic: "/shoot_controller/reference" control: trans_vel: 2.5 # m/s rot_vel: 3.0 # rad/s diff --git a/meta_bringup/launch/sentry.launch.py b/meta_bringup/launch/sentry.launch.py index 6d40316..4627330 100644 --- a/meta_bringup/launch/sentry.launch.py +++ b/meta_bringup/launch/sentry.launch.py @@ -95,6 +95,7 @@ def generate_launch_description(): load_controller('wheels_pid_controller'), load_controller('gimbal_controller'), load_controller('omni_chassis_controller'), + load_controller('shoot_controller') ] dbus_control = Node(