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 {
34 {
35  std_srvs::Trigger trigger;
36  ROS_DEBUG_STREAM("Calling service: " << srv_client.getService() << ")");
37  bool call_success = srv_client.call(trigger);
38  if (!call_success)
39  {
40  ROS_ERROR_STREAM("No success calling service: " << srv_client.getService());
41  }
42 
43  if (!trigger.response.success)
44  {
45  ROS_ERROR_STREAM("Service: " << srv_client.getService() << " failed with error message:\n"
46  << trigger.response.message);
47  }
48  return call_success && trigger.response.success;
49 }
50 
51 } // namespace prbt_hardware_support
52 
53 using namespace prbt_hardware_support;
54 
55 int main(int argc, char** argv)
56 {
57  ros::init(argc, argv, "stop1_executor");
58  ros::NodeHandle nh;
59 
61  ros::ServiceClient hold_srv = nh.serviceClient<std_srvs::Trigger>(HOLD_SERVICE);
62 
64  ros::ServiceClient unhold_srv = nh.serviceClient<std_srvs::Trigger>(UNHOLD_SERVICE);
65 
67  ros::ServiceClient recover_srv = nh.serviceClient<std_srvs::Trigger>(RECOVER_SERVICE);
68 
70  ros::ServiceClient halt_srv = nh.serviceClient<std_srvs::Trigger>(HALT_SERVICE);
71 
72  TServiceCallFunc hold_func = std::bind(callService, hold_srv);
73  TServiceCallFunc unhold_func = std::bind(callService, unhold_srv);
74  TServiceCallFunc recover_func = std::bind(callService, recover_srv);
75  TServiceCallFunc halt_func = std::bind(callService, halt_srv);
76 
77  Stop1Executor stop1_executor(hold_func, unhold_func, recover_func, halt_func);
78  ros::ServiceServer run_permitted_serv =
79  nh.advertiseService("run_permitted", &Stop1Executor::updateRunPermittedCallback, &stop1_executor);
80 
81  ros::spin();
82 
83  return EXIT_FAILURE;
84 }
85 // LCOV_EXCL_STOP
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool updateRunPermittedCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
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)
const std::string HALT_SERVICE


prbt_hardware_support
Author(s):
autogenerated on Thu Sep 10 2020 03:14:36