static void mdlStart(SimStruct *S) { // ######### YARP INITIALIZATION STUFF ################## Network::init(); fprintf(stderr,"YARP NETWORK INITIALIZED\n"); if (!Network::checkNetwork() || !Network::initialized()){ ssSetErrorStatus(S,"YARP server wasn't found active!! \n"); return; } else cout<<"YARP is running!!\n"<<endl; int_T buflen, status; char *String; buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_1)))*sizeof(mxChar)+1; String = static_cast<char*>(mxMalloc(buflen)); status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)),String,buflen); if (status) { ssSetErrorStatus(S,"Cannot retrieve string from parameter 1!! \n"); return; } //string port_name = String; char *port_name = String; //FROM port name buflen = mxGetN((ssGetSFcnParam(S, PARAM_IDX_2)))*sizeof(mxChar)+1; String = static_cast<char*>(mxMalloc(buflen)); status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_2)),String,buflen); if (status) { ssSetErrorStatus(S,"Cannot retrieve string from parameter 2!! \n"); return; } char *toPort_name = String; // ######## CHECKING INPUT PARAMETERS ############ BufferedPort<Vector> *toPort; toPort = new BufferedPort<Vector>; toPort->open(toPort_name); ConstString toPortName = toPort->getName(); cout<<"[From] Port name will be: "<<port_name<<endl; cout<<"[To] Port name will be: "<<toPort->getName()<<endl; ssGetPWork(S)[0] = toPort; Network::connect(port_name,toPortName); }
bool open(Searchable& p) { bool dev = true; if (p.check("nodevice")) { dev = false; } if (dev) { poly.open(p); if (!poly.isValid()) { printf("cannot open driver\n"); return false; } if (!p.check("mute")) { // Make sure we can write sound poly.view(put); if (put==NULL) { printf("cannot open interface\n"); return false; } } } port.setStrict(true); if (!port.open(p.check("name",Value("/yarphear")).asString())) { printf("Communication problem\n"); return false; } if (p.check("remote")) { Network::connect(p.check("remote",Value("/remote")).asString(), port.getName()); } return true; }
bool configure(ResourceFinder &rf) { string name=rf.check("name",Value("imuFilter")).asString(); string robot=rf.check("robot",Value("icub")).asString(); size_t gyro_order=(size_t)rf.check("gyro-order",Value(5)).asInt(); size_t mag_order=(size_t)rf.check("mag-order",Value(51)).asInt(); mag_vel_thres_up=rf.check("mag-vel-thres-up",Value(0.04)).asDouble(); mag_vel_thres_down=rf.check("mag-vel-thres-down",Value(0.02)).asDouble(); bias_gain=rf.check("bias-gain",Value(0.001)).asDouble(); verbose=rf.check("verbose"); gyroFilt.setOrder(gyro_order); magFilt.setOrder(mag_order); biasInt.setTs(bias_gain); gyroBias.resize(3,0.0); adaptGyroBias=false; iPort.open("/"+name+"/inertial:i"); oPort.open("/"+name+"/inertial:o"); bPort.open("/"+name+"/bias:o"); string imuPortName=("/"+robot+"/inertial"); if (!Network::connect(imuPortName,iPort.getName())) yWarning("Unable to connect to %s",imuPortName.c_str()); return true; }
int main(int argc, char** argv) { /* ###################################################################################### ############################## GENERAL PORT CREATION ################################# ###################################################################################### */ bool imgIn = imageInPort.open("/home/image/in"); // open the ports and save the return value bool imgOut = imageOutPort.open("/home/image/out"); // ^^ bool iKinIn = iKinInPort.open("/home/iKinIn"); // ^^ bool iKinOut = iKinOutPort.open("/home/iKinOut"); // ^^ waitKey(50); // give the connections some time to be established if (!iKinIn || !iKinOut || !imgIn || !imgOut) { // Error handling in case a connection couldn't be established printf("Failed to create ports.\n"); // notify the user printf("Maybe you need to start a nameserver (run 'yarpserver' \n"); printf("Press Return to Exit\n"); std::cin.get(); // Wait for the Return Key return ERROR_DEV_NOT_EXIST; // give an appropriate error code } bool yc1 = yarpPort.connect("/icubSim/cam/left", "/home/image/in"); // connect the ports and save the return value bool yc2 = yarpPort.connect("/iKinGazeCtrl/x:o", iKinInPort.getName()); // ^^ bool yc3 = yarpPort.connect(iKinOutPort.getName(), "/iKinGazeCtrl/xd:i"); // ^^ yarpPort.connect(imageOutPort.getName(), "/view/ase"); // we connect the YarpView -> but in case its not yet open, we don't care waitKey(250); // give the connections some time to be established if (!yc1 || !yc2 || !yc3) { // Error handling in case a connection couldn't be established printf("Failed to connect the ports.\n"); // notify the user printf("Maybe you need to start a nameserver (run 'yarpserver' \n"); printf("Press Return to Exit\n"); std::cin.get(); // Wait for the Return Key return ERROR_DEV_NOT_EXIST; // give an appropriate error code } /* ###################################################################################### #################################### MAIN LOOP ####################################### ###################################################################################### */ for (;;)//(int i = 0;i < 500;i++) { if (int return_val = MainLoop()) // Running the inner while encapsulated, helps managing the memory return return_val; // catching possible error messages } // ################# // hypothetical exit (for using an ending loop) // ################# yarpPort.disconnect("/icubSim/cam/left", "/home/image/in"); // we disconnect all connections to ensure a nice application exit yarpPort.disconnect("/iKinGazeCtrl/x:o", iKinInPort.getName()); yarpPort.disconnect(iKinOutPort.getName(), "/iKinGazeCtrl/xd:i"); std::cin.get(); // if we ever get here, wait for user input return ERROR_SUCCESS; }
int main(void) { char cmd; /* string ressac_position_port = "/ors/robots/OBRessac/Motion_Controller/vxvyvz "; string ressac_rotation_port = "/ors/robots/OBRessac/Motion_Controller/vxvyvz "; //string target_position_port = "/ors/robots/OBATRV/Motion_Controller/destination"; string target_position_port ="/ors/robots/OBATRV/Motion_Controller/vxvyvz"; */ string ressac_position_port = "/ors/robots/OBRessac/Position_Controller/position"; string ressac_rotation_port = "/ors/robots/OBRessac/Position_Controller/angle"; //string target_position_port = "/ors/robots/OBATRV/Motion_Controller/destination"; string target_position_port ="/ors/robots/OBATRV/Position_Controller/position"; string ressac_port = "ressac"; string target_port = "target"; cout << "********* Ressac client *********" << endl; cout << "Display RESSAC drone gps position + move RESSAC in OpenRobots simulator" << endl; cout << "Press ctrl+c to exit." << endl; //We catch ctrl+c to cleanly close the application signal( SIGINT,sigproc); //setvbuf(stdin, NULL, _IONBF, 0); //Initialization of Yarp network Network::init(); cout << "\n * YARP network initialized." << endl; //Connect to OpenRobots simulator toRessacPositionPort.open(("/" + ressac_port + "/position").c_str()); toTargetPositionPort.open(("/" + target_port + "/position").c_str()); Network::connect(toRessacPositionPort.getName().c_str(), ressac_position_port.c_str()); Network::connect(toTargetPositionPort.getName().c_str(), target_position_port.c_str()); cout << " * Writing commands to " << ressac_position_port << endl; cout << " * Writing commands to " << target_position_port << endl; cout << " * Starting now..." << endl; ifstream file; file.open("data_sync_vol4.txt", ifstream::in); //nextCmd=time(NULL); if(!file.is_open()) { cout <<"File not found."<<endl; } else { cout <<"File opened."<<endl; double nextTime=3.5822500e+005; double prevTime=3.5822500e+005; double rx=0; double ry=0; double rz=0; double rvx=0; double rvy=0; double rvz=0; double rax=0; double ray=0; double raz=0; double rrx=0; double rry=0; double rrz=0; double tx=0; double ty=0; double tz=0; double tvx=0; double tvy=0; double tvz=0; double prrx=0; double prry=0; double prrz=0; double clockTime=0; while(!file.eof()) { usleep((nextTime-prevTime)*1000.0); // nextCmd=time(NULL); prevTime=nextTime; file >> nextTime; file >> rx; file >> ry; file >> rz; file >> rvx; file >> rvy; file >> rvz; file >> rax; file >> ray; file >> raz; file >> rrx; file >> rry; file >> rrz; file >> tx; file >> ty; file >> tz; file >> tvx; file >> tvy; file >> tvz; static double stx=tx; static double sty=ty; static double stz=tz; Bottle& cmdRessacBottle = toRessacPositionPort.prepare(); cmdRessacBottle.clear(); cmdRessacBottle.addDouble(rx); cmdRessacBottle.addDouble(ry); cmdRessacBottle.addDouble(rz); cmdRessacBottle.addDouble(rrx); cmdRessacBottle.addDouble(rry); cmdRessacBottle.addDouble(rrz); /* cmdBottle.addDouble(ax); cmdBottle.addDouble(ay); cmdBottle.addDouble(az); */ toRessacPositionPort.write(); cout << (nextTime-prevTime)<<" Send Ressac position -> rx: " << rx << " ry: " << ry <<" rz: "<< rz <<endl; cout << (nextTime-prevTime)<<" Send Ressac rotation -> ax: " << rrx-prrx << " ay: " << rry-prry <<" az: "<< rrz-prrz <<endl; prrx=rrx; prry=rry; prrz=rrz; Bottle& cmdTargetBottle = toTargetPositionPort.prepare(); cmdTargetBottle.clear(); cmdTargetBottle.addDouble(tvx); cmdTargetBottle.addDouble(-tvy); cmdTargetBottle.addDouble(tvz); /* cmdBottle.addDouble(ax); cmdBottle.addDouble(ay); cmdBottle.addDouble(az); */ toTargetPositionPort.write(); cout << (nextTime-prevTime)<<" Send Target position -> tx: " << tvx << " ty: " << tvy <<" tz: "<< tvz <<endl; } Bottle& cmdRessacBottle = toRessacPositionPort.prepare(); cmdRessacBottle.clear(); cmdRessacBottle.addDouble(0); cmdRessacBottle.addDouble(0); cmdRessacBottle.addDouble(0); cmdRessacBottle.addDouble(0); cmdRessacBottle.addDouble(0); cmdRessacBottle.addDouble(0); toRessacPositionPort.write(); } }
int main(int argc, char* argv[]) { // No parameter needed (for the moment) if (argc != 1) { usage (argv[0]); exit (1); } string command; string robot_list; bool waiting = true; Bottle *incomingBottle; // Define the names of the ports opened by this program string local_admin_in_port = "/scene/admin/in/"; string local_admin_out_port = "/scene/admin/out/"; // Define the names for the comunication ports opened by Blender string ors_admin_in_port = "/ors/admin/in"; string ors_admin_out_port = "/ors/admin/out"; cout << "********* Scene Initialization Client *********" << endl; cout << "Get the list of robots provided by the ORS" << endl; cout << "Press ctrl+c to exit." << endl; //We catch ctrl+c to cleanly close the application signal( SIGINT,sigproc); //setvbuf(stdin, NULL, _IONBF, 0); //Initialization of Yarp network Network::init(); cout << "\n * YARP network initialized." << endl; // Connect to Open Robots Simulator LocalAdminInPort.open(local_admin_in_port.c_str()); LocalAdminOutPort.open(local_admin_out_port.c_str()); Network::connect(LocalAdminOutPort.getName().c_str(), ors_admin_in_port.c_str()); Network::connect( ors_admin_out_port.c_str(), LocalAdminInPort.getName().c_str()); cout << " * Receiving ORS communication port: " << ors_admin_in_port << endl; cout << " * Transmitting ORS communication port: " << ors_admin_out_port << endl; cout << " * Requesting list of robots..." << endl; // Prepare the data bottle to send Bottle& cmdBottle = LocalAdminOutPort.prepare(); cmdBottle.clear(); cmdBottle.addString("list_robots"); // cin.ignore(); LocalAdminOutPort.write(); cout << " * Waiting for the reply..." << endl; while(waiting) { // Read on the Blender output port, but don't wait. incomingBottle = LocalAdminInPort.read(false); if (incomingBottle != NULL) { cout << " RESPONSE: " << incomingBottle->toString().c_str() << endl; robot_list = (std::string) incomingBottle->toString(); if (! (robot_list == "") ) waiting = false; } } cout << " * Got the list of scene elements: " << robot_list << endl; }
int main(int argc, char* argv[]) { string robot_name; string component_name; int num_cameras; bool connected = false; // Use parameters as the name of the robot and the component if (argc == 3) { robot_name = argv[1]; num_cameras = atoi(argv[2]); // component_name = argv[2]; } // If parameters are not given, use default values else if (argc == 1) { robot_name = "OBATRV"; num_cameras = 0; } else { usage (argv[0]); exit (1); } char cmd; string port_prefix = "/ors/robots/" + robot_name + "/"; string view_prefix = "/img/" + robot_name + "/"; string yarpview_path = "/home/gechever/openrobots/bin/yarpview"; // Define the names for the comunication ports string atrv_motor_port = port_prefix + "OBMotion_Controller/in"; string atrv_output_port_gps = port_prefix + "OBGPS/out"; string local_port_prefix = "/atrv_client/" + robot_name; cout << "********* ATRV client *********" << endl; cout << "Display the ATRV drone gps position + move the ATRV in OpenRobots simulator" << endl; cout << "Press ctrl+c to exit." << endl; //We catch ctrl+c to cleanly close the application signal( SIGINT,sigproc); //setvbuf(stdin, NULL, _IONBF, 0); //Initialization of Yarp network Network::init(); cout << "\n * YARP network initialized." << endl; // Start the yarpview windows and connect them the corresponding ports cout << " * Initializing yarpview windows." << endl; for (int i=0; i<num_cameras; i++) { // Convert the integer to string std::stringstream out; out << i+1; // Prepare the ports to be used string img_view_port = view_prefix + "Camera" + out.str(); string atrv_camera_port = port_prefix + "Camera" + out.str(); cout << "\tConnecting: " << img_view_port << " to " << atrv_camera_port << endl; // string command = yarpview_path + " " + img_view_port; // system (command.c_str()); // int result = execlp("eog", "/home/gechever/Desktop/dala_blender_links.png"); // int result = execl(yarpview_path.c_str(), atrv_camera_port.c_str(), img_view_port.c_str()); // cout << "EXECUTION OF execl RETURNED: " << result << endl; Network::connect( atrv_camera_port.c_str(), img_view_port.c_str() ); } //Connect to OpenRobots simulator toATRVPort.open((local_port_prefix + "/out/destination").c_str()); fromATRVGPSPort.open((local_port_prefix + "/in/gps").c_str()); connected = Network::connect(toATRVPort.getName().c_str(), atrv_motor_port.c_str()); if (!connected) { printf ("\nClient ERROR: Port '%s' not found. Quitting\n", atrv_motor_port.c_str()); exit (1); } connected = Network::connect(atrv_output_port_gps.c_str() ,fromATRVGPSPort.getName().c_str()); if (!connected) { printf ("\nClient WARNING: Port '%s' not found. Quitting\n", atrv_output_port_gps.c_str()); // exit (1); } cout << " * Writing commands to " << atrv_motor_port << endl; cout << " * Listening status on " << atrv_output_port_gps << endl; cout << " * Enter destination coordinates, separated by a space" << endl; cout << " * Example: 1.52 3.38 0.0" << endl; cout << " * NOTE: The third coordinate (Z) is not used in the case of ATRV" << endl; cout << " * Starting now..." << endl; double dest[3] = {0.0, 0.0, 0.0}; while(true) { Bottle *incomingBottle; //read on the ATRV output port, but don't wait. incomingBottle = fromATRVGPSPort.read(false); if (incomingBottle != NULL) { double x = incomingBottle->get(0).asDouble(); double y = incomingBottle->get(1).asDouble(); double z = incomingBottle->get(2).asDouble(); cout << "Current ATRV GPS position -> x: " << x << " y: " << y <<" z: "<< z <<endl; } cout << "Enter destination coordinates:"<<endl; cin >> dest[X] >> dest[Y] >> dest[Z]; cout << "Destination: " << dest[X] << ", " << dest[Y] << ", " << dest[Z] << endl; // Prepare the data bottle to send Bottle& cmdBottle = toATRVPort.prepare(); cmdBottle.clear(); cmdBottle.addDouble(dest[X]); cmdBottle.addDouble(dest[Y]); cmdBottle.addDouble(dest[Z]); cin.ignore(); toATRVPort.write(); } }
static void mdlStart(SimStruct *S) { // ######### YARP INITIALIZATION STUFF ################## Network::init(); #ifndef NDEBUG mexPrintf("YARP NETWORK INITIALIZED\n"); #endif if (!Network::checkNetwork() || !Network::initialized()){ ssSetErrorStatus(S,"YARP server wasn't found active!! \n"); return; } #ifndef NDEBUG mexPrintf("Yarp network running\n"); #endif int_T autoconnect = GET_OPT_AUTOCONNECT; int_T errorOnMissingPort = GET_OPT_ERROR_ON_MISSING_PORT; #ifndef NDEBUG mexPrintf("Autoconnect option is %d\n", autoconnect); #endif ssGetIWork(S)[2] = autoconnect; int_T buflen, status; char *buffer = NULL; buflen = (1 + mxGetN(ssGetSFcnParam(S, PARAM_IDX_1))) * sizeof(mxChar); buffer = static_cast<char*>(mxMalloc(buflen)); status = mxGetString((ssGetSFcnParam(S, PARAM_IDX_1)), buffer, buflen); if (status) { ssSetErrorStatus(S,"Cannot retrieve string from port name parameter"); return; } std::string sourcePortName; std::string destinationPortName; if (autoconnect) { sourcePortName = buffer; destinationPortName = "..."; } else { destinationPortName = buffer; } mxFree(buffer); buffer = NULL; //######## CHECKING INPUT PARAMETERS ############ BufferedPort<Vector> *port; port = new BufferedPort<Vector>(); ssGetPWork(S)[0] = port; if (!port || !port->open(destinationPortName)) { ssSetErrorStatus(S,"Error while opening yarp port"); return; } #ifndef NDEBUG mexPrintf("Opened port %s\n", port->getName().c_str()); #endif int_T blocking = GET_OPT_BLOCKING; #ifndef NDEBUG mexPrintf("Blocking option is %d\n", blocking); #endif ssGetIWork(S)[0] = blocking; int_T timestamp = GET_OPT_TIMESTAMP; #ifndef NDEBUG mexPrintf("Timestamp option is %d\n", timestamp); #endif ssGetIWork(S)[1] = timestamp; if (autoconnect) { if (!Network::connect(sourcePortName, port->getName())) { mexPrintf("Failed to connect %s to %s\n", sourcePortName.c_str(), port->getName().c_str()); if (errorOnMissingPort) { ssSetErrorStatus(S,"ERROR connecting ports!"); } return; } #ifndef NDEBUG mexPrintf("Connected %s to %s\n", sourcePortName.c_str(), port->getName().c_str()); #endif } }