diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d3a2c73..c0c27242 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp + rcl_interfaces rcutils realtime_tools ) @@ -41,28 +42,44 @@ target_include_directories(control_toolbox PUBLIC ament_target_dependencies(control_toolbox PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(control_toolbox PRIVATE "CONTROL_TOOLBOX_BUILDING_LIBRARY") - ######################## # Build control filters ######################## set(CONTROL_FILTERS_INCLUDE_DEPENDS + Eigen3 filters - rclcpp + geometry_msgs generate_parameter_library pluginlib - geometry_msgs - Eigen3 + tf2_geometry_msgs + tf2 + tf2_ros ) foreach(Dependency IN ITEMS ${CONTROL_FILTERS_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach() +generate_parameter_library( + gravity_compensation_filter_parameters + src/control_filters/gravity_compensation_filter_parameters.yaml +) + +add_library(gravity_compensation SHARED + src/control_filters/gravity_compensation.cpp +) +target_compile_features(gravity_compensation PUBLIC cxx_std_17) +target_include_directories(gravity_compensation PUBLIC + $ + $ +) +target_link_libraries(gravity_compensation PUBLIC gravity_compensation_filter_parameters) +ament_target_dependencies(gravity_compensation PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + generate_parameter_library( low_pass_filter_parameters src/control_filters/low_pass_filter_parameters.yaml ) - add_library(low_pass_filter SHARED src/control_filters/low_pass_filter.cpp ) @@ -75,6 +92,7 @@ target_include_directories(low_pass_filter PUBLIC target_link_libraries(low_pass_filter PUBLIC low_pass_filter_parameters) ament_target_dependencies(low_pass_filter PUBLIC ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + # Install pluginlib xml pluginlib_export_plugin_description_file(filters control_filters.xml) @@ -97,6 +115,18 @@ if(BUILD_TESTING) ament_target_dependencies(pid_publisher_tests rclcpp_lifecycle) ## Control Filters + ### Gravity Compensation + add_rostest_with_parameters_gmock(test_gravity_compensation test/control_filters/test_gravity_compensation.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_gravity_compensation_parameters.yaml + ) + target_link_libraries(test_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) + ament_target_dependencies(test_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + ament_add_gmock(test_load_gravity_compensation test/control_filters/test_load_gravity_compensation.cpp) + target_link_libraries(test_load_gravity_compensation gravity_compensation gravity_compensation_filter_parameters) + ament_target_dependencies(test_load_gravity_compensation ${CONTROL_FILTERS_INCLUDE_DEPENDS}) + + ### Low-pass add_rostest_with_parameters_gmock(test_low_pass_filter test/control_filters/test_low_pass_filter.cpp ${CMAKE_CURRENT_SOURCE_DIR}/test/control_filters/test_low_pass_filter_parameters.yaml ) @@ -113,7 +143,7 @@ install( DIRECTORY include/ DESTINATION include/control_toolbox ) -install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters +install(TARGETS control_toolbox low_pass_filter low_pass_filter_parameters gravity_compensation gravity_compensation_filter_parameters EXPORT export_control_toolbox ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/README.md b/README.md index 21acbbc7..0421232d 100644 --- a/README.md +++ b/README.md @@ -27,3 +27,25 @@ doi = {10.21105/joss.00456}, URL = {http://www.theoj.org/joss-papers/joss.00456/10.21105.joss.00456.pdf} } ``` + + +## Code Formatting + +This repository uses `pre-commit` tool for code formatting. +The tool checks formatting each time you commit to a repository. +To install it locally use: + ``` + pip3 install pre-commit # (prepend `sudo` if you want to install it system wide) + ``` + +To run it initially over the whole repo you can use: + ``` + pre-commit run -a + ``` + +If you get error that something is missing on your computer, do the following for: + + - `clang-format-14` + ``` + sudo apt install clang-format-14 + ``` diff --git a/control_filters.xml b/control_filters.xml index a2280008..a541b429 100644 --- a/control_filters.xml +++ b/control_filters.xml @@ -1,18 +1,29 @@ - - - - This is a low pass filter working with a double value. - - - - - This is a low pass filter working with geometry_msgs::WrenchStamped. - - - + + + + This is a gravity compensation filter working with geometry_msgs::WrenchStamped. + It subtracts the influence of a force caused by the gravitational force from force + measurements. + + + + + + + This is a low pass filter working with a double value. + + + + + This is a low pass filter working with geometry_msgs::WrenchStamped. + + + diff --git a/include/control_filters/gravity_compensation.hpp b/include/control_filters/gravity_compensation.hpp new file mode 100644 index 00000000..adfa8f00 --- /dev/null +++ b/include/control_filters/gravity_compensation.hpp @@ -0,0 +1,130 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#ifndef CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ +#define CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ + +#include +#include +#include + +#include "gravity_compensation_filter_parameters.hpp" +#include "filters/filter_base.hpp" +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +namespace control_filters +{ + + +template +class GravityCompensation : public filters::FilterBase +{ +public: + /** \brief Constructor */ + GravityCompensation(); + + /** \brief Destructor */ + ~GravityCompensation(); + + /** @brief Configure filter parameters */ + bool configure() override; + + /** \brief Update the filter and return the data separately + * \param data_in T array with length width + * \param data_out T array with length width + */ + bool update(const T & data_in, T & data_out) override; + +protected: + void compute_internal_params() + { + cog_.vector.x = parameters_.CoG.pos[0]; + cog_.vector.y = parameters_.CoG.pos[1]; + cog_.vector.z = parameters_.CoG.pos[2]; + cst_ext_force_.vector.x = parameters_.CoG.force[0]; + cst_ext_force_.vector.y = parameters_.CoG.force[1]; + cst_ext_force_.vector.z = parameters_.CoG.force[2]; + }; + +private: + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr logger_; + std::shared_ptr parameter_handler_; + gravity_compensation_filter::Params parameters_; + + // Frames for Transformation of Gravity / CoG Vector + std::string world_frame_; // frame in which gravity is given + std::string sensor_frame_; // frame in which Cog is given and compution occur + // Storage for Calibration Values + geometry_msgs::msg::Vector3Stamped cog_; // Center of Gravity Vector (wrt sensor frame) + geometry_msgs::msg::Vector3Stamped cst_ext_force_; // Gravity Force Vector (wrt world frame) + + // Filter objects + std::unique_ptr p_tf_Buffer_; + std::unique_ptr p_tf_Listener_; + geometry_msgs::msg::TransformStamped transform_sensor_datain_, transform_world_dataout_, + transform_data_out_sensor_, transform_sensor_world_; +}; + +template +GravityCompensation::GravityCompensation() +{ +} + +template +GravityCompensation::~GravityCompensation() +{ +} + +template +bool GravityCompensation::configure() +{ + clock_ = std::make_shared(RCL_SYSTEM_TIME); + p_tf_Buffer_.reset(new tf2_ros::Buffer(clock_)); + p_tf_Listener_.reset(new tf2_ros::TransformListener(*p_tf_Buffer_.get(), true)); + + logger_.reset( + new rclcpp::Logger(this->logging_interface_->get_logger().get_child(this->filter_name_))); + + // Initialize the parameter_listener once + if (!parameter_handler_) + { + try + { + parameter_handler_ = + std::make_shared(this->params_interface_, + this->param_prefix_); + } + catch (rclcpp::exceptions::ParameterUninitializedException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + catch (rclcpp::exceptions::InvalidParameterValueException & ex) { + RCLCPP_ERROR((*logger_), "GravityCompensation filter cannot be configured: %s", ex.what()); + parameter_handler_.reset(); + return false; + } + } + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); + + return true; +} + +} // namespace control_filters + +#endif // CONTROL_FILTERS__GRAVITY_COMPENSATION_HPP_ diff --git a/package.xml b/package.xml index 7bbdec0f..f534d51e 100644 --- a/package.xml +++ b/package.xml @@ -26,6 +26,9 @@ rclcpp rcutils realtime_tools + tf2 + tf2_ros + tf2_geometry_msgs ament_cmake_gmock ament_cmake_gtest diff --git a/src/control_filters/README.md b/src/control_filters/README.md index cac37a26..4946c187 100644 --- a/src/control_filters/README.md +++ b/src/control_filters/README.md @@ -2,8 +2,64 @@ ## Available filters +* Gravity Compensation: implements a gravity compensation algorithm, removing the gravity component from the incoming data (Wrench). * Low Pass: implements a low-pass filter based on a time-invariant [Infinite Impulse Response (IIR) filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), for different data types (doubles or wrench). + +## Gravity compensation filter + + This filter implements a gravity compensation algorithm, applied to an `data_in` wrench, computed at a `sensor_frame` in which the center of gravity (CoG) of the to-be-compensated mass is known. + + The filter relies on tf2, and might fail if transforms are missing. + + Note that, for convenience, the filter can perform additional frame changes if data_out frame id is given. + +### Required parameters + +* `world_frame` (ℛw): frame in which the `CoG.force` is represented. +* `sensor_frame` (ℛs): frame in which the `CoG.pos` is defined +* `CoG.pos` (ps): position of the CoG of the mass the filter should compensate for +* `CoG.force` (gw): constant (but updatable) force of gravity at the Cog (typically m.G), defined along axes of the `world_frame` + +### Algorithm + +Given + +* above-required parameters, ℛw, ℛs, ps, gw +* `data_in`, a wrench ℱi = {fi, τi} represented in the `data_in` frame ℛi +* access to tf2 homogeneous transforms: + * Tsi from ℛi to ℛs + * Tsw from ℛw to ℛs + * Tos from ℛs to ℛo + +Compute `data_out` compensated wrench ℱco = {fco, τco} represented in the `data_out` frame ℛo if given, or the `data_in` frame ℛi otherwise, with equations: + +ℱco = Tos.ℱcs, + + +with ℱcs = {fcs, τcs} the compensated wrench in `sensor_frame` (common frame for computation) + +and, + +fcs = fs - Tswgw + +its force and, + +τcs = τs - ps x (Tswgw) + +its torque, and + +ℱs = Tsi.ℱi = {fs, τs} + +the full transform of the input wrench ℱi to sensor frame ℛs + +Remarks : +* a full vector is used for gravity force, to not impose gravity to be only along z of `world_frame`. +* `data_in` frame is usually equal to `sensor_frame`, but could be different since measurement of wrench might occur in another frame. E.g.: measurements are at the **FT sensor flange** = `data_in` frame, but CoG is given in **FT sensor base** = `sensor_frame` (=frame to which it is mounted on the robot), introducing an offset (thickness of the sensor) to be accounted for. +* `data_out` frame is usually `data_in` frame, but for convenience, can be set to any other useful frame. E.g.: wrench expressed in a `control_frame` like the center of a gripper. +* Tsw will only rotate the gw vector, because gravity is a field applied everywhere, and not a wrench (no torque should be induced by transforming from ℛw to ℛs). + + ## Low Pass filter This filter implements a low-pass filter in the form of an [IIR filter](https://en.wikipedia.org/wiki/Infinite_impulse_response), applied to a `data_in` (double or wrench). @@ -18,6 +74,7 @@ The feedforward and feedback coefficients of the IIR filter are computed from th ### Algorithm Given + * above-required parameters, `sf`, `df`, `di` * `data_in`, a double or wrench `x` diff --git a/src/control_filters/gravity_compensation.cpp b/src/control_filters/gravity_compensation.cpp new file mode 100644 index 00000000..b525cdad --- /dev/null +++ b/src/control_filters/gravity_compensation.cpp @@ -0,0 +1,108 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "control_filters/gravity_compensation.hpp" + +#include "geometry_msgs/msg/vector3_stamped.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/LinearMath/Vector3.h" + +namespace control_filters +{ +template <> +bool GravityCompensation::update( + const geometry_msgs::msg::WrenchStamped & data_in, geometry_msgs::msg::WrenchStamped & data_out) +{ + if (!this->configured_) + { + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 2000, "Filter is not configured"); + return false; + } + + // Update internal parameters if required + if (parameter_handler_->is_old(parameters_)) + { + parameters_ = parameter_handler_->get_params(); + compute_internal_params(); + } + + try + { + // transform from data_in frame to sensor_frame + transform_sensor_datain_ = p_tf_Buffer_->lookupTransform(parameters_.sensor_frame, + data_in.header.frame_id, rclcpp::Time()); + + // use data_out frame id for the back transformation, otherwise same is data_in + if (!data_out.header.frame_id.empty()) + { + data_out.header.stamp = data_in.header.stamp; // only copy the timestamp + } + else + { + data_out.header = data_in.header; // keep the same header and same frame_id + } + transform_data_out_sensor_ = p_tf_Buffer_->lookupTransform( + data_out.header.frame_id, parameters_.sensor_frame, rclcpp::Time()); + // transform from world (gravity) frame to sensor frame + transform_sensor_world_ = p_tf_Buffer_->lookupTransform( + parameters_.sensor_frame, parameters_.world_frame, rclcpp::Time()); + } + catch (const tf2::TransformException & ex) + { + std::stringstream frames_sstr; + frames_sstr << "datain:" << data_in.header.frame_id << " dataout:" << data_out.header.frame_id; + frames_sstr << " world:" << parameters_.world_frame << " sensor:" << parameters_.sensor_frame; + RCLCPP_ERROR_SKIPFIRST_THROTTLE((*logger_), *clock_, 5000, + "GravityCompensation update failed:%s, given frames are %s", ex.what(), + frames_sstr.str().c_str()); + return false; // if cannot transform, result of subsequent computations is invalid + } + + // Transform data_in to sensor_frame frame + geometry_msgs::msg::Wrench wrench_sensor; + tf2::doTransform(data_in.wrench, wrench_sensor, transform_sensor_datain_); + + // CoG is already in sensor_frame + + // Rotate (no wrench, just a force) the gravity force to sensor frame + geometry_msgs::msg::Vector3Stamped cst_ext_force_transformed; + tf2::doTransform(cst_ext_force_, cst_ext_force_transformed, transform_sensor_world_); + + // Compensate for gravity force in sensor frame + wrench_sensor.force.x -= cst_ext_force_transformed.vector.x; + wrench_sensor.force.y -= cst_ext_force_transformed.vector.y; + wrench_sensor.force.z -= cst_ext_force_transformed.vector.z; + // Compensate for torque produced by offset CoG in sensor frame + // result from cross-product of cog Vector and force + tf2::Vector3 cog_vector = {cog_.vector.x, cog_.vector.y, cog_.vector.z}; + auto added_torque = cog_vector.cross({cst_ext_force_transformed.vector.x, + cst_ext_force_transformed.vector.y, + cst_ext_force_transformed.vector.z}); + wrench_sensor.torque.x -= added_torque.getX(); + wrench_sensor.torque.y -= added_torque.getY(); + wrench_sensor.torque.z -= added_torque.getZ(); + // Transform wrench_world to data_out frame_id if not empty otherwise to data_in frame id + tf2::doTransform(wrench_sensor, data_out.wrench, transform_data_out_sensor_); + + return true; +} + +} // namespace control_filters + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + control_filters::GravityCompensation, + filters::FilterBase) diff --git a/src/control_filters/gravity_compensation_filter_parameters.yaml b/src/control_filters/gravity_compensation_filter_parameters.yaml new file mode 100644 index 00000000..ebfef4a3 --- /dev/null +++ b/src/control_filters/gravity_compensation_filter_parameters.yaml @@ -0,0 +1,32 @@ +gravity_compensation_filter: + world_frame: { + type: string, + description: "Fixed world frame in which the constant field-induced force is given.", + read_only: true, + validation: { + not_empty<>: [] + }, + } + sensor_frame: { + type: string, + description: "Sensor frame in which center of gravity (CoG) is measured and computation occur.", + read_only: true, + validation: { + not_empty<>: [] + }, + } + CoG: + pos: { + type: double_array, + description: "Position of the CoG of tool attached to the FT sensor in the sensor frame.", + validation: { + fixed_size<>: 3 + }, + } + force: { + type: double_array, + description: "Specifies the constant field-induced force applied at the CoG of tool defined in fixed world frame, e.g [0, 0, -mass * 9.81].", + validation: { + fixed_size<>: 3 + }, + } diff --git a/test/control_filters/test_gravity_compensation.cpp b/test/control_filters/test_gravity_compensation.cpp new file mode 100644 index 00000000..b1e9e08a --- /dev/null +++ b/test/control_filters/test_gravity_compensation.cpp @@ -0,0 +1,111 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 "test_gravity_compensation.hpp" +#include + +TEST_F(GravityCompensationTest, TestGravityCompensationMissingParameters) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // one mandatory param missing, should fail + ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + /* NOTE: one cannot declare or set the missing param afterwards, to then test if configure works, + * because the param is read only and cannot be set anymore. + */ +} + +TEST_F(GravityCompensationTest, TestGravityCompensationInvalidThenFixedParameter) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // wrong vector size, should fail + ASSERT_FALSE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // fixed wrong vector size + node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.0}))); + // all parameters correctly set AND second call to yet unconfigured filter + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + // change a parameter + node_->set_parameter(rclcpp::Parameter("CoG.pos", std::vector({0.0, 0.0, 0.2}))); + // accept second call to configure with valid parameters to already configured filter + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); +} + + +TEST_F(GravityCompensationTest, TestGravityCompensationMissingTransform) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + // all parameters correctly set + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + // should fail due to missing datain frame to sensor frame transform + ASSERT_FALSE(filter_->update(in, out)); +} + +TEST_F(GravityCompensationTest, TestGravityCompensationComputation) +{ + std::shared_ptr> filter_ = + std::make_shared>(); + + double gravity_acc = 9.81; + double mass = 5.0; + + ASSERT_TRUE(filter_->configure("", "TestGravityCompensation", + node_->get_node_logging_interface(), node_->get_node_parameters_interface())); + + geometry_msgs::msg::WrenchStamped in, out; + in.header.frame_id = "world"; + in.wrench.force.x = 1.0; + in.wrench.torque.x = 10.0; + + // should pass (transform is identity) + ASSERT_TRUE(filter_->update(in, out)); + + ASSERT_EQ(out.wrench.force.x, 1.0); + ASSERT_EQ(out.wrench.force.y, 0.0); + ASSERT_NEAR(out.wrench.force.z, gravity_acc * mass, 0.0001); + + ASSERT_EQ(out.wrench.torque.x, 10.0); + ASSERT_EQ(out.wrench.torque.y, 0.0); + ASSERT_EQ(out.wrench.torque.z, 0.0); + + out.header.frame_id = "base"; + // should fail due to missing transform for desired output frame + ASSERT_FALSE(filter_->update(in, out)); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/test/control_filters/test_gravity_compensation.hpp b/test/control_filters/test_gravity_compensation.hpp new file mode 100644 index 00000000..367811d6 --- /dev/null +++ b/test/control_filters/test_gravity_compensation.hpp @@ -0,0 +1,63 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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. + +#ifndef CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ +#define CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ + +#include +#include +#include "gmock/gmock.h" + +#include "control_filters/gravity_compensation.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("test_gravity_compensation"); +} // namespace + +class GravityCompensationTest : public ::testing::Test +{ +public: + void SetUp() override + { + auto testname = ::testing::UnitTest::GetInstance()->current_test_info()->name(); + node_ = std::make_shared(testname); + executor_->add_node(node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + GravityCompensationTest() + { + executor_ = std::make_shared(); + } + + void TearDown() override + { + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + node_.reset(); + } + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; +}; + +#endif // CONTROL_FILTERS__TEST_GRAVITY_COMPENSATION_HPP_ diff --git a/test/control_filters/test_gravity_compensation_parameters.yaml b/test/control_filters/test_gravity_compensation_parameters.yaml new file mode 100644 index 00000000..1224e284 --- /dev/null +++ b/test/control_filters/test_gravity_compensation_parameters.yaml @@ -0,0 +1,35 @@ +TestGravityCompensationAllParameters: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] + +TestGravityCompensationMissingParameters: + ros__parameters: + world_frame: world + +TestGravityCompensationInvalidThenFixedParameter: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0] + +TestGravityCompensationMissingTransform: + ros__parameters: + world_frame: world + sensor_frame: sensor + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] + +TestGravityCompensationComputation: + ros__parameters: + world_frame: world + sensor_frame: world + CoG: + force: [0.0, 0.0, -49.05] + pos: [0.0, 0.0, 0.0] diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp new file mode 100644 index 00000000..12abf276 --- /dev/null +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// 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 +#include +#include +#include "control_filters/gravity_compensation.hpp" +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "rclcpp/utilities.hpp" +#include + + +TEST(TestLoadGravityCompensationFilter, load_gravity_compensation_filter_wrench) +{ + rclcpp::init(0, nullptr); + + pluginlib::ClassLoader> + filter_loader("filters", "filters::FilterBase"); + std::shared_ptr> filter; + auto available_classes = filter_loader.getDeclaredClasses(); + std::stringstream sstr; + sstr << "available filters:" << std::endl; + for (const auto& available_class : available_classes) + { + sstr << " " << available_class << std::endl; + } + + std::string filter_type = "control_filters/GravityCompensationWrench"; + ASSERT_TRUE(filter_loader.isClassAvailable(filter_type)) << sstr.str(); + ASSERT_NO_THROW(filter = filter_loader.createSharedInstance(filter_type)); + + rclcpp::shutdown(); +}