stop1_executor_node.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019 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 <ros/ros.h>
19 
21 
24 
25 const std::string HOLD_SERVICE{"manipulator_joint_trajectory_controller/hold"};
26 const std::string UNHOLD_SERVICE{"manipulator_joint_trajectory_controller/unhold"};
27 const std::string RECOVER_SERVICE{"driver/recover"};
28 const std::string HALT_SERVICE{"driver/halt"};
29 
30 // LCOV_EXCL_START
31 namespace prbt_hardware_support
32 {
33 
35 {
36  std_srvs::Trigger trigger;
37  ROS_DEBUG_STREAM("Calling service: " << srv_client.getService() << ")");
38  bool call_success = srv_client.call(trigger);
39  if (!call_success)
40  {
41  ROS_ERROR_STREAM("No success calling service: " << srv_client.getService());
42  }
43 
44  if (!trigger.response.success)
45  {
46  ROS_ERROR_STREAM("Service: " << srv_client.getService()
47  << " failed with error message:\n"
48  << trigger.response.message);
49  }
50  return call_success && trigger.response.success;
51 }
52 
53 } // namespace prbt_hardware_support
54 
55 using namespace prbt_hardware_support;
56 
57 int main(int argc, char **argv)
58 {
59  ros::init(argc, argv, "stop1_executor");
60  ros::NodeHandle nh;
61 
63  ros::ServiceClient hold_srv = nh.serviceClient<std_srvs::Trigger>(HOLD_SERVICE);
64 
66  ros::ServiceClient unhold_srv = nh.serviceClient<std_srvs::Trigger>(UNHOLD_SERVICE);
67 
69  ros::ServiceClient recover_srv = nh.serviceClient<std_srvs::Trigger>(RECOVER_SERVICE);
70 
72  ros::ServiceClient halt_srv = nh.serviceClient<std_srvs::Trigger>(HALT_SERVICE);
73 
74  TServiceCallFunc hold_func = std::bind(callService, hold_srv);
75  TServiceCallFunc unhold_func = std::bind(callService, unhold_srv);
76  TServiceCallFunc recover_func = std::bind(callService, recover_srv);
77  TServiceCallFunc halt_func = std::bind(callService, halt_srv);
78 
79  Stop1Executor stop1_executor(hold_func, unhold_func, recover_func, halt_func);
80  ros::ServiceServer sto_serv = nh.advertiseService("safe_torque_off",
82  &stop1_executor);
83 
84  ros::spin();
85 
86  return EXIT_FAILURE;
87 }
88 // LCOV_EXCL_STOP
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const std::string RECOVER_SERVICE
bool callService(ros::ServiceClient &srv_client)
const std::string UNHOLD_SERVICE
const std::string HOLD_SERVICE
static void waitForService(const std::string service_name, const double retry_timeout=DEFAULT_RETRY_TIMEOUT, const double msg_output_period=DEFAULT_MSG_OUTPUT_PERIOD)
ROSCPP_DECL void spin()
#define ROS_DEBUG_STREAM(args)
int main(int argc, char **argv)
std::string getService()
Performs service calls for Stop1 and the respective reversal, that is enabling the manipulator...
std::function< bool()> TServiceCallFunc
#define ROS_ERROR_STREAM(args)
bool updateStoCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
const std::string HALT_SERVICE


prbt_hardware_support
Author(s):
autogenerated on Tue Feb 25 2020 03:19:31