pre-commit

spot_driver/launch/spot_driver.launch.py

@@ -163,7 +163,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
         "mock_enable": mock_enable,
         "publish_depth_registered": False,
         "publish_depth": False,
-        "publish_rgb": False
+        "publish_rgb": False,
     }
 
     if mock_enable:
@@ -226,10 +226,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
     )
     ld.add_action(robot_state_publisher)
 
-    spot_robot_state_publisher_params = {
-        "spot_name": spot_name,
-        "preferred_odom_frame": "odom"
-    }
+    spot_robot_state_publisher_params = {"spot_name": spot_name, "preferred_odom_frame": "odom"}
     spot_robot_state_publisher = launch_ros.actions.Node(
         package="spot_driver_cpp",
         executable="spot_robot_state_publisher_node",

spot_driver/spot_driver/spot_ros2.py

@@ -43,7 +43,6 @@
     FullBodyCommandFeedback,
     GripperCommandFeedback,
     ManipulationApiFeedbackResponse,
-    ManipulatorState,
     MobilityCommandFeedback,
     RobotCommandFeedback,
     RobotCommandFeedbackStatusStatus,
@@ -53,11 +52,8 @@
     PoseStamped,
     TransformStamped,
     Twist,
-    TwistWithCovarianceStamped,
-    Vector3Stamped,
 )
 from google.protobuf.timestamp_pb2 import Timestamp
-from nav_msgs.msg import Odometry
 from rclpy import Parameter
 from rclpy.action import ActionServer
 from rclpy.action.server import ServerGoalHandle
@@ -70,7 +66,7 @@
 from rclpy.impl import rcutils_logger
 from rclpy.publisher import Publisher
 from rclpy.timer import Rate
-from sensor_msgs.msg import CameraInfo, Image, JointState
+from sensor_msgs.msg import CameraInfo, Image
 from std_srvs.srv import SetBool, Trigger
 
 import spot_driver.conversions as conv
@@ -85,18 +81,11 @@
 )
 from spot_msgs.action import Manipulation, NavigateTo, RobotCommand, Trajectory  # type: ignore
 from spot_msgs.msg import (  # type: ignore
-    BatteryStateArray,
-    BehaviorFaultState,
-    EStopStateArray,
     Feedback,
-    FootStateArray,
     LeaseArray,
     LeaseResource,
     Metrics,
     MobilityParams,
-    PowerState,
-    SystemFaultState,
-    WiFiState,
 )
 from spot_msgs.srv import (  # type: ignore
     ChoreographyRecordedStateToAnimation,

spot_driver_cpp/include/spot_driver_cpp/api/default_robot_state_client.hpp

@@ -13,11 +13,13 @@ namespace spot_ros2 {
 
 class DefaultRobotStateClient : public RobotStateClientInterface {
  public:
-   /**
+  /**
    * @brief constructor for DefaultRobotStateClient.
    *
-   * @param client pointer to Spot's RobotStateClient. A DefaultRobotStateClient SHOULD NOT delete this pointer since it does not take ownership.
-   * @param time_sync_api A shared_ptr to a time_sync_api. This allows the client to retreive a Spot's current time and clock skew
+   * @param client pointer to Spot's RobotStateClient. A DefaultRobotStateClient SHOULD NOT delete this pointer since it
+   * does not take ownership.
+   * @param time_sync_api A shared_ptr to a time_sync_api. This allows the client to retreive a Spot's current time and
+   * clock skew
    * @param robot_name Name of Spot. Used to apply frame_prefix
    */
   explicit DefaultRobotStateClient(::bosdyn::client::RobotStateClient* client,

spot_driver_cpp/include/spot_driver_cpp/conversions/geometry.hpp

@@ -9,14 +9,14 @@
 
 namespace spot_ros2::conversions {
 
- /**
-   * @brief Convenience method to convert SE3Pose messages to ROS TransformStamped messages
-   * 
-   * @param transform Spot SE3Pose proto message
-   * @param parent_frame transform's parent frame
-   * @param child_frame transform's child frame
-   * @param tf_time transform's timestamp
-   */
+/**
+ * @brief Convenience method to convert SE3Pose messages to ROS TransformStamped messages
+ *
+ * @param transform Spot SE3Pose proto message
+ * @param parent_frame transform's parent frame
+ * @param child_frame transform's child frame
+ * @param tf_time transform's timestamp
+ */
 
 geometry_msgs::msg::TransformStamped toTransformStamped(const ::bosdyn::api::SE3Pose& transform,
                                                         const std::string& parent_frame, const std::string& child_frame,

spot_driver_cpp/include/spot_driver_cpp/interfaces/robot_state_client_interface.hpp

@@ -2,8 +2,8 @@
 
 #pragma once
 
-#include <tl_expected/expected.hpp>
 #include <spot_driver_cpp/types.hpp>
+#include <tl_expected/expected.hpp>
 
 #include <optional>
 #include <string>

spot_driver_cpp/include/spot_driver_cpp/robot_state/robot_middleware_handle.hpp

@@ -8,31 +8,30 @@
 #include <spot_driver_cpp/interfaces/rclcpp_tf_interface.hpp>
 #include <spot_driver_cpp/interfaces/rclcpp_wall_timer_interface.hpp>
 #include <spot_driver_cpp/robot_state/spot_robot_state_publisher.hpp>
-#include <tl_expected/expected.hpp>
 #include <spot_driver_cpp/types.hpp>
+#include <tl_expected/expected.hpp>
 
 #include <memory>
 #include <string>
 
 namespace spot_ros2 {
 
-  /**
-   * @brief pProduction implementation of a SpotRobotStatePublisher::MiddlewareHandle
-   */ 
+/**
+ * @brief pProduction implementation of a SpotRobotStatePublisher::MiddlewareHandle
+ */
 
 class RobotMiddlewareHandle : public SpotRobotStatePublisher::MiddlewareHandle {
  public:
-
   /**
    * @brief Constructor for RobotMiddlewareHandle.
-   * 
+   *
    * @param node A shared instance to a rclcpp::node
-   */ 
+   */
   explicit RobotMiddlewareHandle(std::shared_ptr<rclcpp::Node> node);
-  
+
   /**
    * @brief Delegating constructor for RobotMiddlewareHandle.
-   * 
+   *
    * @param node_options configuration options for a rclcpp::node
    */
   explicit RobotMiddlewareHandle(const rclcpp::NodeOptions& node_options = rclcpp::NodeOptions{});
@@ -41,12 +40,12 @@ class RobotMiddlewareHandle : public SpotRobotStatePublisher::MiddlewareHandle {
 
   /**
    * @brief Create ROS publishers for Robot State
-   */ 
+   */
   void createPublishers() override;
 
   /**
    * @brief Publish robot state messages
-   */ 
+   */
 
   void publishRobotState(const RobotState& robot_state) override;
 

spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher.hpp

@@ -23,7 +23,7 @@ namespace spot_ros2 {
  */
 class SpotRobotStatePublisher {
  public:
-   /**
+  /**
    * @brief A handle that enables dependency injection of ROS and rclcpp::node operations
    */
   class MiddlewareHandle {
@@ -42,10 +42,12 @@ class SpotRobotStatePublisher {
 
   /**
    * @brief Constructor for SpotRobotStatePublisher.
-   * @details As opposed to other Spot Publishers, initialization takes place inside of the constructor because initialization cannot fail.
+   * @details As opposed to other Spot Publishers, initialization takes place inside of the constructor because
+   * initialization cannot fail.
    *
-   * @param robot_state_client_interface a shared instance of a RobotStateClientInterface. 
-   * @param middleware_handle A unique instance of a MiddlewareHandle that SpotRobotStatePublisher will take ownership of
+   * @param robot_state_client_interface a shared instance of a RobotStateClientInterface.
+   * @param middleware_handle A unique instance of a MiddlewareHandle that SpotRobotStatePublisher will take ownership
+   * of
    */
 
   SpotRobotStatePublisher(std::shared_ptr<RobotStateClientInterface> robot_state_client_interface,

spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher_node.hpp

@@ -22,7 +22,8 @@ class SpotRobotStatePublisherNode {
    * @details This constructor enables dependency inversion for improved testibility
    *
    * @param spot_api a unique_ptr of a SpotApi instance that this SpotRobotStatePublisherNode will take ownership of
-   * @param mw_handle a unique_ptr of a SpotRobotStatePublisher::MiddlewareHandle instance that this SpotRobotStatePublisherNode will take ownership of
+   * @param mw_handle a unique_ptr of a SpotRobotStatePublisher::MiddlewareHandle instance that this
+   * SpotRobotStatePublisherNode will take ownership of
    */
   SpotRobotStatePublisherNode(std::unique_ptr<SpotApi> spot_api,
                               std::unique_ptr<SpotRobotStatePublisher::MiddlewareHandle> mw_handle);

spot_driver_cpp/include/spot_driver_cpp/types.hpp

@@ -2,12 +2,12 @@
 
 #pragma once
 
-#include <sensor_msgs/msg/camera_info.hpp>
-#include <sensor_msgs/msg/image.hpp>
 #include <bosdyn_msgs/msg/manipulator_state.hpp>
 #include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <spot_msgs/msg/battery_state_array.hpp>
 #include <spot_msgs/msg/behavior_fault_state.hpp>
@@ -19,8 +19,8 @@
 #include <tf2_msgs/msg/tf_message.hpp>
 
 #include <functional>
-#include <string>
 #include <optional>
+#include <string>
 
 namespace spot_ros2 {
 /** @brief Represents the six different cameras on Spot. */

spot_driver_cpp/src/api/default_robot_state_client.cpp

@@ -120,24 +120,23 @@ std::optional<tf2_msgs::msg::TFMessage> GetTf(const ::bosdyn::api::RobotState& r
     tf2_msgs::msg::TFMessage tf_msg;
 
     const auto local_time =
-          spot_ros2::applyClockSkew(robot_state.kinematic_state().acquisition_timestamp(), clock_skew);
+        spot_ros2::applyClockSkew(robot_state.kinematic_state().acquisition_timestamp(), clock_skew);
 
     for (const auto& [frame_id, transform] :
          robot_state.kinematic_state().transforms_snapshot().child_to_parent_edge_map()) {
       // Do not publish frames without parents
-      if(transform.parent_frame_name().empty()){
+      if (transform.parent_frame_name().empty()) {
         continue;
       }
-      const auto parent_frame_name = transform.parent_frame_name().find("/") == std::string::npos ?
-        prefix + transform.parent_frame_name() : transform.parent_frame_name();
-      const auto frame_name = frame_id.find("/") == std::string::npos ?
-        prefix + frame_id : frame_id;
-      
-      // set target frame(preferred odom frame) as the root node in tf tree 
+      const auto parent_frame_name = transform.parent_frame_name().find("/") == std::string::npos
+                                         ? prefix + transform.parent_frame_name()
+                                         : transform.parent_frame_name();
+      const auto frame_name = frame_id.find("/") == std::string::npos ? prefix + frame_id : frame_id;
+
+      // set target frame(preferred odom frame) as the root node in tf tree
       if (inverse_target_frame_id == frame_name) {
         tf_msg.transforms.push_back(spot_ros2::conversions::toTransformStamped(
-            ~(transform.parent_tform_child()), frame_name, parent_frame_name,
-            local_time));
+            ~(transform.parent_tform_child()), frame_name, parent_frame_name, local_time));
       } else {
         tf_msg.transforms.push_back(spot_ros2::conversions::toTransformStamped(
             transform.parent_tform_child(), parent_frame_name, frame_name, local_time));
@@ -175,7 +174,7 @@ std::optional<nav_msgs::msg::Odometry> GetOdom(const ::bosdyn::api::RobotState&
     nav_msgs::msg::Odometry odom_msg;
     ::bosdyn::api::SE3Pose tf_body_pose;
     geometry_msgs::msg::PoseWithCovariance pose_odom_msg;
-    
+
     odom_msg.header.stamp =
         spot_ros2::applyClockSkew(robot_state.kinematic_state().acquisition_timestamp(), clock_skew);
     if (is_using_vision) {
@@ -362,23 +361,20 @@ tl::expected<RobotState, std::string> DefaultRobotStateClient::getRobotState(con
 
   const auto robot_state = get_robot_state_result.response.robot_state();
 
-  const auto out =
-      RobotState{GetBatteryStates(robot_state, clock_skew_result.value()),
-                 GetWifiState(robot_state),
-                 GetFootState(robot_state),
-                 GetEstopStates(robot_state, clock_skew_result.value()),
-                 GetJointStates(robot_state, clock_skew_result.value(),
-                                frame_prefix_),
-                 GetTf(robot_state, clock_skew_result.value(), frame_prefix_,
-                       preferred_odom_frame),
-                 GetOdomTwist(robot_state, clock_skew_result.value()),
-                 GetOdom(robot_state, clock_skew_result.value(), frame_prefix_,
-                         preferred_odom_frame == frame_prefix_ + "vision"),
-                 GetPowerState(robot_state, clock_skew_result.value()),
-                 GetSystemFaultState(robot_state, clock_skew_result.value()),
-                 GetManipulatorState(robot_state),
-                 GetEndEffectorForce(robot_state, clock_skew_result.value(), frame_prefix_),
-                 GetBehaviorFaultState(robot_state, clock_skew_result.value())};
+  const auto out = RobotState{
+      GetBatteryStates(robot_state, clock_skew_result.value()),
+      GetWifiState(robot_state),
+      GetFootState(robot_state),
+      GetEstopStates(robot_state, clock_skew_result.value()),
+      GetJointStates(robot_state, clock_skew_result.value(), frame_prefix_),
+      GetTf(robot_state, clock_skew_result.value(), frame_prefix_, preferred_odom_frame),
+      GetOdomTwist(robot_state, clock_skew_result.value()),
+      GetOdom(robot_state, clock_skew_result.value(), frame_prefix_, preferred_odom_frame == frame_prefix_ + "vision"),
+      GetPowerState(robot_state, clock_skew_result.value()),
+      GetSystemFaultState(robot_state, clock_skew_result.value()),
+      GetManipulatorState(robot_state),
+      GetEndEffectorForce(robot_state, clock_skew_result.value(), frame_prefix_),
+      GetBehaviorFaultState(robot_state, clock_skew_result.value())};
 
   return out;
 }

spot_driver_cpp/src/api/default_spot_api.cpp

@@ -55,10 +55,11 @@ tl::expected<void, std::string> DefaultSpotApi::authenticate(const std::string&
 
   const auto robot_state_result = robot_->EnsureServiceClient<::bosdyn::client::RobotStateClient>(
       ::bosdyn::client::RobotStateClient::GetDefaultServiceName());
-  if(!robot_state_result.status){
+  if (!robot_state_result.status) {
     return tl::make_unexpected("Failed to get robot state service client.");
   }
-  robot_state_client_interface_ = std::make_shared<DefaultRobotStateClient>(robot_state_result.response, time_sync_api_, robot_name_);
+  robot_state_client_interface_ =
+      std::make_shared<DefaultRobotStateClient>(robot_state_result.response, time_sync_api_, robot_name_);
 
   return {};
 }

spot_driver_cpp/src/conversions/geometry.cpp

@@ -3,11 +3,11 @@
 
 #include <spot_driver_cpp/conversions/geometry.hpp>
 
-namespace spot_ros2::conversions{
+namespace spot_ros2::conversions {
 
 geometry_msgs::msg::TransformStamped toTransformStamped(const ::bosdyn::api::SE3Pose& transform,
                                                         const std::string& parent_frame, const std::string& child_frame,
-                                                        const builtin_interfaces::msg::Time& tf_time){
+                                                        const builtin_interfaces::msg::Time& tf_time) {
   geometry_msgs::msg::TransformStamped tf;
   tf.header.stamp = tf_time;
   tf.header.frame_id = parent_frame;
@@ -24,4 +24,4 @@ geometry_msgs::msg::TransformStamped toTransformStamped(const ::bosdyn::api::SE3
 
   return tf;
 }
-} // namespace spot_ros2::conversions
+}  // namespace spot_ros2::conversions

spot_driver_cpp/src/interfaces/rclcpp_parameter_interface.cpp

@@ -112,8 +112,7 @@ bool RclcppParameterInterface::getPublishDepthRegisteredImages() const {
 }
 
 std::string RclcppParameterInterface::getPreferredOdomFrame() const {
-  return declareAndGetParameter<std::string>(node_, kParameterPreferredOdomFrame,
-                                      kDefaultPreferredOdomFrame);
+  return declareAndGetParameter<std::string>(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame);
 }
 
 std::string RclcppParameterInterface::getSpotName() const {

spot_driver_cpp/src/interfaces/rclcpp_tf_interface.cpp

@@ -3,9 +3,8 @@
 #include <spot_driver_cpp/interfaces/rclcpp_tf_interface.hpp>
 
 namespace spot_ros2 {
-RclcppTfInterface::RclcppTfInterface(const std::shared_ptr<rclcpp::Node>& node) 
-  : static_tf_broadcaster_{node}
-  , dynamic_tf_broadcaster_{node} {}
+RclcppTfInterface::RclcppTfInterface(const std::shared_ptr<rclcpp::Node>& node)
+    : static_tf_broadcaster_{node}, dynamic_tf_broadcaster_{node} {}
 
 tl::expected<void, std::string> RclcppTfInterface::updateStaticTransforms(
     const std::vector<geometry_msgs::msg::TransformStamped>& transforms) {
@@ -29,10 +28,8 @@ tl::expected<void, std::string> RclcppTfInterface::updateStaticTransforms(
 
 tl::expected<void, std::string> RclcppTfInterface::sendDynamicTransforms(
     const std::vector<geometry_msgs::msg::TransformStamped>& transforms) {
-
   dynamic_tf_broadcaster_.sendTransform(transforms);
   return {};
-
 }
 
 }  // namespace spot_ros2