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