Exemplo n.º 1
0
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);
}
Exemplo n.º 2
0
    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;
    }
Exemplo n.º 3
0
    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;
    }
Exemplo n.º 4
0
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;
}
Exemplo n.º 5
0
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();
    }
}
Exemplo n.º 6
0
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;
}
Exemplo n.º 7
0
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
    }
}