diff --git a/decision/dbus_vehicle/src/dbus_interpreter.cpp b/decision/dbus_vehicle/src/dbus_interpreter.cpp index 8870211f..35e482e2 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 06f8858d..491c1e51 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_chassis_controller/CMakeLists.txt b/decomposition/meta_chassis_controller/CMakeLists.txt index 132136a1..82e72077 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 5025e3ea..250c357d 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 5219fb28..00000000 --- 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 4ef6b4ec..4e4c4631 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 00000000..ea7055ca --- /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 17af2a69..f8cafabb 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/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp index df565fb1..049730ba 100644 --- a/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp +++ b/decomposition/meta_gimbal_controller/include/gimbal_controller/gimbal_controller.hpp @@ -30,7 +30,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; diff --git a/decomposition/meta_gimbal_controller/package.xml b/decomposition/meta_gimbal_controller/package.xml index 929b8c96..d92220be 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 9daa761e..209d59e9 100644 --- a/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp +++ b/decomposition/meta_gimbal_controller/src/gimbal_controller.cpp @@ -4,27 +4,15 @@ #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 +20,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 +45,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() { @@ -132,7 +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); - try { // State publisher s_publisher_ = get_node()->create_publisher( @@ -229,7 +212,6 @@ GimbalController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/ 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; } @@ -255,13 +237,12 @@ GimbalController::update_and_write_commands(const rclcpp::Time &time, auto current_feedback = *(input_feedback_.readFromRT()); 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::Quaternion q_imu; + fromMsg(current_feedback->orientation, q_imu); + tf2::Matrix3x3(q_imu).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 = 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; diff --git a/decomposition/meta_shoot_controller/CMakeLists.txt b/decomposition/meta_shoot_controller/CMakeLists.txt new file mode 100644 index 00000000..623db3d5 --- /dev/null +++ b/decomposition/meta_shoot_controller/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.8) +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) +endif() + +# find dependencies +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 + src/shoot_controller.cpp +) + +pluginlib_export_plugin_description_file( + controller_interface shoot_controller.xml) + +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_parameters) + +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_auto_package() diff --git a/decomposition/meta_shoot_controller/LICENSE b/decomposition/meta_shoot_controller/LICENSE new file mode 100644 index 00000000..30e8e2ec --- /dev/null +++ b/decomposition/meta_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/meta_shoot_controller/include/shoot_controller/shoot_controller.hpp b/decomposition/meta_shoot_controller/include/shoot_controller/shoot_controller.hpp new file mode 100644 index 00000000..5a16a385 --- /dev/null +++ b/decomposition/meta_shoot_controller/include/shoot_controller/shoot_controller.hpp @@ -0,0 +1,96 @@ +#ifndef SHOOT_CONTROLLER__SHOOT_CONTROLLER_HPP_ +#define SHOOT_CONTROLLER__SHOOT_CONTROLLER_HPP_ + +#include +#include +#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 "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 "meta_shoot_controller_parameters.hpp" + + + +namespace shoot_controller { + class ShootController : public controller_interface::ChainableControllerInterface { + public: + ShootController()=default; + + 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: + 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_; + + rclcpp::Publisher::SharedPtr s_publisher_; + std::unique_ptr state_publisher_; + + // Command subscribers + rclcpp::Subscription::SharedPtr ref_subscriber_; + 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/meta_shoot_controller/package.xml b/decomposition/meta_shoot_controller/package.xml new file mode 100644 index 00000000..bbc601a5 --- /dev/null +++ b/decomposition/meta_shoot_controller/package.xml @@ -0,0 +1,30 @@ + + + + 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 + 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/meta_shoot_controller/shoot_controller.xml b/decomposition/meta_shoot_controller/shoot_controller.xml new file mode 100644 index 00000000..403be864 --- /dev/null +++ b/decomposition/meta_shoot_controller/shoot_controller.xml @@ -0,0 +1,29 @@ + + + + + + Shoot Controller ros2_control controller. + + + \ No newline at end of file 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 00000000..118d9548 --- /dev/null +++ b/decomposition/meta_shoot_controller/src/meta_shoot_controller.yaml @@ -0,0 +1,23 @@ +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 00000000..4ffa4110 --- /dev/null +++ b/decomposition/meta_shoot_controller/src/shoot_controller.cpp @@ -0,0 +1,264 @@ +#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_joint_vel2eff", true + ); + fric2_vel_pid_ = std::make_shared( + get_node(), "gains.friction_wheel2_joint_vel2eff", true + ); + load_vel_pid_ = std::make_shared( + get_node(), "gains.bullet_loader_joint_vel2eff", 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("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; + } + + 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("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; + } + + 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 = NaN; + } + + 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 = 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]; + + fric1_vel_fb = state_interfaces_[0].get_value(); + fric2_vel_fb = state_interfaces_[1].get_value(); + load_vel_fb = state_interfaces_[2].get_value(); + + 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); + + command_interfaces_[0].set_value(fric1_eff_cmd); + command_interfaces_[1].set_value(fric2_eff_cmd); + command_interfaces_[2].set_value(load_eff_cmd); + } + + + + // Publish state + if (state_publisher_ && state_publisher_->trylock()) { + state_publisher_->msg_.header.stamp = time; + + 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(); + } + return controller_interface::return_type::OK; + } + + std::vector + ShootController::on_export_reference_interfaces() { + reference_interfaces_.resize(2, 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); + } + + bool ShootController::on_set_chained_mode(bool chained_mode){ + return true || chained_mode; + } +} + +#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/common/omni_wheel.xacro b/decomposition/metav_description/urdf/common/omni_wheel.xacro index 322f442d..9af95b63 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/playground/dm_motor.xacro b/decomposition/metav_description/urdf/playground/dm_motor.xacro new file mode 100644 index 00000000..23d7dc7e --- /dev/null +++ b/decomposition/metav_description/urdf/playground/dm_motor.xacro @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + ign_ros2_control/IgnitionSystem + + + + + + + + + + + + + meta_hardware/MetaRobotDmMotorNetwork + can0 + 0 + + + + + + + 4310 + 2 + 1.0 + 3.141593 + 30 + 10 + 0.0 + position + 150.0 + 0.5 + + + + + + + + + $(find meta_bringup)/config/dm_motor.yaml + + + + \ No newline at end of file diff --git a/decomposition/metav_description/urdf/playground/multiple_motors.xacro b/decomposition/metav_description/urdf/playground/multiple_motors.xacro index 64150d65..cffbd632 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/power_test.xacro b/decomposition/metav_description/urdf/playground/power_test.xacro new file mode 100644 index 00000000..cb331b03 --- /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/metav_description/urdf/playground/shooter.xacro b/decomposition/metav_description/urdf/playground/shooter.xacro new file mode 100644 index 00000000..b0bfdd42 --- /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/decomposition/metav_description/urdf/playground/single_motor.xacro b/decomposition/metav_description/urdf/playground/single_motor.xacro index cf327593..b9f7948e 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 00000000..c407c6b5 --- /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/decomposition/metav_description/urdf/sentry/sentry.gimbal.xacro b/decomposition/metav_description/urdf/sentry/sentry.gimbal.xacro index 6cbe1669..244447be 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.shooter.xacro b/decomposition/metav_description/urdf/sentry/sentry.shooter.xacro new file mode 100644 index 00000000..cfcecd0d --- /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 539c8264..ef576151 100644 --- a/decomposition/metav_description/urdf/sentry/sentry.xacro +++ b/decomposition/metav_description/urdf/sentry/sentry.xacro @@ -24,9 +24,12 @@ + + + + rpy="0 0 0" /> meta_hardware/MetaRobotDjiMotorNetwork can1 - - - - + + + + + motor_id="5" /> + motor_id="6" /> + + + @@ -58,18 +63,14 @@ ign_ros2_control/IgnitionSystem - - - - + + + + + motor_id="5" /> + motor_id="6" /> diff --git a/decomposition/power_limit/CMakeLists.txt b/decomposition/power_limit/CMakeLists.txt new file mode 100644 index 00000000..10c3d8db --- /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.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_auto_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 00000000..57635442 --- /dev/null +++ b/decomposition/power_limit/include/power_limit/power_limit.hpp @@ -0,0 +1,80 @@ +#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 "controller_interface/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::ControllerInterface { + 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(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_; + +}; + +} // 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 00000000..ae4c3330 --- /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.xml b/decomposition/power_limit/power_limit.xml new file mode 100644 index 00000000..7e89f4e6 --- /dev/null +++ b/decomposition/power_limit/power_limit.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 00000000..0fe9d127 --- /dev/null +++ b/decomposition/power_limit/src/power_limit.cpp @@ -0,0 +1,113 @@ +#include "power_limit/power_limit.hpp" + +#include +#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::ControllerInterface() {} + +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; +} + +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(const rclcpp::Time &/*time*/, + const rclcpp::Duration &/*period*/) { + if (param_listener_->is_old(params_)) { + params_ = param_listener_->get_params(); + } + + 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 = 0.0; + state_publisher_->msg_.motor_effort = 0.0; + 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::ControllerInterface) diff --git a/decomposition/power_limit/src/power_limit.yaml b/decomposition/power_limit/src/power_limit.yaml new file mode 100644 index 00000000..1e1af0c2 --- /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/execution/meta_hardware/CMakeLists.txt b/execution/meta_hardware/CMakeLists.txt index 3296ae7e..efa57b6b 100644 --- a/execution/meta_hardware/CMakeLists.txt +++ b/execution/meta_hardware/CMakeLists.txt @@ -14,6 +14,8 @@ find_package(pluginlib REQUIRED) 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 @@ -24,9 +26,14 @@ 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 + src/im1253c_interface.cpp + src/modbus_rtu_driver/modbus_rtu_driver.cpp ) target_include_directories( meta_hardware @@ -39,6 +46,8 @@ ament_target_dependencies( rclcpp 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/brt_encoder_interface.hpp b/execution/meta_hardware/include/meta_hardware/brt_encoder_interface.hpp index e930308b..027ef4f2 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/include/meta_hardware/can_driver/can_driver.hpp b/execution/meta_hardware/include/meta_hardware/can_driver/can_driver.hpp index fc6d9677..583560a2 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; } 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 00000000..1419c3b2 --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/dm_motor_interface.hpp @@ -0,0 +1,99 @@ +#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/dm_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 + MIT_POS, + MIT_VEL, + MIT_EFF, + MIT_POS_FF, + MIT_VEL_FF, + 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 dm_motor_network_; +}; + +} // namespace meta_hardware + +#endif // META_HARDWARE__DM_MOTOR_INTERFACE_HPP_ 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 00000000..46775403 --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/im1253c_interface.hpp @@ -0,0 +1,69 @@ +#ifndef META_HARDWARE__IM1253C_INTERFACE_HPP_ +#define META_HARDWARE__IM1253C_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "meta_hardware/modbus_rtu_driver/modbus_rtu_driver.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: + 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 + +#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 00000000..fc970d0d --- /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); + std::vector get_reg_data(){return reg_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 reg_data_; + + 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/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 00000000..5208e5b9 --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/motor_driver/dm_motor_driver.hpp @@ -0,0 +1,82 @@ +#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 +#include + +#include + +#include "angles/angles.h" + +namespace meta_hardware { +class DmMotor { + public: + DmMotor(const std::unordered_map &motor_param, + uint8_t master_id); + + ~DmMotor() = default; + + enum RunMode { + MIT, + POSITION, + VELOCITY, + }; + + uint32_t get_dm_motor_id() const; + uint32_t get_tx_can_id() const; + uint32_t get_rx_can_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) const; + can_frame motor_vel_frame(double velocity) const; + + void set_motor_feedback(const can_frame &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; + + // Motor information + std::string motor_model_; + uint32_t dm_motor_id_; + 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_; + + double Kp_; + double Kd_; + uint8_t Kp_raw_; + uint16_t Kd_raw_; + + // 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_driver/mi_motor_driver.hpp b/execution/meta_hardware/include/meta_hardware/motor_driver/mi_motor_driver.hpp index bb0c47b2..c0374b5a 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/include/meta_hardware/motor_network/dji_motor_network.hpp b/execution/meta_hardware/include/meta_hardware/motor_network/dji_motor_network.hpp index ea99b188..5e91de21 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/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 00000000..97416a43 --- /dev/null +++ b/execution/meta_hardware/include/meta_hardware/motor_network/dm_motor_network.hpp @@ -0,0 +1,65 @@ +#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( + const std::string &can_network_name, uint8_t master_id, + const std::vector> &joint_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 position The position to write + * @param velocity The velocity to write + * @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); + void write_vel(uint32_t joint_id, double velocity); + + private: + // 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> dm_motors_; + + // Master ID + uint32_t master_id_; +}; + +} // namespace meta_hardware + +#endif // META_HARDWARE__MOTOR_NETWORK__DM_MOTOR_NETWORK_HPP_ 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 a1bd863c..2eea345b 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/meta_hardware.xml b/execution/meta_hardware/meta_hardware.xml index 8a36524e..d9c3eefd 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. + + @@ -20,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 bc5683f6..8d43358f 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/dji_motor_interface.cpp b/execution/meta_hardware/src/dji_motor_interface.cpp index c7682b4a..ae7f8d41 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/dm_motor_interface.cpp b/execution/meta_hardware/src/dm_motor_interface.cpp new file mode 100644 index 00000000..fdefc5c7 --- /dev/null +++ b/execution/meta_hardware/src/dm_motor_interface.cpp @@ -0,0 +1,265 @@ +#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/dm_motor_interface.hpp" +#include "meta_hardware/motor_network/dm_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; + +MetaRobotDmMotorNetwork::~MetaRobotDmMotorNetwork() = default; + +hardware_interface::CallbackReturn +MetaRobotDmMotorNetwork::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; +} + +MetaRobotDmMotorNetwork::DmMotorMode +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") { + 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") { + return VELOCITY; + } else { + throw std::runtime_error("Unknown control mode: " + mode); + } +} + +hardware_interface::CallbackReturn MetaRobotDmMotorNetwork::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"); + 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 +MetaRobotDmMotorNetwork::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 +MetaRobotDmMotorNetwork::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 +MetaRobotDmMotorNetwork::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + + return CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn MetaRobotDmMotorNetwork::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { + + return CallbackReturn::SUCCESS; +} + +hardware_interface::return_type +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] = dm_motor_network_->read(i); + + double reduction = joint_motor_info_[i].mechanical_reduction; + double offset = joint_motor_info_[i].offset; + position = position / reduction + offset; + velocity /= reduction; + effort *= reduction; + + 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 +MetaRobotDmMotorNetwork::write(const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) { + + 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; + + 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 MetaRobotDmMotorNetwork::DmMotorMode; + switch (joint_motor_info_[i].mode) { + case MIT: + if (std::isnan(position) || std::isnan(velocity) || std::isnan(effort)) + 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)){ + continue; + } + dm_motor_network_->write_pos(i, position); + break; + case VELOCITY: + if (std::isnan(velocity)) + continue; + dm_motor_network_->write_vel(i, velocity); + break; + } + } + + return hardware_interface::return_type::OK; +} + +} // namespace meta_hardware + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(meta_hardware::MetaRobotDmMotorNetwork, + hardware_interface::SystemInterface) diff --git a/execution/meta_hardware/src/im1253c_interface.cpp b/execution/meta_hardware/src/im1253c_interface.cpp new file mode 100644 index 00000000..ef6e055f --- /dev/null +++ b/execution/meta_hardware/src/im1253c_interface.cpp @@ -0,0 +1,126 @@ +#include +#include +#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 { + +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; + } + sensor_interface_data_.resize(info_.sensors.size()); + return CallbackReturn::SUCCESS; +} + + +hardware_interface::CallbackReturn MetaRobotIm1253cManager::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) { + 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; +} + +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_.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; +} + +std::vector +MetaRobotIm1253cManager::export_command_interfaces() { + std::vector command_interfaces; + 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*/) { + 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]); + // 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 +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 00000000..fb38e4c7 --- /dev/null +++ b/execution/meta_hardware/src/modbus_rtu_driver/modbus_rtu_driver.cpp @@ -0,0 +1,157 @@ + +#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): + 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) { + std::cerr << "Error creating serial port: " << device_name_ << " - " << ex.what() << std::endl; + 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)); + } + + device_name_ = serial_params.at("device_name"); +}; + + +auto ModbusRtuDriver::awake_time() +{ + using std::chrono::operator""ms; + return std::chrono::steady_clock::now() + 10ms; +} + +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_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){ + 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()) { + auto start_time = std::chrono::steady_clock::now(); + + // std::cout << "Sending Command..." << std::endl; + // Send Command with blocking + serial_driver_->port()->send(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 << "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); + } +} + + +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(); + } + } 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 00000000..882ca15e --- /dev/null +++ b/execution/meta_hardware/src/motor_driver/dm_motor_driver.cpp @@ -0,0 +1,204 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "angles/angles.h" +#include "meta_hardware/motor_driver/dm_motor_driver.hpp" + + + +namespace meta_hardware { + +using std::tuple; + +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 (control_mode == "mit") { + Kp_ = std::stod(motor_param.at("Kp")); + Kd_ = std::stod(motor_param.at("Kd")); + + 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; + + } else if (control_mode == "position") { + run_mode_ = RunMode::POSITION; + id_offeset = 0x100; + } else if (control_mode == "velocity") { + run_mode_ = RunMode::VELOCITY; + id_offeset = 0x200; + } else { + throw std::runtime_error("Unknown motor mode: " + control_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_; } + +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() 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() 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() 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}}; + return disable_frame; +} + +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_, 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_, 12); + 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; +} + +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 & 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 = std::bit_cast(static_cast(velocity)); + can_frame frame = { + .can_id = tx_can_id_, + .can_dlc = 4, + .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; +} + +void DmMotor::set_motor_feedback(const can_frame &can_msg){ + + 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) << 4) | + ((static_cast(can_msg.data[4]) & 0xF0) >> 4) + ); + + auto effort_raw = static_cast + (((static_cast(can_msg.data[4]) & 0x0F) << 8) | + (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])); + + + 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 << 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 { + 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< &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; } else { 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 +79,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); @@ -103,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)); } @@ -138,15 +170,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/brt_encoder_network.cpp b/execution/meta_hardware/src/motor_network/brt_encoder_network.cpp index c5490bb8..05dae4f3 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/execution/meta_hardware/src/motor_network/dji_motor_network.cpp b/execution/meta_hardware/src/motor_network/dji_motor_network.cpp index 06ef1cae..be65bebd 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,16 +52,20 @@ 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; + } } } } -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(); 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 00000000..94bd2253 --- /dev/null +++ b/execution/meta_hardware/src/motor_network/dm_motor_network.cpp @@ -0,0 +1,113 @@ +#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(const std::string &can_network_name, uint8_t master_id, + const std::vector> &joint_params) + : master_id_(master_id) { + + 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("control_mode"); + + auto dm_motor = + std::make_shared(joint_param, master_id); + dm_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_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([this](std::stop_token s) { rx_loop(s); }); +} + +DmMotorNetwork::~DmMotorNetwork() { + // 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; + } +} + +std::tuple +DmMotorNetwork::read(uint32_t joint_id) const { + return dm_motors_[joint_id]->get_motor_feedback(); +} + +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; + } +} + +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)); + } catch (CanIOException &e) { + std::cerr << "Error writing DM motor command CAN message: " << e.what() + << std::endl; + } +} + +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(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.data[0] & 0x0F); + 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; + } + } +} + +} // namespace meta_hardware 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 0b4d7f4b..7a13c98d 100644 --- a/execution/meta_hardware/src/motor_network/mi_motor_network.cpp +++ b/execution/meta_hardware/src/motor_network/mi_motor_network.cpp @@ -19,37 +19,42 @@ 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 - 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, false, can_filters); - // Enable all motors + // Enable all motors and set parameters 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->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()); } } diff --git a/interfaces/behavior_interface/CMakeLists.txt b/interfaces/behavior_interface/CMakeLists.txt index 20378496..8f766cea 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 00000000..8e1df562 --- /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 diff --git a/interfaces/behavior_interface/msg/Shoot.msg b/interfaces/behavior_interface/msg/Shoot.msg index c7141fcb..c3ed3ca6 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 diff --git a/interfaces/device_interface/CMakeLists.txt b/interfaces/device_interface/CMakeLists.txt index 46c2b752..1c187edb 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/interfaces/device_interface/msg/MotorPower.msg b/interfaces/device_interface/msg/MotorPower.msg new file mode 100644 index 00000000..66e79fdc --- /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/dm_motor.yaml b/meta_bringup/config/dm_motor.yaml new file mode 100644 index 00000000..bc57074a --- /dev/null +++ b/meta_bringup/config/dm_motor.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +forward_position_controller: + ros__parameters: + joints: + - motor_joint + interface_name: position \ No newline at end of file diff --git a/meta_bringup/config/im1253c.yaml b/meta_bringup/config/im1253c.yaml new file mode 100644 index 00000000..bf278ec8 --- /dev/null +++ b/meta_bringup/config/im1253c.yaml @@ -0,0 +1,65 @@ +controller_manager: + ros__parameters: + 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/config/sentry.yaml b/meta_bringup/config/sentry.yaml index e6a4b4e2..5ee528ed 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/config/shooter.yaml b/meta_bringup/config/shooter.yaml new file mode 100644 index 00000000..a4154718 --- /dev/null +++ b/meta_bringup/config/shooter.yaml @@ -0,0 +1,27 @@ +controller_manager: + ros__parameters: + update_rate: 250 # Hz + + shoot_controller: + type: meta_shoot_controller/ShootController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +shoot_controller: + ros__parameters: + friction_wheel: + velocity: 50.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: 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-3, i: 0.0, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true } + 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 00000000..34727e3f --- /dev/null +++ b/meta_bringup/config/single_motor_ext_encoder.yaml @@ -0,0 +1,33 @@ +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: effort + + reference_and_state_interfaces: ["position"] + + gains: + motor_joint: { p: 12.0, i: 0.0, d: 0.0, angle_wraparound: true } diff --git a/meta_bringup/launch/dm_motor.launch.py b/meta_bringup/launch/dm_motor.launch.py new file mode 100644 index 00000000..6c03c5f5 --- /dev/null +++ b/meta_bringup/launch/dm_motor.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', '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('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, + ]) diff --git a/meta_bringup/launch/im1253c.launch.py b/meta_bringup/launch/im1253c.launch.py new file mode 100644 index 00000000..55e756b4 --- /dev/null +++ b/meta_bringup/launch/im1253c.launch.py @@ -0,0 +1,123 @@ +# 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', 'power_test.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', 'im1253c_test'), + ('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('power_limit'), + # load_controller('forward_position_controller'), + ] + + motor_tester_node = Node( + package='motor_tester', + executable='motor_tester', + name='motor_tester_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, + ]) diff --git a/meta_bringup/launch/sentry.launch.py b/meta_bringup/launch/sentry.launch.py index 6d403161..46273309 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( diff --git a/meta_bringup/launch/shooter.launch.py b/meta_bringup/launch/shooter.launch.py new file mode 100644 index 00000000..a3dec891 --- /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, + ]) 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 00000000..f28f172e --- /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, + ])