예제 #1
0
파일: link.cpp 프로젝트: RhobanProject/Vrep
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());
}
예제 #2
0
파일: link.cpp 프로젝트: RhobanProject/Vrep
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());
}
예제 #3
0
파일: link.cpp 프로젝트: RhobanProject/Vrep
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);
	}
}
예제 #4
0
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;
}
예제 #5
0
파일: link.cpp 프로젝트: RhobanProject/Vrep
void urdfLink::setCylinder(std::string gazebo_radius,std::string gazebo_length,std::string choose)
{
    urdfElement *element = NULL;

	if(choose == "visual")
	{
        element = &currentVisual();	
	}
	if(choose == "collision")
	{
        element = &currentCollision();	
	}

    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
    }
}
예제 #6
0
void joint::setAxis(std::string gazebo_axis_xyz)
{
	stringToArray(axis,gazebo_axis_xyz.c_str());
}
예제 #7
0
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);
}
예제 #8
0
파일: link.cpp 프로젝트: RhobanProject/Vrep
void urdfLink::setColor(std::string color)
{
	stringToArray(currentVisual().rgba,color);
}
예제 #9
0
    StringTokenizer(const cc_string & str, const cc_string & szTokens)
	{
		stringToArray(str,m_array,szTokens);
		m_index = 0;
	}
예제 #10
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
	}
	}
}
예제 #11
0
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
	}
}
예제 #12
0
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;
}