integrationtest_operation_mode_setup.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2020 Pilz GmbH & Co. KG
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8 
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13 
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #include <utility>
19 
20 #include <gtest/gtest.h>
21 #include <gmock/gmock.h>
22 
23 #include <ros/ros.h>
24 
25 #include <std_srvs/SetBool.h>
26 
27 #include <pilz_msgs/GetSpeedOverride.h>
28 
29 #include <pilz_testutils/async_test.h>
30 
31 #include <prbt_hardware_support/OperationModes.h>
33 
34 namespace prbt_hardware_support
35 {
36 static const std::string OPERATON_MODE_SETUP_EXECUTOR_NODE_NAME{ "/operation_mode_setup_executor_node" };
37 static const std::string MONITOR_CARTESIAN_SPEED_SERVICE{ "manipulator_joint_trajectory_controller/"
38  "monitor_cartesian_speed" };
39 static const std::string OPERATION_MODE_TOPIC{ "operation_mode" };
40 static const std::string GET_SPEED_OVERRIDE_SERVICE{ "get_speed_override" };
41 static const std::string MONITOR_CARTESIAN_SPEED_CALLBACK_EVENT{ "monitor_cartesian_speed_callback_event" };
42 
43 static constexpr uint32_t DEFAULT_QUEUE_SIZE{ 10 };
44 
45 static constexpr double EXPECTED_SPEED_OVERRIDE_AUTO{ 1.0 };
46 static constexpr double EXPECTED_SPEED_OVERRIDE_T1{ 0.1 };
47 
48 struct TestData
49 {
50  constexpr TestData(const int8_t op_mode_, const bool expected_request_data_, const double expected_speed_override_)
51  : op_mode(op_mode_)
52  , expected_request_data(expected_request_data_)
53  , expected_speed_override(expected_speed_override_)
54  {
55  }
56 
57  const int8_t op_mode;
58 
61 };
62 
63 static constexpr TestData OP_MODE_T1_TEST_DATA{ OperationModes::T1, true, EXPECTED_SPEED_OVERRIDE_T1 };
64 static constexpr TestData OP_MODE_AUTO_TEST_DATA{ OperationModes::AUTO, false, EXPECTED_SPEED_OVERRIDE_AUTO };
65 
68 {
69 public:
70  void advertiseService();
71  MOCK_METHOD2(monitor_cartesian_speed_callback_, bool(std_srvs::SetBoolRequest&, std_srvs::SetBoolResponse&));
72 
73 private:
76 };
77 
79 {
80  monitor_cartesian_speed_srv_ =
81  nh_.advertiseService(MONITOR_CARTESIAN_SPEED_SERVICE, &ControllerMock::monitor_cartesian_speed_callback_, this);
82 }
83 
89 class OperationModeSetupTest : public testing::Test,
90  public testing::AsyncTest,
91  public testing::WithParamInterface<TestData>
92 {
93 public:
94  void SetUp() override;
95 
96 protected:
97  using OperationModes = prbt_hardware_support::OperationModes;
98  using GetSpeedOverride = pilz_msgs::GetSpeedOverride;
99 
100 protected:
101  testing::AssertionResult isSpeedOverrideEqualTo(const double& expected_speed_override);
102 
103 protected:
108 };
109 
111 {
112  controller_mock_.advertiseService();
113  operation_mode_pub_ = nh_.advertise<OperationModes>(OPERATION_MODE_TOPIC, DEFAULT_QUEUE_SIZE, true);
114 
117  ASSERT_TRUE(waitForSubscriber(operation_mode_pub_));
118 }
119 
121 {
122  if (!get_speed_override_client_.isValid())
123  {
124  get_speed_override_client_ = nh_.serviceClient<GetSpeedOverride>(GET_SPEED_OVERRIDE_SERVICE);
125  }
126 
127  GetSpeedOverride get_speed_override;
128  if (!get_speed_override_client_.call(get_speed_override))
129  {
130  return testing::AssertionFailure() << "Service call failed: " << get_speed_override_client_.getService();
131  }
132  if (get_speed_override.response.speed_override != expected_speed_override)
133  {
134  return testing::AssertionFailure() << "Speed overrides do not match. Expected: " << expected_speed_override
135  << ", actual: " << get_speed_override.response.speed_override;
136  }
137  return testing::AssertionSuccess();
138 }
139 
140 using testing::_;
141 using testing::DoAll;
142 using testing::Field;
143 using testing::Return;
144 using testing::SetArgReferee;
145 
149 TEST_P(OperationModeSetupTest, testSpeedMonitoringActivation)
150 {
151  const auto test_data{ GetParam() };
152 
153  std_srvs::SetBoolResponse response;
154  response.success = true;
155  EXPECT_CALL(controller_mock_, monitor_cartesian_speed_callback_(
156  Field(&std_srvs::SetBoolRequest::data, test_data.expected_request_data), _))
157  .WillOnce(DoAll(SetArgReferee<1>(response), ACTION_OPEN_BARRIER(MONITOR_CARTESIAN_SPEED_CALLBACK_EVENT)));
158 
159  OperationModes msg;
160  msg.value = test_data.op_mode;
161  msg.time_stamp = ros::Time::now();
162  operation_mode_pub_.publish(msg);
163 
165 
166  EXPECT_TRUE(isSpeedOverrideEqualTo(test_data.expected_speed_override));
167 }
168 
169 INSTANTIATE_TEST_CASE_P(TestActivationOfSpeedMonitoring, OperationModeSetupTest,
170  testing::Values(OP_MODE_AUTO_TEST_DATA, OP_MODE_T1_TEST_DATA), );
171 
172 } // namespace prbt_hardware_support
173 
174 int main(int argc, char* argv[])
175 {
176  ros::init(argc, argv, "integrationtest_operation_mode_setup");
177  ros::NodeHandle nh;
178 
180  spinner.start();
181 
182  testing::InitGoogleTest(&argc, argv);
183  return RUN_ALL_TESTS();
184 }
msg
static constexpr TestData OP_MODE_T1_TEST_DATA
static const std::string MONITOR_CARTESIAN_SPEED_CALLBACK_EVENT
static const std::string OPERATON_MODE_SETUP_EXECUTOR_NODE_NAME
int main(int argc, char *argv[])
ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function< bool(const typename ActionSpec::_action_goal_type::_goal_type &, typename ActionSpec::_action_result_type::_result_type &result)> service_cb)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
testing::AssertionResult isSpeedOverrideEqualTo(const double &expected_speed_override)
INSTANTIATE_TEST_CASE_P(TestActivationOfSpeedMonitoring, OperationModeSetupTest, testing::Values(OP_MODE_AUTO_TEST_DATA, OP_MODE_T1_TEST_DATA),)
Provides the monitor_cartesian_speed service.
void spinner()
static const std::string MONITOR_CARTESIAN_SPEED_SERVICE
static const std::string GET_SPEED_OVERRIDE_SERVICE
inline ::testing::AssertionResult waitForNode(const std::string node_name, const double loop_frequency=10.0, const ros::Duration timeout=ros::Duration(10.0))
Blocks until a node defined by node_name comes up.
static constexpr TestData OP_MODE_AUTO_TEST_DATA
static constexpr int DEFAULT_QUEUE_SIZE
static constexpr double EXPECTED_SPEED_OVERRIDE_AUTO
static Time now()
TEST_P(OperationModeSetupTest, testSpeedMonitoringActivation)
Tests that the Cartesian speed monitoring is activated or de-activated depending on the operation mod...
constexpr TestData(const int8_t op_mode_, const bool expected_request_data_, const double expected_speed_override_)
inline ::testing::AssertionResult waitForSubscriber(const ros::Publisher &publisher, const unsigned int subscriber_count=1, const double loop_frequency=10.0, const ros::Duration timeout=ros::Duration(10.0))
Blocks until the specified number of subscribers is registered on a topic.
static constexpr double EXPECTED_SPEED_OVERRIDE_T1
const std::string response
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)


prbt_hardware_support
Author(s):
autogenerated on Sun Oct 25 2020 03:13:00