diff --git a/CMakeLists.txt b/CMakeLists.txt index b0e600c9..276f1fa4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -101,6 +101,10 @@ if(BUILD_TESTING) 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}) + endif() # Install diff --git a/test/control_filters/test_load_gravity_compensation.cpp b/test/control_filters/test_load_gravity_compensation.cpp index 22f66c1e..d32851d2 100644 --- a/test/control_filters/test_load_gravity_compensation.cpp +++ b/test/control_filters/test_load_gravity_compensation.cpp @@ -12,30 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. -// #include -// -// #include -// -// #include "controller_manager/controller_manager.hpp" -// #include "hardware_interface/resource_manager.hpp" -// #include "rclcpp/executor.hpp" -// #include "rclcpp/executors/single_threaded_executor.hpp" -// #include "rclcpp/utilities.hpp" -// #include "ros2_control_test_assets/descriptions.hpp" -// -// TEST(TestLoadAdmittanceController, load_controller) -// { -// rclcpp::init(0, nullptr); -// -// std::shared_ptr executor = -// std::make_shared(); -// -// controller_manager::ControllerManager cm(std::make_unique( -// ros2_control_test_assets::minimal_robot_urdf), -// executor, "test_controller_manager"); -// -// ASSERT_NO_THROW( -// cm.load_controller( -// "test_admittance_controller", -// "admittance_controller/AdmittanceController")); -// } +#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(); +}