def Get_Body_Data(body_id):
    bodies = Qs.get_updated_bodies()
    body_length = len(bodies)
    body_indice = -1

    if isinstance(bodies, list):
        for i in range(0, body_length):
            if (bodies[i]['id'] == body_id):
                body_indice = i

    data = QuadPosition()

    if (body_indice == -1):
        data.found_body = False
        return (data)

    data.found_body = True
    data.x = bodies[body_indice]["x"]
    data.y = bodies[body_indice]["y"]
    data.z = bodies[body_indice]["z"]
    data.pitch = bodies[body_indice]["pitch"]
    data.roll = bodies[body_indice]["roll"]
    data.yaw = bodies[body_indice]["yaw"]

    return (data)
    def get_data(self, bodies, body_id, time):
        data = QuadPosition()

        data.found_body = False

        if bodies == []:
            utils.loginfo("Gazebo not started")
        else:
            if hasattr(bodies, 'name'):
                if body_id < len(bodies.name):
                    data.found_body = True
                    data.x = bodies.pose[body_id].position.x
                    data.y = bodies.pose[body_id].position.y
                    data.z = bodies.pose[body_id].position.z
                    x = bodies.pose[body_id].orientation.x
                    y = bodies.pose[body_id].orientation.y
                    z = bodies.pose[body_id].orientation.z
                    w = bodies.pose[body_id].orientation.w

                    dcm = quat_to_dcm(w, x, y, z)
                    (roll, pitch, yaw) = dcm.to_euler()

                    data.pitch = degrees(pitch)
                    data.roll = degrees(roll)
                    data.yaw = degrees(yaw)
                else:
                    print 'Body not found'

        return copy.deepcopy(data)
Example #3
0
    def get_data(self, body):
        data = QuadPosition()

        body_id = body

        data.found_body = False

        if self.bodies == []:
            utils.loginfo("Gazebo not started")
        else:
            if hasattr(self.bodies, 'name'):
                if body_id < len(self.bodies.name):
                    data.found_body = True
                    data.x = self.bodies.pose[body_id].position.x
                    data.y = self.bodies.pose[body_id].position.y
                    data.z = self.bodies.pose[body_id].position.z
                    x = self.bodies.pose[body_id].orientation.x
                    y = self.bodies.pose[body_id].orientation.y
                    z = self.bodies.pose[body_id].orientation.z
                    w = self.bodies.pose[body_id].orientation.w

                    dcm = quat_to_dcm(w, x, y, z)
                    (roll, pitch, yaw) = dcm.to_euler()

                    data.pitch = degrees(pitch)
                    data.roll = degrees(roll)
                    data.yaw = degrees(yaw)

                    print "Sending data for the body with id: " + str(body_id)
                else:
                    print 'Body not found'

        return (data)
Example #4
0
def Get_Body_Data(body_id):
	bodies=Qs.get_updated_bodies()
	body_length=len(bodies)
	body_indice=-1

	if isinstance(bodies,list):
		for i in range(0,body_length):
			if(bodies[i]['id']==body_id):
				body_indice=i

	data=QuadPosition()

	if(body_indice==-1):
		#utils.logerr('Body %i not found'%(body_id))
		data.found_body=False
		return(data)
	
	
	data.found_body=True
	data.x=bodies[body_indice]["x"]
	data.y=bodies[body_indice]["y"]
	data.z=bodies[body_indice]["z"]
	data.pitch=bodies[body_indice]["pitch"]
	data.roll=bodies[body_indice]["roll"]
	data.yaw=bodies[body_indice]["yaw"]

	return(data)
Example #5
0
	def get_data(self,body):
		data = QuadPosition()

		body_id = body

		data.found_body = False
		
		if self.bodies==[]:
			utils.loginfo("Gazebo not started")
		else:
			if hasattr(self.bodies,'name'):
				if body_id<len(self.bodies.name):
					data.found_body=True
					data.x=self.bodies.pose[body_id].position.x
					data.y=self.bodies.pose[body_id].position.y
					data.z=self.bodies.pose[body_id].position.z
					x = self.bodies.pose[body_id].orientation.x
					y = self.bodies.pose[body_id].orientation.y
					z = self.bodies.pose[body_id].orientation.z
					w = self.bodies.pose[body_id].orientation.w

					dcm = quat_to_dcm(w,x,y,z)
					(roll,pitch,yaw) = dcm.to_euler()

					data.pitch=degrees(pitch)
					data.roll=degrees(roll)
					data.yaw=degrees(yaw)
					
					print "Sending data for the body with id: "+str(body_id)
				else:
					print 'Body not found'
			
		return(data)
	def get_data(self,bodies,body_id,time):
		data = QuadPosition()

		data.found_body = False
		
		if bodies==[]:
			utils.loginfo("Gazebo not started")
		else:
			if hasattr(bodies,'name'):
				if body_id<len(bodies.name):
					data.found_body=True
					data.x=bodies.pose[body_id].position.x
					data.y=bodies.pose[body_id].position.y
					data.z=bodies.pose[body_id].position.z
					x = bodies.pose[body_id].orientation.x
					y = bodies.pose[body_id].orientation.y
					z = bodies.pose[body_id].orientation.z
					w = bodies.pose[body_id].orientation.w

					dcm = quat_to_dcm(w,x,y,z)
					(roll,pitch,yaw) = dcm.to_euler()

					data.pitch=degrees(pitch)
					data.roll=degrees(roll)
					data.yaw=degrees(yaw)
				else:
					print 'Body not found'
			
		return copy.deepcopy(data)