void urdfLink::setPosition(std::string gazebo_xyz,std::string choose)
{
if(choose == "inertial")
stringToArray(inertial_xyz,gazebo_xyz.c_str());
if(choose == "visual")
stringToArray(currentVisual().xyz,gazebo_xyz.c_str());
if(choose == "collision")
stringToArray(currentCollision().xyz,gazebo_xyz.c_str());
}
void urdfLink::setRotation(std::string gazebo_rpy,std::string choose)
{
if(choose == "inertial")
stringToArray(inertial_rpy,gazebo_rpy.c_str());
if(choose == "visual")
stringToArray(currentVisual().rpy,gazebo_rpy.c_str());
if(choose == "collision")
stringToArray(currentCollision().rpy,gazebo_rpy.c_str());
}
void urdfLink::setBox(std::string gazebo_size,std::string choose)
{
if(choose == "visual")
{
stringToArray(currentVisual().box_size, gazebo_size);
}
if(choose == "collision")
{
stringToArray(currentCollision().box_size, gazebo_size);
}
}
FB::variant hapticAPI::sendForce(std::string str)
{
double force[3];
stringToArray(str,force);
force[1] = -force[1];
if (hdPhantomSetForce(deviceID, /*Z*/&force[2], /*X*/&force[0], /*Y*/ &force[1])!=-1)
return true;
else
return false;
}
void urdfLink::setCylinder(std::string gazebo_radius,std::string gazebo_length,std::string choose)
{
urdfElement *element = NULL;
if(choose == "visual")
{
element = ¤tVisual();
}
if(choose == "collision")
{
element = ¤tCollision();
}
if (element != NULL) {
stringToArray(element->cylinder_size, gazebo_radius+" "+gazebo_radius+" "+gazebo_length);
element->cylinder_size[0] = element->cylinder_size[0] * 2; //Radius to bounding box conversion
element->cylinder_size[1] = element->cylinder_size[1] * 2; //Radius to bounding box conversion
}
}
void joint::setAxis(std::string gazebo_axis_xyz)
{
stringToArray(axis,gazebo_axis_xyz.c_str());
}
void joint::setOrigin(std::string gazebo_origin_xyz,std::string gazebo_origin_rpy)
{
stringToArray(origin_xyz,gazebo_origin_xyz);
stringToArray(origin_rpy,gazebo_origin_rpy);
}
void urdfLink::setColor(std::string color)
{
stringToArray(currentVisual().rgba,color);
}
StringTokenizer(const cc_string & str, const cc_string & szTokens)
{
stringToArray(str,m_array,szTokens);
m_index = 0;
}
void robot::readSensors()
{
{ // URDF sensors:
tinyxml2::XMLElement* sensorElement;
sensorElement = robotElement->FirstChildElement("sensor");
while (sensorElement)
{
sensor* Sensor=new sensor();
Sensor->name = sensorElement->Attribute("name");
Sensor->gazeboSpec=true;
tinyxml2::XMLElement* sensor_parentElement = sensorElement->FirstChildElement("parent");
if (sensor_parentElement!=NULL)
{
if (sensor_parentElement->Attribute("link") != NULL)
Sensor->parentLink=sensor_parentElement->Attribute("link");
}
tinyxml2::XMLElement* sensor_originElement = sensorElement->FirstChildElement("origin");
if(sensor_originElement != NULL)
{
if (sensor_originElement->Attribute("xyz")!= NULL)
stringToArray(Sensor->origin_xyz,sensor_originElement->Attribute("xyz"));
if (sensor_originElement->Attribute("rpy")!= NULL)
stringToArray(Sensor->origin_rpy,sensor_originElement->Attribute("rpy"));
}
tinyxml2::XMLElement* sensor_cameraElement = sensorElement->FirstChildElement("camera");
Sensor->cameraSensorPresent=(sensor_cameraElement != NULL);
if(sensor_cameraElement != NULL)
{
tinyxml2::XMLElement* sensor_imageElement = sensor_cameraElement->FirstChildElement("image");
if(sensor_imageElement != NULL)
{
if (sensor_imageElement->Attribute("width")!= NULL)
Sensor->resolution[0]=getInt(sensor_imageElement->Attribute("width"));
if (sensor_imageElement->Attribute("height")!= NULL)
Sensor->resolution[1]=getInt(sensor_imageElement->Attribute("height"));
if (sensor_imageElement->Attribute("near")!= NULL)
Sensor->clippingPlanes[0]=getFloat(sensor_imageElement->Attribute("near"));
if (sensor_imageElement->Attribute("far")!= NULL)
Sensor->clippingPlanes[1]=getFloat(sensor_imageElement->Attribute("far"));
}
}
tinyxml2::XMLElement* sensor_rayElement = sensorElement->FirstChildElement("ray");
Sensor->proximitySensorPresent=(sensor_rayElement != NULL);
vSensors.push_back(Sensor);
sensorElement = sensorElement->NextSiblingElement("sensor"); //moves to the next sensor
}
}
{ // GAZEBO SENSORS:
tinyxml2::XMLElement* gazeboElement;
gazeboElement = robotElement->FirstChildElement("gazebo");
while (gazeboElement)
{
tinyxml2::XMLElement* sensorElement;
sensorElement = gazeboElement->FirstChildElement("sensor");
while (sensorElement)
{
sensor* Sensor=new sensor();
Sensor->name = sensorElement->Attribute("name");
Sensor->gazeboSpec=true;
tinyxml2::XMLElement* sensor_parentElement = sensorElement->FirstChildElement("parent");
if (sensor_parentElement!=NULL)
{
if (sensor_parentElement->Attribute("link") != NULL)
Sensor->parentLink=sensor_parentElement->Attribute("link");
}
tinyxml2::XMLElement* sensor_originElement = sensorElement->FirstChildElement("origin");
if(sensor_originElement != NULL)
{
if (sensor_originElement->Attribute("xyz")!= NULL)
stringToArray(Sensor->origin_xyz,sensor_originElement->Attribute("xyz"));
if (sensor_originElement->Attribute("rpy")!= NULL)
stringToArray(Sensor->origin_rpy,sensor_originElement->Attribute("rpy"));
}
tinyxml2::XMLElement* sensor_cameraElement = sensorElement->FirstChildElement("camera");
Sensor->cameraSensorPresent=(sensor_cameraElement != NULL);
if(sensor_cameraElement != NULL)
{
tinyxml2::XMLElement* sensor_imageElement = sensor_cameraElement->FirstChildElement("image");
if(sensor_imageElement != NULL)
{
if (sensor_imageElement->Attribute("width")!= NULL)
Sensor->resolution[0]=getInt(sensor_imageElement->Attribute("width"));
if (sensor_imageElement->Attribute("height")!= NULL)
Sensor->resolution[1]=getInt(sensor_imageElement->Attribute("height"));
if (sensor_imageElement->Attribute("near")!= NULL)
Sensor->clippingPlanes[0]=getFloat(sensor_imageElement->Attribute("near"));
if (sensor_imageElement->Attribute("far")!= NULL)
Sensor->clippingPlanes[1]=getFloat(sensor_imageElement->Attribute("far"));
}
}
tinyxml2::XMLElement* sensor_rayElement = sensorElement->FirstChildElement("ray");
Sensor->proximitySensorPresent=(sensor_rayElement != NULL);
vSensors.push_back(Sensor);
sensorElement = sensorElement->NextSiblingElement("sensor"); //moves to the next sensor
}
gazeboElement = gazeboElement->NextSiblingElement("gazebo"); //moves to the next gazebo thing
}
}
}
void robot::readLinks()
{
tinyxml2::XMLElement* linkElement = robotElement->FirstChildElement("link");
while (linkElement)
{
urdfLink* Link=new urdfLink();
Link->name = linkElement->Attribute("name");
//INERTIAL
tinyxml2::XMLElement* inertialElement = linkElement->FirstChildElement("inertial");
if(inertialElement!=NULL)
{
Link->inertiaPresent=true;
tinyxml2::XMLElement* inertial_originElement = inertialElement->FirstChildElement("origin");
if(inertial_originElement != NULL)
{
if (inertial_originElement->Attribute("xyz")!= NULL)
Link->setPosition(inertial_originElement->Attribute("xyz"),"inertial");
if (inertial_originElement->Attribute("rpy")!= NULL)
Link->setRotation(inertial_originElement->Attribute("rpy"),"inertial");
}
tinyxml2::XMLElement* inertial_massElement = inertialElement->FirstChildElement("mass");
if(inertial_massElement!= NULL)
{
if (inertial_massElement->Attribute("value") != NULL)
Link->setMass(inertial_massElement->Attribute("value"));
}
tinyxml2::XMLElement* inertial_inertiaElement = inertialElement->FirstChildElement("inertia");
if(inertial_inertiaElement != NULL)
{
if (inertial_inertiaElement->Attribute("ixx") != NULL){ Link->setInertia(0,inertial_inertiaElement->Attribute("ixx")); }
if (inertial_inertiaElement->Attribute("ixy") != NULL){ Link->setInertia(1,inertial_inertiaElement->Attribute("ixy")); Link->setInertia(3,inertial_inertiaElement->Attribute("ixy"));}
if (inertial_inertiaElement->Attribute("ixz") != NULL){ Link->setInertia(2,inertial_inertiaElement->Attribute("ixz")); Link->setInertia(6,inertial_inertiaElement->Attribute("ixz")); }
if (inertial_inertiaElement->Attribute("iyy") != NULL){ Link->setInertia(4,inertial_inertiaElement->Attribute("iyy")); }
if (inertial_inertiaElement->Attribute("iyz") != NULL){ Link->setInertia(5,inertial_inertiaElement->Attribute("iyz")); Link->setInertia(7,inertial_inertiaElement->Attribute("iyz")); }
if (inertial_inertiaElement->Attribute("izz") != NULL){ Link->setInertia(8,inertial_inertiaElement->Attribute("izz")); }
}
Link->verifyInertia(); // required because some of the URDF files have inertia matrices that are 0!!!
}
//VISUAL
tinyxml2::XMLNode* visualElement = linkElement->FirstChildElement("visual");
while(visualElement != NULL)
{
Link->addVisual();
tinyxml2::XMLElement* visual_originElement = visualElement->FirstChildElement("origin");
if(visual_originElement != NULL)
{
if (visual_originElement->Attribute("xyz")!= NULL)
Link->setPosition(visual_originElement->Attribute("xyz"),"visual");
if (visual_originElement->Attribute("rpy")!= NULL)
Link->setRotation(visual_originElement->Attribute("rpy"),"visual");
}
tinyxml2::XMLElement* visual_geometryElement = visualElement->FirstChildElement("geometry");
if(visual_geometryElement!=NULL)
{
tinyxml2::XMLElement* visual_geometry_meshElement = visual_geometryElement->FirstChildElement("mesh");
if(visual_geometry_meshElement!=NULL)
{
if (visual_geometry_meshElement->Attribute("filename") != NULL)
Link->setMeshFilename(packagePath,visual_geometry_meshElement->Attribute("filename"),"visual");
if (visual_geometry_meshElement->Attribute("scale") != NULL)
stringToArray(Link->currentVisual().mesh_scaling,visual_geometry_meshElement->Attribute("scale"));
}
tinyxml2::XMLElement* visual_geometry_boxElement = visual_geometryElement->FirstChildElement("box");
if(visual_geometry_boxElement!=NULL)
{
if(visual_geometry_boxElement->Attribute("size") != NULL){Link->setBox(visual_geometry_boxElement->Attribute("size"),"visual");}
}
tinyxml2::XMLElement* visual_geometry_sphereElement = visual_geometryElement->FirstChildElement("sphere");
if(visual_geometry_sphereElement!=NULL)
{
if(visual_geometry_sphereElement->Attribute("radius") != NULL){Link->setSphere(visual_geometry_sphereElement->Attribute("radius"),"visual");}
}
tinyxml2::XMLElement* visual_geometry_cylinderElement = visual_geometryElement->FirstChildElement("cylinder");
if(visual_geometry_cylinderElement!=NULL)
{
if( visual_geometry_cylinderElement->Attribute("radius") != NULL && visual_geometry_cylinderElement->Attribute("length") != NULL)
{Link->setCylinder( visual_geometry_cylinderElement->Attribute("radius"),visual_geometry_cylinderElement->Attribute("length"),"visual");}
}
}
tinyxml2::XMLElement* visual_materialElement = visualElement->FirstChildElement("material");
if(visual_materialElement!=NULL)
{
tinyxml2::XMLElement* visual_geometry_colorElement = visual_materialElement->FirstChildElement("color");
if(visual_geometry_colorElement!=NULL)
{
if (visual_geometry_colorElement->Attribute("rgba") != NULL)
Link->setColor(visual_geometry_colorElement->Attribute("rgba"));
}
}
visualElement = visualElement->NextSiblingElement("visual");
}
//COLLISION
tinyxml2::XMLElement* collisionElement = linkElement->FirstChildElement("collision");
while(collisionElement != NULL)
{
Link->addCollision();
tinyxml2::XMLElement* collision_originElement = collisionElement->FirstChildElement("origin");
if(collision_originElement != NULL)
{
if (collision_originElement->Attribute("xyz")!= NULL)
Link->setPosition(collision_originElement->Attribute("xyz"),"collision");
if (collision_originElement->Attribute("rpy")!= NULL)
Link->setRotation(collision_originElement->Attribute("rpy"),"collision");
}
tinyxml2::XMLElement* collision_geometryElement = collisionElement->FirstChildElement("geometry");
if(collision_geometryElement!=NULL)
{
tinyxml2::XMLElement* collision_geometry_meshElement = collision_geometryElement->FirstChildElement("mesh");
if(collision_geometry_meshElement!=NULL)
{
if (collision_geometry_meshElement->Attribute("filename") != NULL)
Link->setMeshFilename(packagePath,collision_geometry_meshElement->Attribute("filename"),"collision");
if (collision_geometry_meshElement->Attribute("scale") != NULL)
stringToArray(Link->currentCollision().mesh_scaling,collision_geometry_meshElement->Attribute("scale"));
}
tinyxml2::XMLElement* collision_geometry_boxElement = collision_geometryElement->FirstChildElement("box");
if(collision_geometry_boxElement!=NULL)
{
if(collision_geometry_boxElement->Attribute("size") != NULL)
{Link->setBox(collision_geometry_boxElement->Attribute("size"),"collision");}
}
tinyxml2::XMLElement* collision_geometry_sphereElement = collision_geometryElement->FirstChildElement("sphere");
if(collision_geometry_sphereElement!=NULL)
{
if(collision_geometry_sphereElement->Attribute("radius") != NULL)
{Link->setSphere(collision_geometry_sphereElement->Attribute("radius"),"collision");}
}
tinyxml2::XMLElement* collision_geometry_cylinderElement = collision_geometryElement->FirstChildElement("cylinder");
if(collision_geometry_cylinderElement!=NULL)
{
if(collision_geometry_cylinderElement->Attribute("radius") != NULL && collision_geometry_cylinderElement->Attribute("length") != NULL)
{Link->setCylinder( collision_geometry_cylinderElement->Attribute("radius"),collision_geometry_cylinderElement->Attribute("length"),"collision");}
}
collisionElement = collisionElement->NextSiblingElement("collision");
}
}
vLinks.push_back(Link);
linkElement = linkElement->NextSiblingElement("link"); //moves to the next link
}
}
int main(int argc, char** argv){
char* myargs[100][100];
int i=1;
int k=0;
int m=0;
int fd_in;
while(i<argc){
if(strcmp(argv[i],"!")==0){k++;m=0;i++;}
else{
myargs[k][m]=argv[i];
i++;m++;}
}
// TODO: Create the child process.
signal(SIGUSR1,sigusr1_handler);
pid_t pid;
int h=0;
int mypipe[2];
while(h<=k)
{
pipe(mypipe);
pid=fork();
// TODO: Branch the program routine for parent and child
if(pid>0) //parent
{
signal(SIGINT, sigint_handler1);
int status;
struct timespec start;
struct timespec end;
close(mypipe[1]);
fd_in = mypipe[0];
clock_gettime(CLOCK_REALTIME,&start);
siginfo_t* infop;
if(kill(pid,SIGUSR1)==0){
//printf("Process with id: %d created for the command: %s\n",pid,myargs[h][0]);
}
waitid(P_PID,pid, infop, WEXITED|WNOWAIT);
clock_gettime(CLOCK_REALTIME,&end);
FILE * stat;
FILE * fstatus;
sprintf(fn1,"/proc/%d/stat",pid);
stat=fopen(fn1,"r");
if(stat==NULL){
printf("Error when open stat file\n");
exit(-1);
}
char str[300];
fgets(str,sizeof(char)*300,stat);
char** statget = stringToArray(str);
double count = sysconf(_SC_CLK_TCK);
fclose(stat);
sprintf(fn2,"/proc/%d/status",pid);
fstatus = fopen(fn2, "r");
if(fstatus==NULL){
printf("Error when open status file\n");
exit(-1);
}
char* strs=NULL;
size_t len=0;
int totalsw = 0;
int added = 0;
while(getline(&strs, &len, fstatus)!=-1){
if(!strstr(strs, "ctx"))
{continue;printf("sss111\n");}
else {
char* p;
p = strstr(strs, "ctxt");
char** temp = stringToArray(p);
//printf("switch1:%d \n",atol(temp[1]));
totalsw += atol(temp[1]);}
}
fclose(fstatus);
waitpid(pid,&status,0);
if(WIFSIGNALED(status)==1){
printf("The command %s is interrupted by the signal number = %d (%s) \n",myargs[h][0],WTERMSIG(status),getsigname(WTERMSIG(status)));
}
else{
printf("The child process %s has returned with code = %u\n",myargs[h][0],WEXITSTATUS(status));
}
double time = (double)(end.tv_sec-start.tv_sec)+(end.tv_nsec-start.tv_nsec)/1000000000.0;
printf ("Real: %.3fs ", time);
printf("user: %.3fs ", atol(statget[13])/count);
printf("system: %.3fs ", atol(statget[14])/count);
printf("context switch: %d\n", totalsw);
}
else if(pid==0) //child
{
printf("Process with id: %d created for the command: %s, h: %d\n", getpid(), myargs[h][0], h);
dup2(fd_in, 0);
if (k>0) //if there is a next cmd
{
if(h<k){dup2(mypipe[1], 1);}
//if(h!=k){
//close(1);}
close(mypipe[0]);}
if(execvp(myargs[h][0], myargs[h])==-1){
perror("Error: ");
exit(1);
}
}
else{
}
h++;
}
return 0;
}