Merge branch 'main' into mhidalgo-bdai/bump-spot-sdk

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
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/spot_controllers>
+)
+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()

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/).

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 <memory>
+#include <string>
+#include <vector>
+
+#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<ParamListener> param_listener_;
+  Params params_;
+};
+
+}  // namespace spot_controllers

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",
+  }

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

spot_controllers/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>spot_controllers</name>
+  <version>0.0.0</version>
+  <description>ROS 2 controllers that can be used with Spot</description>
+  <maintainer email="[email protected]">khughes</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>controller_interface</depend>
+  <depend>forward_command_controller</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

spot_controllers/spot_controllers.xml

@@ -0,0 +1,7 @@
+<library path="spot_controllers">
+  <class name="spot_controllers/ForwardStateController" type="spot_controllers::ForwardStateController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    General passthrough controller that can forward commands for a set of joints over a set of interfaces.
+  </description>
+  </class>
+</library>

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 <algorithm>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#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<ParamListener>(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)

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,

spot_driver/src/image_stitcher/image_stitcher.cpp

@@ -327,8 +327,8 @@ Image::SharedPtr MiddleCamera::stitch(const std::shared_ptr<const Image>& 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_);