|
|
|
@ -159,27 +159,7 @@ class mavlink_publisher():
|
|
|
|
def packEmit_global_rel(cls, emitParams, publisher):
|
|
|
|
def packEmit_global_rel(cls, emitParams, publisher):
|
|
|
|
mav_msg = emitParams['global_position']
|
|
|
|
mav_msg = emitParams['global_position']
|
|
|
|
msg = std_msgs.msg.Float64()
|
|
|
|
msg = std_msgs.msg.Float64()
|
|
|
|
msg.data = mav_msg.relative_alt/1e3
|
|
|
|
msg.data = float(mav_msg.relative_alt/1e3)
|
|
|
|
publisher.publish(msg)
|
|
|
|
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def create_global_vel(self, sysid, component_obj):
|
|
|
|
|
|
|
|
target_topic = 'global_position/gp_vel'
|
|
|
|
|
|
|
|
try:
|
|
|
|
|
|
|
|
_ = component_obj.emitParams['global_position']
|
|
|
|
|
|
|
|
except KeyError:
|
|
|
|
|
|
|
|
logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
|
|
|
|
|
|
|
|
return False
|
|
|
|
|
|
|
|
topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
|
|
|
|
|
|
|
|
publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1)
|
|
|
|
|
|
|
|
component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_vel]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def packEmit_global_vel(cls, emitParams, publisher):
|
|
|
|
|
|
|
|
mav_msg = emitParams['global_position']
|
|
|
|
|
|
|
|
msg = geometry_msgs.msg.Vector3()
|
|
|
|
|
|
|
|
msg.x = mav_msg.vx/1e2
|
|
|
|
|
|
|
|
msg.y = mav_msg.vy/1e2
|
|
|
|
|
|
|
|
msg.z = mav_msg.vz/1e2
|
|
|
|
|
|
|
|
publisher.publish(msg)
|
|
|
|
publisher.publish(msg)
|
|
|
|
pass
|
|
|
|
pass
|
|
|
|
|
|
|
|
|
|
|
|
|