From 1441e2c523c2cdd61763affbecc45648bdc989ac Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Mon, 24 Feb 2025 17:08:58 -0500 Subject: [PATCH] Saving my progress, still non functional --- spot_controllers/CMakeLists.txt | 8 +++++- .../spot_forward_controller.hpp | 13 +++++++-- .../spot_forward_controller_parameters.yaml | 16 +++++++++++ spot_controllers/package.xml | 1 + .../src/spot_forward_controller.cpp | 17 ++++++------ .../spot_default_controllers_with_arm.yaml | 27 +++++++++++++++++++ 6 files changed, 71 insertions(+), 11 deletions(-) create mode 100644 spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml diff --git a/spot_controllers/CMakeLists.txt b/spot_controllers/CMakeLists.txt index 6e91d3b9c..ae5f351af 100644 --- a/spot_controllers/CMakeLists.txt +++ b/spot_controllers/CMakeLists.txt @@ -38,6 +38,11 @@ generate_parameter_library( include/spot_controllers/forward_state_controller_parameters.yaml ) +generate_parameter_library( + spot_forward_controller_parameters + include/spot_controllers/spot_forward_controller_parameters.yaml +) + # Add the hardware interface add_library( spot_controllers @@ -53,6 +58,7 @@ target_include_directories(spot_controllers PUBLIC target_link_libraries( spot_controllers PUBLIC forward_state_controller_parameters + spot_forward_controller_parameters forward_command_controller::forward_command_controller ) ament_target_dependencies( @@ -72,7 +78,7 @@ install( DESTINATION include/${PROJECT_NAME} ) -install(TARGETS spot_controllers forward_state_controller_parameters +install(TARGETS spot_controllers forward_state_controller_parameters spot_forward_controller_parameters EXPORT export_spot_controllers ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/spot_controllers/include/spot_controllers/spot_forward_controller.hpp b/spot_controllers/include/spot_controllers/spot_forward_controller.hpp index d6f918184..b7e7031e8 100644 --- a/spot_controllers/include/spot_controllers/spot_forward_controller.hpp +++ b/spot_controllers/include/spot_controllers/spot_forward_controller.hpp @@ -18,16 +18,19 @@ #include #include +#include #include "controller_interface/controller_interface.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/realtime_buffer.hpp" +#include "sensor_msgs/msg/joint_state.hpp" #include "spot_controllers/visibility_control.h" #include "spot_msgs/msg/joint_command.hpp" #include "std_msgs/msg/float64_multi_array.hpp" namespace spot_controllers { -using CmdType = spot_msgs::msg::JointCommand; +// using CmdType = spot_msgs::msg::JointCommand; +using CmdType = sensor_msgs::msg::JointState; /** * \brief Forward command controller for a set of joints and interfaces. @@ -35,7 +38,7 @@ using CmdType = spot_msgs::msg::JointCommand; * This class forwards the command signal down to a set of joints or interfaces. * * Subscribes to: - * - \b commands (spot_msgs::msg::JointCommand) : The commands to apply. + * - \b joint_commands (spot_msgs::msg::JointCommand) : The commands to apply. */ class SpotForwardController : public controller_interface::ControllerInterface { public: @@ -58,6 +61,12 @@ class SpotForwardController : public controller_interface::ControllerInterface { controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; protected: + using Params = spot_forward_controller::Params; + using ParamListener = spot_forward_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; + std::vector joint_names_; std::string interface_name_; diff --git a/spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml b/spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml new file mode 100644 index 000000000..2a37affdf --- /dev/null +++ b/spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml @@ -0,0 +1,16 @@ +spot_forward_controller: + joints: { + type: string_array, + default_value: [], + description: "Names of the joint to control", + } + joint_prefix: { + type: string, + default_value: "", + description: "Optional prefix to apply to joint names" + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } diff --git a/spot_controllers/package.xml b/spot_controllers/package.xml index 0b441fbbe..8cbf732f2 100644 --- a/spot_controllers/package.xml +++ b/spot_controllers/package.xml @@ -17,6 +17,7 @@ pluginlib rclcpp rclcpp_lifecycle + spot_msgs ament_cmake diff --git a/spot_controllers/src/spot_forward_controller.cpp b/spot_controllers/src/spot_forward_controller.cpp index a457bcfea..380a1653b 100644 --- a/spot_controllers/src/spot_forward_controller.cpp +++ b/spot_controllers/src/spot_forward_controller.cpp @@ -42,18 +42,15 @@ controller_interface::CallbackReturn SpotForwardController::on_init() { // return controller_interface::CallbackReturn::ERROR; // } + auto param_listener = std::make_shared(get_node()); + params_ = param_listener->get_params(); + return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn SpotForwardController::on_configure( const rclcpp_lifecycle::State& /*previous_state*/) { - // auto ret = this->read_parameters(); - // if (ret != controller_interface::CallbackReturn::SUCCESS) - // { - // return ret; - // } - - joints_command_subscriber_ = get_node()->create_subscription("~/commands", rclcpp::SystemDefaultsQoS(), + joints_command_subscriber_ = get_node()->create_subscription("~/joint_commands", rclcpp::SystemDefaultsQoS(), [this](const CmdType::SharedPtr msg) { rt_command_ptr_.writeFromNonRT(msg); }); @@ -65,12 +62,16 @@ controller_interface::CallbackReturn SpotForwardController::on_configure( controller_interface::InterfaceConfiguration SpotForwardController::command_interface_configuration() const { controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - command_interfaces_config.names = command_interface_types_; + for (const auto& joint : params_.joints) { + const auto interface_name = joint + "/" + "position"; + command_interfaces_config.names.push_back(interface_name); + } return command_interfaces_config; } controller_interface::InterfaceConfiguration SpotForwardController::state_interface_configuration() const { + // this controller has no knowledge of robot state... but it could (i.e. basic command validation?) return controller_interface::InterfaceConfiguration{controller_interface::interface_configuration_type::NONE}; } diff --git a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml index 557322fea..2b6a5ff00 100644 --- a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml +++ b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml @@ -14,6 +14,9 @@ controller_manager: forward_state_controller: type: spot_controllers/ForwardStateController + spot_forward_controller: + type: spot_controllers/SpotForwardController + forward_position_controller: ros__parameters: @@ -65,3 +68,27 @@ forward_state_controller: - position - velocity - effort + + +spot_forward_controller: + ros__parameters: + joints: + - front_left_hip_x + - front_left_hip_y + - front_left_knee + - front_right_hip_x + - front_right_hip_y + - front_right_knee + - rear_left_hip_x + - rear_left_hip_y + - rear_left_knee + - rear_right_hip_x + - rear_right_hip_y + - rear_right_knee + - arm_sh0 + - arm_sh1 + - arm_el0 + - arm_el1 + - arm_wr0 + - arm_wr1 + - arm_f1x