From 561a7f29a09ed0ef27e7555be94d70d4f5196b67 Mon Sep 17 00:00:00 2001 From: Anthony Baker Date: Fri, 15 Dec 2023 07:39:53 -0700 Subject: [PATCH] pre-commit --- spot_driver/launch/spot_driver.launch.py | 7 +-- spot_driver/spot_driver/spot_ros2.py | 13 +---- .../api/default_robot_state_client.hpp | 8 +-- .../spot_driver_cpp/conversions/geometry.hpp | 16 +++--- .../robot_state_client_interface.hpp | 2 +- .../robot_state/robot_middleware_handle.hpp | 21 ++++---- .../spot_robot_state_publisher.hpp | 10 ++-- .../spot_robot_state_publisher_node.hpp | 3 +- .../include/spot_driver_cpp/types.hpp | 6 +-- .../src/api/default_robot_state_client.cpp | 52 +++++++++---------- spot_driver_cpp/src/api/default_spot_api.cpp | 5 +- spot_driver_cpp/src/conversions/geometry.cpp | 6 +-- .../interfaces/rclcpp_parameter_interface.cpp | 3 +- .../src/interfaces/rclcpp_tf_interface.cpp | 7 +-- .../spot_robot_state_publisher.cpp | 10 ++-- .../mock/mock_robot_state_client.hpp | 3 +- .../test_spot_robot_state_publisher.cpp | 5 +- 17 files changed, 82 insertions(+), 95 deletions(-) diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index 848dfc4a5..c6f78cc67 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/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", diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 2e031999b..0d55f38e6 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/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, diff --git a/spot_driver_cpp/include/spot_driver_cpp/api/default_robot_state_client.hpp b/spot_driver_cpp/include/spot_driver_cpp/api/default_robot_state_client.hpp index fcda214df..22f4abc0a 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/api/default_robot_state_client.hpp +++ b/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, diff --git a/spot_driver_cpp/include/spot_driver_cpp/conversions/geometry.hpp b/spot_driver_cpp/include/spot_driver_cpp/conversions/geometry.hpp index bb025159f..89dfd3d61 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/conversions/geometry.hpp +++ b/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, diff --git a/spot_driver_cpp/include/spot_driver_cpp/interfaces/robot_state_client_interface.hpp b/spot_driver_cpp/include/spot_driver_cpp/interfaces/robot_state_client_interface.hpp index 8fdbc1636..721dcded2 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/interfaces/robot_state_client_interface.hpp +++ b/spot_driver_cpp/include/spot_driver_cpp/interfaces/robot_state_client_interface.hpp @@ -2,8 +2,8 @@ #pragma once -#include #include +#include #include #include diff --git a/spot_driver_cpp/include/spot_driver_cpp/robot_state/robot_middleware_handle.hpp b/spot_driver_cpp/include/spot_driver_cpp/robot_state/robot_middleware_handle.hpp index e8e70e761..6f1044cd6 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/robot_state/robot_middleware_handle.hpp +++ b/spot_driver_cpp/include/spot_driver_cpp/robot_state/robot_middleware_handle.hpp @@ -8,31 +8,30 @@ #include #include #include -#include #include +#include #include #include 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 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; diff --git a/spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher.hpp b/spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher.hpp index 9c7ab4226..b2eae8040 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher.hpp +++ b/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 robot_state_client_interface, diff --git a/spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher_node.hpp b/spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher_node.hpp index 4d0c9fc3f..58f06b556 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/robot_state/spot_robot_state_publisher_node.hpp +++ b/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 spot_api, std::unique_ptr mw_handle); diff --git a/spot_driver_cpp/include/spot_driver_cpp/types.hpp b/spot_driver_cpp/include/spot_driver_cpp/types.hpp index 40592146a..7a602bbb2 100644 --- a/spot_driver_cpp/include/spot_driver_cpp/types.hpp +++ b/spot_driver_cpp/include/spot_driver_cpp/types.hpp @@ -2,12 +2,12 @@ #pragma once -#include -#include #include #include #include #include +#include +#include #include #include #include @@ -19,8 +19,8 @@ #include #include -#include #include +#include namespace spot_ros2 { /** @brief Represents the six different cameras on Spot. */ diff --git a/spot_driver_cpp/src/api/default_robot_state_client.cpp b/spot_driver_cpp/src/api/default_robot_state_client.cpp index 1e0a87677..9528052fa 100644 --- a/spot_driver_cpp/src/api/default_robot_state_client.cpp +++ b/spot_driver_cpp/src/api/default_robot_state_client.cpp @@ -120,24 +120,23 @@ std::optional 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 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 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; } diff --git a/spot_driver_cpp/src/api/default_spot_api.cpp b/spot_driver_cpp/src/api/default_spot_api.cpp index 7a2b7ae15..b0a22daec 100644 --- a/spot_driver_cpp/src/api/default_spot_api.cpp +++ b/spot_driver_cpp/src/api/default_spot_api.cpp @@ -55,10 +55,11 @@ tl::expected 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(robot_state_result.response, time_sync_api_, robot_name_); + robot_state_client_interface_ = + std::make_shared(robot_state_result.response, time_sync_api_, robot_name_); return {}; } diff --git a/spot_driver_cpp/src/conversions/geometry.cpp b/spot_driver_cpp/src/conversions/geometry.cpp index dfea5f520..7d73c2020 100644 --- a/spot_driver_cpp/src/conversions/geometry.cpp +++ b/spot_driver_cpp/src/conversions/geometry.cpp @@ -3,11 +3,11 @@ #include -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 diff --git a/spot_driver_cpp/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver_cpp/src/interfaces/rclcpp_parameter_interface.cpp index 6abe8c370..a96e16e9d 100644 --- a/spot_driver_cpp/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver_cpp/src/interfaces/rclcpp_parameter_interface.cpp @@ -112,8 +112,7 @@ bool RclcppParameterInterface::getPublishDepthRegisteredImages() const { } std::string RclcppParameterInterface::getPreferredOdomFrame() const { - return declareAndGetParameter(node_, kParameterPreferredOdomFrame, - kDefaultPreferredOdomFrame); + return declareAndGetParameter(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame); } std::string RclcppParameterInterface::getSpotName() const { diff --git a/spot_driver_cpp/src/interfaces/rclcpp_tf_interface.cpp b/spot_driver_cpp/src/interfaces/rclcpp_tf_interface.cpp index 0a99a6cc5..1dbebc36a 100644 --- a/spot_driver_cpp/src/interfaces/rclcpp_tf_interface.cpp +++ b/spot_driver_cpp/src/interfaces/rclcpp_tf_interface.cpp @@ -3,9 +3,8 @@ #include namespace spot_ros2 { -RclcppTfInterface::RclcppTfInterface(const std::shared_ptr& node) - : static_tf_broadcaster_{node} - , dynamic_tf_broadcaster_{node} {} +RclcppTfInterface::RclcppTfInterface(const std::shared_ptr& node) + : static_tf_broadcaster_{node}, dynamic_tf_broadcaster_{node} {} tl::expected RclcppTfInterface::updateStaticTransforms( const std::vector& transforms) { @@ -29,10 +28,8 @@ tl::expected RclcppTfInterface::updateStaticTransforms( tl::expected RclcppTfInterface::sendDynamicTransforms( const std::vector& transforms) { - dynamic_tf_broadcaster_.sendTransform(transforms); return {}; - } } // namespace spot_ros2 diff --git a/spot_driver_cpp/src/robot_state/spot_robot_state_publisher.cpp b/spot_driver_cpp/src/robot_state/spot_robot_state_publisher.cpp index 22aa076af..055d94830 100644 --- a/spot_driver_cpp/src/robot_state/spot_robot_state_publisher.cpp +++ b/spot_driver_cpp/src/robot_state/spot_robot_state_publisher.cpp @@ -17,7 +17,7 @@ SpotRobotStatePublisher::SpotRobotStatePublisher( std::shared_ptr robot_state_client_interface, std::unique_ptr middleware_handle) : client_interface_{robot_state_client_interface}, middleware_handle_{std::move(middleware_handle)} { - // Create a publisher for all messages in robot state + // Create a publisher for all messages in robot state middleware_handle_->createPublishers(); // Create a timer to request and publish robot state at a fixed rate @@ -27,9 +27,11 @@ SpotRobotStatePublisher::SpotRobotStatePublisher( } void SpotRobotStatePublisher::timerCallback() { - const auto preferred_odom_frame = middleware_handle_->parameter_interface()->getPreferredOdomFrame().find("/") == std::string::npos ? - middleware_handle_->parameter_interface()->getSpotName() + "/" + middleware_handle_->parameter_interface()->getPreferredOdomFrame() : - middleware_handle_->parameter_interface()->getPreferredOdomFrame(); + const auto preferred_odom_frame = + middleware_handle_->parameter_interface()->getPreferredOdomFrame().find("/") == std::string::npos + ? middleware_handle_->parameter_interface()->getSpotName() + "/" + + middleware_handle_->parameter_interface()->getPreferredOdomFrame() + : middleware_handle_->parameter_interface()->getPreferredOdomFrame(); const auto robot_state_result = client_interface_->getRobotState(preferred_odom_frame); if (!robot_state_result.has_value()) { diff --git a/spot_driver_cpp/test/include/spot_driver_cpp/mock/mock_robot_state_client.hpp b/spot_driver_cpp/test/include/spot_driver_cpp/mock/mock_robot_state_client.hpp index 7e514a303..bcb84b6f5 100644 --- a/spot_driver_cpp/test/include/spot_driver_cpp/mock/mock_robot_state_client.hpp +++ b/spot_driver_cpp/test/include/spot_driver_cpp/mock/mock_robot_state_client.hpp @@ -9,6 +9,7 @@ namespace spot_ros2::test { class MockRobotStateClient : public RobotStateClientInterface { public: - MOCK_METHOD((tl::expected), getRobotState, (const std::string& preferred_odom_frame), (override)); + MOCK_METHOD((tl::expected), getRobotState, (const std::string& preferred_odom_frame), + (override)); }; } // namespace spot_ros2::test diff --git a/spot_driver_cpp/test/src/robot_state/test_spot_robot_state_publisher.cpp b/spot_driver_cpp/test/src/robot_state/test_spot_robot_state_publisher.cpp index d130b0513..88bb6e20c 100644 --- a/spot_driver_cpp/test/src/robot_state/test_spot_robot_state_publisher.cpp +++ b/spot_driver_cpp/test/src/robot_state/test_spot_robot_state_publisher.cpp @@ -99,7 +99,6 @@ TEST_F(TestSpotRobotStatePublisherFixture, InitSucceeds) { // WHEN a robot state publisher is constructed robot_state_publisher = std::make_unique(robot_state_client_interface, std::move(middleware_handle)); - } TEST_F(TestSpotRobotStatePublisherFixture, PublishCallbackTriggers) { @@ -143,7 +142,9 @@ TEST_F(TestSpotRobotStatePublisherFixture, PublishCallbackTriggers_Fail_GetRobot // THEN we request the robot state from the Spot interface // THEN we publish the robot state to the appropriate topics InSequence seq; - EXPECT_CALL(*robot_state_client_interface, getRobotState).Times(1).WillOnce(Return(tl::make_unexpected("Failed to get robot state"))); + EXPECT_CALL(*robot_state_client_interface, getRobotState) + .Times(1) + .WillOnce(Return(tl::make_unexpected("Failed to get robot state"))); EXPECT_CALL(*logger_interface_ptr, logError).Times(1); EXPECT_CALL(*middleware_handle, publishRobotState).Times(0); }