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)
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_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)
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)
