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
}
}
