in servo_pkg/src/servo_node.cpp [30:127]
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
const char* SERVO_GPIO_SERVICE_NAME = "servo_gpio";
const char* SET_CAL_SERVICE_NAME = "set_calibration";
const char* GET_CAL_SERVICE_NAME = "get_calibration";
const char* SET_LED_STATE_SERVICE_NAME = "set_led_state";
const char* GET_LED_STATE_SERVICE_NAME = "get_led_state";
const char* SERVO_TOPIC = "/ctrl_pkg/servo_msg";
const char* RAW_PWM_TOPIC = "/ctrl_pkg/raw_pwm";
std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("servo_node");
auto servoMgr = std::make_unique<PWM::ServoMgr>(node->get_logger());
auto ledMgr = std::make_unique<PWM::LedMgr>(node->get_logger());
auto qos = rclcpp::QoS(rclcpp::KeepLast(1));
qos.best_effort();
auto servoMsgStrategy = std::make_shared<rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy<deepracer_interfaces_pkg::msg::ServoCtrlMsg, 1>>();
auto rawPWMMsgStrategy = std::make_shared<rclcpp::strategies::message_pool_memory_strategy::MessagePoolMemoryStrategy<deepracer_interfaces_pkg::msg::ServoCtrlMsg, 1>>();
auto sub_ = node->create_subscription<deepracer_interfaces_pkg::msg::ServoCtrlMsg>(SERVO_TOPIC,
qos,
std::bind(&PWM::ServoMgr::servoSubscriber,
servoMgr.get(),
std::placeholders::_1),
rclcpp::SubscriptionOptions(),
servoMsgStrategy);
auto rawPWMsub_ = node->create_subscription<deepracer_interfaces_pkg::msg::ServoCtrlMsg>(RAW_PWM_TOPIC,
qos,
std::bind(&PWM::ServoMgr::rawPWMSubscriber,
servoMgr.get(),
std::placeholders::_1),
rclcpp::SubscriptionOptions(),
rawPWMMsgStrategy);
auto servoCalServiceCbGrp_ = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant);
rclcpp::Service<deepracer_interfaces_pkg::srv::SetCalibrationSrv>::SharedPtr servoCalService =
node->create_service<deepracer_interfaces_pkg::srv::SetCalibrationSrv>(SET_CAL_SERVICE_NAME,
std::bind(&PWM::ServoMgr::setCalHdl,
servoMgr.get(),
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3),
::rmw_qos_profile_default,
servoCalServiceCbGrp_);
auto setGPIOServiceCbGrp_ = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
rclcpp::Service<deepracer_interfaces_pkg::srv::ServoGPIOSrv>::SharedPtr setGPIOService =
node->create_service<deepracer_interfaces_pkg::srv::ServoGPIOSrv>(SERVO_GPIO_SERVICE_NAME,
std::bind(&PWM::ServoMgr::setGPIOHdl,
servoMgr.get(),
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3),
::rmw_qos_profile_default,
setGPIOServiceCbGrp_);
auto getCalServiceCbGrp_ = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant);
rclcpp::Service<deepracer_interfaces_pkg::srv::GetCalibrationSrv>::SharedPtr getCalService =
node->create_service<deepracer_interfaces_pkg::srv::GetCalibrationSrv>(GET_CAL_SERVICE_NAME,
std::bind(&PWM::ServoMgr::getCalHdl,
servoMgr.get(),
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3),
::rmw_qos_profile_default,
getCalServiceCbGrp_);
auto setLedCtrlCbGrp_ = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant);
rclcpp::Service<deepracer_interfaces_pkg::srv::SetLedCtrlSrv>::SharedPtr setLedCtrlService =
node->create_service<deepracer_interfaces_pkg::srv::SetLedCtrlSrv>(SET_LED_STATE_SERVICE_NAME,
std::bind(&PWM::LedMgr::setLedCtrlHdl,
ledMgr.get(),
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3),
::rmw_qos_profile_default,
setLedCtrlCbGrp_);
auto getLedCtrlCbGrp_ = node->create_callback_group(rclcpp::callback_group::CallbackGroupType::Reentrant);
rclcpp::Service<deepracer_interfaces_pkg::srv::GetLedCtrlSrv>::SharedPtr getLedCtrlService =
node->create_service<deepracer_interfaces_pkg::srv::GetLedCtrlSrv>(GET_LED_STATE_SERVICE_NAME,
std::bind(&PWM::LedMgr::getLedCtrlHdl,
ledMgr.get(),
std::placeholders::_1,
std::placeholders::_2,
std::placeholders::_3),
::rmw_qos_profile_default,
getLedCtrlCbGrp_);
RCLCPP_INFO(node->get_logger(), "servo_node started");
rclcpp::executors::MultiThreadedExecutor exec;
exec.add_node(node);
exec.spin();
rclcpp::shutdown();
return 0;
}