diff --git a/ros_utilities b/ros_utilities index 2dc3c6b04..06077a2fd 160000 --- a/ros_utilities +++ b/ros_utilities @@ -1 +1 @@ -Subproject commit 2dc3c6b04e1ca5b901328a3af705f9baf5dcf82a +Subproject commit 06077a2fdcecc444e0127fe421cca3fbded5b72d diff --git a/spot_controllers/CMakeLists.txt b/spot_controllers/CMakeLists.txt new file mode 100644 index 000000000..469489382 --- /dev/null +++ b/spot_controllers/CMakeLists.txt @@ -0,0 +1,82 @@ +# Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +cmake_minimum_required(VERSION 3.22) + +# This is here so we can use jthread from C++ 20 +set(CMAKE_CXX_STANDARD 20) + +project(spot_controllers) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Dependencies +find_package(ament_cmake REQUIRED) +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + forward_command_controller + pluginlib + rclcpp + rclcpp_lifecycle +) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +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) + ament_lint_auto_find_test_dependencies() +endif() + +generate_parameter_library( + forward_state_controller_parameters + include/spot_controllers/forward_state_controller_parameters.yaml +) + +# Add the hardware interface +add_library( + spot_controllers + SHARED + src/forward_state_controller.cpp +) +target_compile_features(spot_controllers PUBLIC cxx_std_20) +target_include_directories(spot_controllers PUBLIC + $ + $ +) +target_link_libraries( + spot_controllers PUBLIC + forward_state_controller_parameters forward_command_controller::forward_command_controller +) +ament_target_dependencies( + spot_controllers PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "SPOT_CONTROLLERS_BUILDING_DLL") + +# Export controller plugin +pluginlib_export_plugin_description_file(controller_interface spot_controllers.xml) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS spot_controllers forward_state_controller_parameters + EXPORT export_spot_controllers + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_spot_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_package() diff --git a/spot_controllers/README.md b/spot_controllers/README.md new file mode 100644 index 000000000..deeb63c40 --- /dev/null +++ b/spot_controllers/README.md @@ -0,0 +1,7 @@ +# spot_controllers + +This is a ROS 2 package that provides custom ROS 2 controllers that can be used with [spot_ros2_control](../spot_ros2_control/). + +Currently, this package consists of a single generic controller: `spot_controllers/ForwardStateController`. This controller allows you to forward a set of commands over a set of interfaces. It is used with `spot_ros2_control` to forwad commands for position, velocity, and effort for all joints at the same time. + +Example configurations for setting up this controller can be found in [`spot_ros2_control/config`](../spot_ros2_control/config/). diff --git a/spot_controllers/include/spot_controllers/forward_state_controller.hpp b/spot_controllers/include/spot_controllers/forward_state_controller.hpp new file mode 100644 index 000000000..01526d2dd --- /dev/null +++ b/spot_controllers/include/spot_controllers/forward_state_controller.hpp @@ -0,0 +1,57 @@ +// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC. +// All rights reserved. + +// -------------------------------------------------------------- +// Copyright 2020 PAL Robotics S.L. +// +// 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. + +#pragma once + +#include +#include +#include + +#include "forward_command_controller/forward_command_controller/forward_controllers_base.hpp" +#include "forward_state_controller_parameters.hpp" // NOLINT(build/include_subdir) +#include "spot_controllers/visibility_control.h" + +namespace spot_controllers { +/** + * \brief Forward command controller for a set of interfaces. + * + * This class forwards the command signal for a set of interfaces over a set of joints. + * + * \param joints Names of the joint to control. + * \param interface_names Names of the interfaces to command. + * + * Subscribes to: + * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. + */ +class ForwardStateController : public forward_command_controller::ForwardControllersBase { + public: + SPOT_CONTROLLERS_PUBLIC + ForwardStateController(); + + protected: + void declare_parameters() override; + controller_interface::CallbackReturn read_parameters() override; + + using Params = forward_state_controller::Params; + using ParamListener = forward_state_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; +}; + +} // namespace spot_controllers diff --git a/spot_controllers/include/spot_controllers/forward_state_controller_parameters.yaml b/spot_controllers/include/spot_controllers/forward_state_controller_parameters.yaml new file mode 100644 index 000000000..8ae617532 --- /dev/null +++ b/spot_controllers/include/spot_controllers/forward_state_controller_parameters.yaml @@ -0,0 +1,11 @@ +forward_state_controller: + joints: { + type: string_array, + default_value: [], + description: "Names of the joint to control", + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } diff --git a/spot_controllers/include/spot_controllers/visibility_control.h b/spot_controllers/include/spot_controllers/visibility_control.h new file mode 100644 index 000000000..ecc3c30d2 --- /dev/null +++ b/spot_controllers/include/spot_controllers/visibility_control.h @@ -0,0 +1,60 @@ +// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC. +// All rights reserved. + +// -------------------------------------------------------------- +// Copyright 2021 ros2_control Development Team +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +// Source: +// https://github.com/ros-controls/ros2_control_demos/blob/master/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h + +#pragma once + +// 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 SPOT_CONTROLLERS_EXPORT __attribute__((dllexport)) +#define SPOT_CONTROLLERS_IMPORT __attribute__((dllimport)) +#else +#define SPOT_CONTROLLERS_EXPORT __declspec(dllexport) +#define SPOT_CONTROLLERS_IMPORT __declspec(dllimport) +#endif +#ifdef SPOT_CONTROLLERS_BUILDING_DLL +#define SPOT_CONTROLLERS_PUBLIC SPOT_CONTROLLERS_EXPORT +#else +#define SPOT_CONTROLLERS_PUBLIC SPOT_CONTROLLERS_IMPORT +#endif +#define SPOT_CONTROLLERS_PUBLIC_TYPE SPOT_CONTROLLERS_PUBLIC +#define SPOT_CONTROLLERS_LOCAL +#else +#define SPOT_CONTROLLERS_EXPORT __attribute__((visibility("default"))) +#define SPOT_CONTROLLERS_IMPORT +#if __GNUC__ >= 4 +#define SPOT_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) +#define SPOT_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) +#else +#define SPOT_CONTROLLERS_PUBLIC +#define SPOT_CONTROLLERS_LOCAL +#endif +#define SPOT_CONTROLLERS_PUBLIC_TYPE +#endif diff --git a/spot_controllers/package.xml b/spot_controllers/package.xml new file mode 100644 index 000000000..0b441fbbe --- /dev/null +++ b/spot_controllers/package.xml @@ -0,0 +1,24 @@ + + + + spot_controllers + 0.0.0 + ROS 2 controllers that can be used with Spot + khughes + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + controller_interface + forward_command_controller + pluginlib + rclcpp + rclcpp_lifecycle + + + ament_cmake + + diff --git a/spot_controllers/spot_controllers.xml b/spot_controllers/spot_controllers.xml new file mode 100644 index 000000000..e967a8da6 --- /dev/null +++ b/spot_controllers/spot_controllers.xml @@ -0,0 +1,7 @@ + + + + General passthrough controller that can forward commands for a set of joints over a set of interfaces. + + + diff --git a/spot_controllers/src/forward_state_controller.cpp b/spot_controllers/src/forward_state_controller.cpp new file mode 100644 index 000000000..34635c40b --- /dev/null +++ b/spot_controllers/src/forward_state_controller.cpp @@ -0,0 +1,69 @@ +// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC. +// All rights reserved. + +// -------------------------------------------------------------- +// Copyright 2020 PAL Robotics S.L. +// +// 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. + +#include "spot_controllers/forward_state_controller.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace spot_controllers { +ForwardStateController::ForwardStateController() : forward_command_controller::ForwardControllersBase() {} + +void ForwardStateController::declare_parameters() { + param_listener_ = std::make_shared(get_node()); +} + +controller_interface::CallbackReturn ForwardStateController::read_parameters() { + if (!param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + if (params_.joints.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.interface_names.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + // Example: if you input joints [1,2,3] and interfaces [A,B,C] as parameters, the order of the command will be + // [1/A, 1/B, 1/C, 2/A, 2/B, 2/C, 3/A, 3/B, 3/C] + for (const auto& interface_name : params_.interface_names) { + for (const auto& joint : params_.joints) { + command_interface_types_.push_back(joint + "/" + interface_name); + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +} // namespace spot_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(spot_controllers::ForwardStateController, controller_interface::ControllerInterface) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 3818145af..bddec9d55 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -569,6 +569,9 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.create_subscription( JointState, "arm_joint_commands", self.arm_joint_cmd_callback, 100, callback_group=self.group ) + self.create_subscription( + PoseStamped, "arm_pose_commands", self.arm_pose_cmd_callback, 100, callback_group=self.group + ) if not self.gripperless: self.create_service( @@ -2530,7 +2533,7 @@ def cmd_velocity_callback(self, data: Twist) -> None: def body_pose_callback(self, data: Pose) -> None: """Callback for cmd_vel command""" if self.spot_wrapper is None: - self.get_logger().info("Mock mode, received command vel " + str(data)) + self.get_logger().info("Mock mode, received command pose " + str(data)) return q = Quaternion() q.x = data.orientation.x @@ -2574,6 +2577,27 @@ def arm_joint_cmd_callback(self, data: JointState) -> None: self.spot_wrapper.arm_joint_cmd(**arm_joint_map) + def arm_pose_cmd_callback(self, data: PoseStamped) -> None: + if not self.spot_wrapper: + self.get_logger().info(f"Mock mode, received arm pose command {data}") + return + result = self.spot_wrapper.spot_arm.hand_pose( + x=data.pose.position.x, + y=data.pose.position.y, + z=data.pose.position.z, + qx=data.pose.orientation.x, + qy=data.pose.orientation.y, + qz=data.pose.orientation.z, + qw=data.pose.orientation.w, + ref_frame=data.header.frame_id, + duration=self.cmd_duration, + ensure_power_on_and_stand=False, + ) + if not result[0]: + self.get_logger().warning(f"Failed to go to arm pose: {result[1]}") + else: + self.get_logger().info("Successfully went to arm pose") + def handle_graph_nav_get_localization_pose( self, request: GraphNavGetLocalizationPose.Response, diff --git a/spot_driver/src/image_stitcher/image_stitcher.cpp b/spot_driver/src/image_stitcher/image_stitcher.cpp index 2b155ccc8..75c7771fc 100644 --- a/spot_driver/src/image_stitcher/image_stitcher.cpp +++ b/spot_driver/src/image_stitcher/image_stitcher.cpp @@ -327,8 +327,8 @@ Image::SharedPtr MiddleCamera::stitch(const std::shared_ptr& left, // While the image is coming from the camera on the left of the robot, it sees the right side // of the scene and vice versa. This may need to be extracted if this code is to be generalized // for something other than the Boston Dynamics Spot Robot, as well as checking the homographies. - const auto scene_right = cv_bridge::toCvShare(left); - const auto scene_left = cv_bridge::toCvShare(right); + const auto scene_right = cv_bridge::toCvShare(left, sensor_msgs::image_encodings::BGR8); + const auto scene_left = cv_bridge::toCvShare(right, sensor_msgs::image_encodings::BGR8); // Transform the images into the virtual center camera space cv::warpPerspective(scene_left->image, warped_images_[0], homography_[0], result_size_); diff --git a/spot_ros2_control/CMakeLists.txt b/spot_ros2_control/CMakeLists.txt index eb51cfd4e..a6c878f7b 100644 --- a/spot_ros2_control/CMakeLists.txt +++ b/spot_ros2_control/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(ament_cmake REQUIRED) find_package(bosdyn REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface - controller_interface controller_manager pluginlib rclcpp @@ -27,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS sensor_msgs std_msgs spot_description + spot_controllers ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -48,8 +48,8 @@ add_library( ) target_compile_features(spot_ros2_control PUBLIC cxx_std_20) target_include_directories(spot_ros2_control PUBLIC -$ -$ + $ + $ ) target_link_libraries(spot_ros2_control PUBLIC bosdyn::bosdyn_client_static) ament_target_dependencies( @@ -61,24 +61,17 @@ ament_target_dependencies( # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "SPOT_ROS2_CONTROL_BUILDING_DLL") -# Export hardware plugins +# Export hardware plugin pluginlib_export_plugin_description_file(hardware_interface spot_ros2_control.xml) # Add example nodes and install them add_executable(noarm_squat examples/src/noarm_squat.cpp) -# ament_target_dependencies(noarm_squat -# ${THIS_PACKAGE_INCLUDE_DEPENDS} -# ) target_link_libraries(noarm_squat spot_ros2_control) + add_executable(wiggle_arm examples/src/wiggle_arm.cpp) -# ament_target_dependencies(wiggle_arm -# ${THIS_PACKAGE_INCLUDE_DEPENDS} -# ) target_link_libraries(wiggle_arm spot_ros2_control) + add_executable(joint_command_passthrough examples/src/joint_command_passthrough.cpp) -# ament_target_dependencies(joint_command_passthrough -# ${THIS_PACKAGE_INCLUDE_DEPENDS} -# ) target_link_libraries(joint_command_passthrough spot_ros2_control) install(TARGETS noarm_squat wiggle_arm joint_command_passthrough diff --git a/spot_ros2_control/README.md b/spot_ros2_control/README.md index 438287db8..b1f74ddaa 100644 --- a/spot_ros2_control/README.md +++ b/spot_ros2_control/README.md @@ -28,10 +28,12 @@ If you wish to launch these nodes in a namespace, add the argument `spot_name:=< This hardware interface will stream the joint angles of the robot using the low level API at 333 Hz onto the topic `//low_level/joint_states`. -Commands can be sent on the topic `//forward_position_controller/commands`. This will forward position commands directly to the spot sdk through the hardware interface. +Commands can be sent on the topic `//forward_position_controller/commands`. This will forward position commands directly to the spot sdk through the hardware interface. The controller expects the command array to contain the list of positions to forward for each joint on the robot. + +An alternative feed-forward controller provided by the `spot_controllers` package can be used to specify the position, velocity, and effort of all joints at the same time. To bring up this controller, add the launch argument `robot_controller:=forward_state_controller`. Commands can then be sent on the topic `//forward_state_controller/commands`. This controller expects the ordering of the command array to be `[, , ]`. > [!CAUTION] -> When using the forward position controller, there is no safety mechanism in place to ensure smooth motion. The ordering of the command must match the ordering of the joints specified in the controller configuration file ([here for robots with an arm](config/spot_default_controllers_with_arm.yaml) or [here for robots without an arm](config/spot_default_controllers_without_arm.yaml)), and the robot can move in unpredictable and dangerous ways if this is not set correctly. Make sure to keep a safe distance from the robot when working with this controller and ensure the e-stop can easily be pressed if needed. +> When using the forward position and state controllers, there is no safety mechanism in place to ensure smooth motion. The ordering of the command must match the ordering of the joints specified in the controller configuration file ([here for robots with an arm](config/spot_default_controllers_with_arm.yaml) or [here for robots without an arm](config/spot_default_controllers_without_arm.yaml)), and the robot can move in unpredictable and dangerous ways if this is not set correctly. Make sure to keep a safe distance from the robot when working with these controllers and ensure the e-stop can easily be pressed if needed. Additionally, the state publisher node, object synchronization node, and image publisher nodes from [`spot_driver`](../spot_driver/) will get launched by default when running on the robot to provide extra information such as odometry topics and camera feeds. To turn off the image publishers (which can cause problems with bandwidth), add the launch argument `launch_image_publishers:=false`. 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 dd9a50148..557322fea 100644 --- a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml +++ b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml @@ -11,6 +11,9 @@ controller_manager: forward_position_controller: type: forward_command_controller/ForwardCommandController + forward_state_controller: + type: spot_controllers/ForwardStateController + forward_position_controller: ros__parameters: @@ -35,3 +38,30 @@ forward_position_controller: - arm_wr1 - arm_f1x interface_name: position + +forward_state_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 + interface_names: + - position + - velocity + - effort diff --git a/spot_ros2_control/config/spot_default_controllers_without_arm.yaml b/spot_ros2_control/config/spot_default_controllers_without_arm.yaml index 33e72c16d..a462a4bbb 100644 --- a/spot_ros2_control/config/spot_default_controllers_without_arm.yaml +++ b/spot_ros2_control/config/spot_default_controllers_without_arm.yaml @@ -11,6 +11,9 @@ controller_manager: forward_position_controller: type: forward_command_controller/ForwardCommandController + forward_state_controller: + type: spot_controllers/ForwardStateController + forward_position_controller: ros__parameters: @@ -28,3 +31,23 @@ forward_position_controller: - rear_right_hip_y - rear_right_knee interface_name: position + +forward_state_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 + interface_names: + - position + - velocity + - effort diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index 82aa9eb1a..0d68251c0 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/spot_ros2_control/launch/spot_ros2_control.launch.py @@ -34,8 +34,8 @@ def create_controllers_config(spot_name: str, has_arm: bool) -> str: Args: spot_name (str): Name of spot. If it's the empty string, the default controller file with no namespace is used. - has_arm (bool): Whether or not your robot has an arm. Necessary for defining the joints that the forward - position controller should use. + has_arm (bool): Whether or not your robot has an arm. Necessary for defining the joints that the controllers + should use. Returns: str: Path to controllers config file to use @@ -49,14 +49,15 @@ def create_controllers_config(spot_name: str, has_arm: bool) -> str: if spot_name: with open(template_filename, "r") as template_file: config = yaml.safe_load(template_file) - forward_position_controller_joints = config["forward_position_controller"]["ros__parameters"]["joints"] - config["forward_position_controller"]["ros__parameters"]["joints"] = [ - f"{spot_name}/{joint}" for joint in forward_position_controller_joints - ] config[f"{spot_name}/controller_manager"] = config["controller_manager"] del config["controller_manager"] - config[f"{spot_name}/forward_position_controller"] = config["forward_position_controller"] - del config["forward_position_controller"] + keys_to_namespace = ["forward_position_controller", "forward_state_controller"] + + for key in keys_to_namespace: + key_joints = config[key]["ros__parameters"]["joints"] + config[key]["ros__parameters"]["joints"] = [f"{spot_name}/{joint}" for joint in key_joints] + config[f"{spot_name}/{key}"] = config[key] + del config[key] with NamedTemporaryFile(suffix=".yaml", mode="w", delete=False) as out_file: yaml.dump(config, out_file) diff --git a/spot_ros2_control/package.xml b/spot_ros2_control/package.xml index c29cd633d..1f1e8a670 100644 --- a/spot_ros2_control/package.xml +++ b/spot_ros2_control/package.xml @@ -12,11 +12,19 @@ Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. ament_cmake - controller_manager + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + bosdyn std_msgs sensor_msgs - spot_description - spot_driver + + controller_manager + spot_description + spot_driver + spot_controllers + xacro ament_lint_auto ament_lint_common