int main(int argc, const char *argv[]) { int res; const SBVersionStatus * compiled = sbGetCompiledVersion(); unsigned int objSizeStatus = sbValidateObjectSize(compiled); printf("Object size status: %04x\n",objSizeStatus); assert(objSizeStatus == 0); SBApiSimpleContext simple; SBController api(&simple); if (argc < 4) { printf("Usage:\n\t%s <port> <load|save> <filename>\n",argv[0]); return -1; } CRITICAL(res = api.initialise(argv[1])); if (strcasecmp(argv[2],"load")==0) { api.load(argv[3]); } if (strcasecmp(argv[2],"save")==0) { api.save(argv[3]); } DEBUG(res = api.terminate()); return 0; }
int main(int argc, char **argv) { int res; const SBVersionStatus * compiled = sbGetCompiledVersion(); unsigned int objSizeStatus = sbValidateObjectSize(compiled); ros::init(argc, argv, "coax_repeat"); ROS_INFO("Object size status: %04x",objSizeStatus); assert(objSizeStatus == 0); SBApiSimpleContext simple; SBController api(&simple); CRITICAL(res = api.initialise((argc<2)?("localhost"):(argv[1]))); ros::NodeHandle n("/coax_repeat"); #ifdef REPEAT_STRINGS ros::Subscriber repeat_sub = n.subscribe("torepeat",1,&SBController::repeat_ros_callback,&api); ros::Publisher repeat_pub = n.advertise<std_msgs::String>("repeated",1); #else ros::Subscriber repeat_sub = n.subscribe("torepeat",1,&SBController::repeat_ros_callback,&api); ros::Publisher repeat_pub = n.advertise<coax_msgs::CoaxRepeat>("repeated",1); #endif api.setupRepeatIO(&repeat_pub,&repeat_sub); ROS_INFO("Coax Server ready"); ros::spin(); DEBUG(res = api.terminate()); return 0; }
int main(int argc, const char *argv[]) { const SBVersionStatus * compiled = sbGetCompiledVersion(); unsigned int objSizeStatus = sbValidateObjectSize(compiled); printf("Object size status: %04x\n",objSizeStatus); assert(objSizeStatus == 0); char flogname[128]; sprintf(flogname,"log-%08ld.txt",(unsigned long)time(NULL)); SBApiSimpleContext simple; SBController api(&simple); int res; CRITICAL(res = api.initialise((argc<2)?("localhost"):(argv[1]))); DEBUG(res = api.joyctrl()); DEBUG(res = api.terminate()); printf("Saving log\n"); api.savelog(flogname); printf("Saved log\n"); return 0; }
int main(int argc, char **argv) { int res; const SBVersionStatus * compiled = sbGetCompiledVersion(); unsigned int objSizeStatus = sbValidateObjectSize(compiled); ros::init(argc, argv, "coax_server"); ROS_INFO("Object size status: %04x",objSizeStatus); assert(objSizeStatus == 0); SBApiSimpleContext simple; SBController api(&simple); CRITICAL(res = api.initialise((argc<2)?("localhost"):(argv[1]))); ros::NodeHandle n("/coax_server"); std::vector<ros::ServiceServer> services; services.push_back(n.advertiseService("get_version", &SBController::get_version, &api)); services.push_back(n.advertiseService("configure_comm", &SBController::configure_comm, &api)); services.push_back(n.advertiseService("configure_control", &SBController::configure_control, &api)); services.push_back(n.advertiseService("configure_oamode", &SBController::configure_oamode, &api)); services.push_back(n.advertiseService("get_control_parameters", &SBController::get_control_parameters, &api)); services.push_back(n.advertiseService("set_control_parameters", &SBController::set_control_parameters, &api)); services.push_back(n.advertiseService("get_trim_mode", &SBController::get_trim_mode, &api)); services.push_back(n.advertiseService("set_trim_mode", &SBController::set_trim_mode, &api)); services.push_back(n.advertiseService("get_sensor_list", &SBController::get_sensor_list, &api)); services.push_back(n.advertiseService("reach_nav_state", &SBController::reach_nav_state, &api)); services.push_back(n.advertiseService("request_state", &SBController::request_state, &api)); services.push_back(n.advertiseService("send_string", &SBController::send_string, &api)); services.push_back(n.advertiseService("set_verbose", &SBController::set_verbose, &api)); services.push_back(n.advertiseService("set_ack_mode", &SBController::set_ack_mode, &api)); services.push_back(n.advertiseService("set_timeout", &SBController::set_timeout, &api)); services.push_back(n.advertiseService("set_control", &SBController::set_control, &api)); services.push_back(n.advertiseService("set_raw_control", &SBController::set_raw_control, &api)); services.push_back(n.advertiseService("reset", &SBController::reset, &api)); services.push_back(n.advertiseService("set_light", &SBController::set_light, &api)); // Publishing the state ros::Publisher coax_pub = n.advertise<coax_msgs::CoaxState>("state",50); api.registerPublisher(&coax_pub); // Subscribing to control message (without acknowledgement) ros::TransportHints hints; ros::Subscriber control_sub = n.subscribe("control",10,&SBController::control_callback,&api,hints.udp()); ros::Subscriber rawcontrol_sub = n.subscribe("rawcontrol",10,&SBController::raw_control_callback,&api,hints.udp()); ROS_INFO("Coax Server ready"); ros::spin(); DEBUG(res = api.terminate()); return 0; }
int main(int argc, const char *argv[]) { int res; const SBVersionStatus * compiled = sbGetCompiledVersion(); unsigned int objSizeStatus = sbValidateObjectSize(compiled); printf("Object size status: %04x\n",objSizeStatus); assert(objSizeStatus == 0); SBApiSimpleContext simple; SBController api(&simple); if (argc < 3) { printf("Usage:\n\t%s <port> <debug>\n",argv[0]); return -1; } int debug = 0; sscanf(argv[2]," %d",&debug); CRITICAL(res = api.initialise(argv[1],debug)); DEBUG(res = api.terminate()); return 0; }