25 const std::string
HOLD_SERVICE{
"manipulator_joint_trajectory_controller/hold"};
26 const std::string
UNHOLD_SERVICE{
"manipulator_joint_trajectory_controller/unhold"};
36 std_srvs::Trigger trigger;
38 bool call_success = srv_client.
call(trigger);
44 if (!trigger.response.success)
47 <<
" failed with error message:\n" 48 << trigger.response.message);
50 return call_success && trigger.response.success;
57 int main(
int argc,
char **argv)
79 Stop1Executor stop1_executor(hold_func, unhold_func, recover_func, halt_func);
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)
ROSCPP_DECL void spin(Spinner &spinner)
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)
#define ROS_DEBUG_STREAM(args)
int main(int argc, char **argv)
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