|
1 | | -// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt) |
| 1 | +// Copyright (c) 2025, b»robotized by Stogl Robotics |
2 | 2 | // |
3 | 3 | // Licensed under the Apache License, Version 2.0 (the "License"); |
4 | 4 | // you may not use this file except in compliance with the License. |
|
12 | 12 | // See the License for the specific language governing permissions and |
13 | 13 | // limitations under the License. |
14 | 14 |
|
15 | | -// |
16 | | -// Source of this file are templates in |
17 | | -// [RosTeamWorkspace](https://github.com/StoglRobotics/ros_team_workspace) repository. |
18 | | -// |
19 | | - |
20 | 15 | #ifndef TEST_IO_GRIPPER_CONTROLLER_HPP_ |
21 | 16 | #define TEST_IO_GRIPPER_CONTROLLER_HPP_ |
22 | 17 |
|
| 18 | +// #include "io_gripper_controller/io_gripper_controller.hpp" |
| 19 | + |
| 20 | +#include <gmock/gmock.h> |
| 21 | + |
23 | 22 | #include <chrono> |
24 | 23 | #include <limits> |
25 | 24 | #include <memory> |
|
28 | 27 | #include <utility> |
29 | 28 | #include <vector> |
30 | 29 |
|
31 | | -#include "gmock/gmock.h" |
32 | | -#include "hardware_interface/loaned_command_interface.hpp" |
33 | | -#include "hardware_interface/loaned_state_interface.hpp" |
34 | | -#include "hardware_interface/types/hardware_interface_return_values.hpp" |
35 | | -#include "io_gripper_controller/io_gripper_controller.hpp" |
36 | | -#include "rclcpp/executor.hpp" |
37 | | -#include "rclcpp/parameter_value.hpp" |
38 | | -#include "rclcpp/time.hpp" |
39 | | -#include "rclcpp/utilities.hpp" |
40 | | -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" |
| 30 | +#include <hardware_interface/loaned_command_interface.hpp> |
| 31 | +#include <hardware_interface/loaned_state_interface.hpp> |
| 32 | +#include <hardware_interface/types/hardware_interface_return_values.hpp> |
| 33 | +#include <rclcpp/executor.hpp> |
| 34 | +#include <rclcpp/parameter_value.hpp> |
| 35 | +#include <rclcpp/time.hpp> |
| 36 | +#include <rclcpp/utilities.hpp> |
| 37 | +#include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp> |
41 | 38 |
|
42 | 39 | // TODO(anyone): replace the state and command message types |
43 | 40 | using JointStateMsg = io_gripper_controller::IOGripperController::JointStateMsg; |
@@ -134,9 +131,11 @@ class IOGripperControllerFixture : public ::testing::Test |
134 | 131 | protected: |
135 | 132 | void SetUpController(const std::string controller_name = "test_io_gripper_controller") |
136 | 133 | { |
| 134 | + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "initializing controller"); |
137 | 135 | ASSERT_EQ( |
138 | 136 | controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), |
139 | 137 | controller_interface::return_type::OK); |
| 138 | + RCLCPP_INFO(rclcpp::get_logger("IOGripperControllerTest"), "initialized successfully"); |
140 | 139 |
|
141 | 140 | // setting the command state interfaces manually |
142 | 141 | std::vector<hardware_interface::LoanedCommandInterface> command_itfs; |
@@ -237,7 +236,7 @@ class IOGripperControllerFixture : public ::testing::Test |
237 | 236 | {"open.set_after_command.low", open_set_after_command_low}); |
238 | 237 |
|
239 | 238 | controller_->get_node()->set_parameter({"possible_closed_states", possible_closed_states}); |
240 | | - controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); |
| 239 | + // controller_->get_node()->set_parameter({"close.joint_states", close_joint_states}); |
241 | 240 | controller_->get_node()->set_parameter( |
242 | 241 | {"close.set_before_command.high", close_set_before_command_high}); |
243 | 242 | controller_->get_node()->set_parameter( |
@@ -345,37 +344,37 @@ class IOGripperControllerFixture : public ::testing::Test |
345 | 344 | std::vector<std::string> open_close_joints = {"gripper_clamp_jaw"}; |
346 | 345 |
|
347 | 346 | std::vector<double> open_joint_states = {0.0}; |
348 | | - std::vector<std::string> open_set_before_command_high = {"EL2008/Bremse_WQG7"}; |
349 | | - std::vector<std::string> open_set_before_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; |
350 | | - std::vector<std::string> open_set_after_command_high = {"EL2008/Bremse_WQG7"}; |
351 | | - std::vector<std::string> open_set_after_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; |
352 | | - std::vector<std::string> open_command_high = {"EL2008/Greiferteil_Oeffnen_WQG1"}; |
353 | | - std::vector<std::string> open_command_low = {"EL2008/Greiferteil_Schliessen_WQG2"}; |
354 | | - std::vector<std::string> open_state_high = {"EL1008/Greifer_Geoeffnet_BG01"}; |
355 | | - std::vector<std::string> open_state_low = {"EL1008/Greifer_Geschloschen_BG02"}; |
| 347 | + std::vector<std::string> open_set_before_command_high = {"Release_Break_valve"}; |
| 348 | + std::vector<std::string> open_set_before_command_low = {"Release_Something"}; |
| 349 | + std::vector<std::string> open_set_after_command_high = {"Release_Break_valve"}; |
| 350 | + std::vector<std::string> open_set_after_command_low = {"Release_Something"}; |
| 351 | + std::vector<std::string> open_command_high = {"Open_valve"}; |
| 352 | + std::vector<std::string> open_command_low = {"Close_valve"}; |
| 353 | + std::vector<std::string> open_state_high = {"Opened_signal"}; |
| 354 | + std::vector<std::string> open_state_low = {"Closed_signal"}; |
356 | 355 |
|
357 | 356 | std::vector<std::string> possible_closed_states = {"empty_close", "full_close"}; |
358 | 357 | std::vector<double> close_joint_states = {0.08}; |
359 | | - std::vector<std::string> close_set_before_command_high = {"EL2008/Bremse_WQG7"}; |
360 | | - std::vector<std::string> close_set_before_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; |
361 | | - std::vector<std::string> close_set_after_command_high = {"EL2008/Bremse_WQG7"}; |
362 | | - std::vector<std::string> close_set_after_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; |
363 | | - std::vector<std::string> close_command_high = {"EL2008/Greiferteil_Schliessen_WQG2"}; |
364 | | - std::vector<std::string> close_command_low = {"EL2008/Greiferteil_Oeffnen_WQG1"}; |
365 | | - std::vector<std::string> close_state_high = {"EL1008/Greifer_Geschloschen_BG02"}; |
366 | | - std::vector<std::string> close_state_low = {"EL1008/Bauteilabfrage_BG06"}; |
367 | | - |
368 | | - std::vector<std::string> configurations_list = {"stichmass_125"}; |
| 358 | + std::vector<std::string> close_set_before_command_high = {"Release_Break_valve"}; |
| 359 | + std::vector<std::string> close_set_before_command_low = {"Open_valve"}; |
| 360 | + std::vector<std::string> close_set_after_command_high = {"Release_Break_valve"}; |
| 361 | + std::vector<std::string> close_set_after_command_low = {"Open_valve"}; |
| 362 | + std::vector<std::string> close_command_high = {"Release_Something"}; |
| 363 | + std::vector<std::string> close_command_low = {"Open_valve"}; |
| 364 | + std::vector<std::string> close_state_high = {"Closed_signal"}; |
| 365 | + std::vector<std::string> close_state_low = {"Part_Grasped_signal"}; |
| 366 | + |
| 367 | + std::vector<std::string> configurations_list = {"narrow_objects"}; |
369 | 368 | std::vector<std::string> configuration_joints = {"gripper_gripper_distance_joint"}; |
370 | 369 |
|
371 | 370 | std::vector<double> stichmass_joint_states = {0.125}; |
372 | | - std::vector<std::string> stichmass_command_high = {"EL2008/Stichmass_125_WQG5"}; |
373 | | - std::vector<std::string> stichmass_command_low = {"EL2008/Stichmass_250_WQG6"}; |
374 | | - std::vector<std::string> stichmass_state_high = {"EL1008/Stichmass_125mm_BG03"}; |
375 | | - std::vector<std::string> stichmass_state_low = {"EL1008/Stichmass_250mm_BG04"}; |
| 371 | + std::vector<std::string> stichmass_command_high = {"Narrow_Configuration_Cmd"}; |
| 372 | + std::vector<std::string> stichmass_command_low = {"Wide_Configuration_Cmd"}; |
| 373 | + std::vector<std::string> stichmass_state_high = {"Narrow_Configuraiton_Signal"}; |
| 374 | + std::vector<std::string> stichmass_state_low = {"Narrow_Configuraiton_Signal"}; |
376 | 375 |
|
377 | | - std::vector<std::string> gripper_specific_sensors = {"hohenabfrage"}; |
378 | | - std::string gripper_interfaces_input = {"EL1008/Hohenabfrage_BG5"}; |
| 376 | + std::vector<std::string> gripper_specific_sensors = {"part_sensor_top"}; |
| 377 | + std::string gripper_interfaces_input = {"Part_Sensor_Top_signal"}; |
379 | 378 |
|
380 | 379 | std::vector<std::string> joint_names_ = {"gripper_joint", "finger_joint"}; |
381 | 380 | std::vector<std::string> state_joint_names_ = {"gripper_joint"}; |
|
0 commit comments