26 #include <prbt_hardware_support/SetSpeedLimit.h> 28 #include <prbt_hardware_support/GetOperationMode.h> 48 int main(
int argc,
char **argv)
50 ros::init(argc, argv,
"operation_mode_setup_executor");
53 using std::placeholders::_1;
56 SetSpeedLimitFunc set_speed_limit_func = std::bind(setSpeedLimitSrv<ros::ServiceClient>,
59 double speed_limit_t1 {0};
60 double speed_limit_auto {0};
67 catch (
const std::runtime_error &ex)
74 set_speed_limit_func);
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
static const std::string PARAM_SPEED_LIMIT_AUTO_STR
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static const std::string SET_SPEED_LIMIT_SERVICE
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool getSpeedOverride(GetSpeedOverride::Request &, GetSpeedOverride::Response &response)
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
ROSCPP_DECL void spin(Spinner &spinner)
Sets the allowed speed limit for each frame based on the current operation mode.
static const std::string PARAM_SPEED_LIMIT_T1_STR
static void waitForService(const std::string service_name, const double retry_timeout=DEFAULT_RETRY_TIMEOUT, const double msg_output_period=DEFAULT_MSG_OUTPUT_PERIOD)
static const std::string GET_SPEED_OVERRIDE_SERVICE
std::function< bool(const double &)> SetSpeedLimitFunc
static const std::string OPERATION_MODE_TOPIC
static constexpr int DEFAULT_QUEUE_SIZE
#define ROS_ERROR_STREAM(args)
void updateOperationMode(const OperationModes &operation_mode)
Function to be called whenever a new operation mode is set.