18 #include <gtest/gtest.h> 19 #include <gmock/gmock.h> 26 #include <condition_variable> 31 #include <std_srvs/SetBool.h> 32 #include <std_srvs/Trigger.h> 34 #include <modbus/modbus.h> 38 #include <prbt_hardware_support/ModbusMsgInStamped.h> 46 #include <pilz_testutils/async_test.h> 51 using ::testing::AtLeast;
52 using ::testing::DoAll;
53 using ::testing::InSequence;
54 using ::testing::Invoke;
55 using ::testing::InvokeWithoutArgs;
56 using ::testing::Return;
57 using ::testing::SetArgReferee;
59 using namespace modbus_api::v3;
76 void SetUp()
override;
77 void TearDown()
override;
80 bool serviceCallStub(
const std::string& barrier_name,
const std_srvs::Trigger::Response& res_exp,
81 std_srvs::Trigger::Request& , std_srvs::Trigger::Response& res)
83 this->triggerClearEvent(barrier_name);
150 ASSERT_TRUE(nh_priv_.getParam(
"modbus_server_ip", ip));
151 ASSERT_TRUE(nh_priv_.getParam(
"modbus_server_port", port));
162 std::thread modbus_server_thread(&initalizeAndRun<prbt_hardware_support::PilzModbusServerMock>,
163 std::ref(modbus_server), ip.c_str(),
static_cast<unsigned int>(port));
169 using std::placeholders::_1;
170 using std::placeholders::_2;
172 std_srvs::Trigger::Response res_exp;
173 res_exp.success =
true;
180 EXPECT_CALL(manipulator, holdCb(_, _)).Times(0);
181 EXPECT_CALL(manipulator, haltCb(_, _)).Times(0);
182 EXPECT_CALL(manipulator, recoverCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(
true)));
183 EXPECT_CALL(manipulator, unholdCb(_, _))
202 BARRIER(
"unhold_callback");
208 EXPECT_CALL(manipulator, unholdCb(_, _)).Times(0);
209 EXPECT_CALL(manipulator, recoverCb(_, _)).Times(0);
210 EXPECT_CALL(manipulator, holdCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(
true)));
211 EXPECT_CALL(manipulator, haltCb(_, _))
221 BARRIER(
"halt_callback");
227 EXPECT_CALL(manipulator, holdCb(_, _)).Times(0);
228 EXPECT_CALL(manipulator, haltCb(_, _)).Times(0);
229 EXPECT_CALL(manipulator, recoverCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(
true)));
230 EXPECT_CALL(manipulator, unholdCb(_, _))
240 BARRIER(
"unhold_callback");
245 EXPECT_CALL(manipulator, holdCb(_, _)).Times(1).WillOnce(DoAll(SetArgReferee<1>(res_exp), Return(
true)));
246 EXPECT_CALL(manipulator, unholdCb(_, _)).Times(0);
247 EXPECT_CALL(manipulator, recoverCb(_, _)).Times(0);
248 EXPECT_CALL(manipulator, haltCb(_, _))
254 modbus_server_thread.join();
259 BARRIER(
"halt_callback");
264 int main(
int argc,
char* argv[])
266 ros::init(argc, argv,
"integrationtest_stop1");
272 testing::InitGoogleTest(&argc, argv);
273 return RUN_ALL_TESTS();
int main(int argc, char *argv[])
Offers a modbus server and read/write functionality via subscription/publication. ...
Mocks the ROS Api of the manipulator relevant for stopping and holding.
static constexpr uint16_t MODBUS_RUN_PERMITTED_FALSE
static const std::string HOLD_SERVICE_NAME
TEST_F(BrakeTestRequiredIntegrationTest, testBrakeTestAnnouncement)
void setHoldingRegister(std::initializer_list< std::pair< unsigned int, uint16_t > > reg_list)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
unsigned short getMaxRegisterDefinition() const
ManipulatorMock manipulator
static constexpr uint16_t MODBUS_API_VERSION_REQUIRED
Stop1IntegrationTest checks if the chain ModbusServerMock -> ModbusReadClient -> RunPermittedModbusAd...
static const std::string UNHOLD_SERVICE_NAME
static const std::string RECOVER_SERVICE_NAME
static const std::string RUN_PERMITTED
static const std::string HALT_SERVICE_NAME
void terminate()
Terminate the Server. Reading or connecting to it will fail.
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.
bool serviceCallStub(const std::string &barrier_name, const std_srvs::Trigger::Response &res_exp, std_srvs::Trigger::Request &, std_srvs::Trigger::Response &res)
static constexpr uint16_t MODBUS_RUN_PERMITTED_TRUE
Specifies the meaning of the holding registers.
static const std::string VERSION