22 #include <std_srvs/SetBool.h> 24 #include <pilz_msgs/GetOperationMode.h> 35 "monitor_cartesian_speed" };
48 int main(
int argc,
char** argv)
50 ros::init(argc, argv,
"operation_mode_setup_executor");
53 using std::placeholders::_1;
57 std::bind(monitorCartesianSpeedSrv<ros::ServiceClient>, monitor_cartesian_speed_srv, _1);
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
bool getSpeedOverride(pilz_msgs::GetSpeedOverride::Request &, pilz_msgs::GetSpeedOverride::Response &response)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void updateOperationMode(const pilz_msgs::OperationModes &operation_mode)
Function to be called whenever a new operation mode is set.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
int main(int argc, char **argv)
Read necessary parameters, start and initialize the prbt_hardware_support::OperationModeSetupExecutor...
static constexpr uint32_t DEFAULT_QUEUE_SIZE
static const std::string MONITOR_CARTESIAN_SPEED_SERVICE
static const std::string MONITOR_CARTESIAN_SPEED_SERVICE
Activates speed monitoring and sets the speed override based on the current operation mode...
static const std::string GET_SPEED_OVERRIDE_SERVICE
static const std::string OPERATION_MODE_TOPIC
static void waitForService(const std::string service_name, const double retry_timeout=DEFAULT_RETRY_TIMEOUT, const double msg_output_period=DEFAULT_MSG_OUTPUT_PERIOD)
std::function< bool(const bool)> MonitorCartesianSpeedFunc
static const std::string GET_SPEED_OVERRIDE_SERVICE
static const std::string OPERATION_MODE_TOPIC
static constexpr int DEFAULT_QUEUE_SIZE