def request_heartbeat(self, request):
self.commander.heartbeat_time = rospy.get_rostime()
reply = messages_pb2.Reply()
reply.timestamp = int(self.commander.heartbeat_time.to_sec())
m_state = self.commander.mavros_gw.get_mavros_state()
m_local_pose = self.commander.mavros_gw.get_mavros_local_pose()
m_gps = self.commander.mavros_gw.get_mavros_global_gps()
# m_bl_trans = self.commander.mavros_gw.get_mavros_base_link_transform()
m_bl_trans = None
b_state = BytesIO()
b_local_pose = BytesIO()
b_gps = BytesIO()
b_bl_trans = BytesIO()
m_state.serialize(b_state)
m_local_pose.serialize(b_local_pose)
m_gps.serialize(b_gps)
if m_bl_trans is not None:
m_bl_trans.serialize(b_bl_trans)
reply.heartbeat_data.base_link_transform = b_bl_trans.getvalue()
l_cmd_id, l_cmd_completed = self.commander.get_last_command()
reply.heartbeat_data.last_command_id = l_cmd_id
reply.heartbeat_data.last_command_completed = l_cmd_completed
reply.heartbeat_data.mavros_state = b_state.getvalue()
reply.heartbeat_data.local_pose = b_local_pose.getvalue()
reply.heartbeat_data.gps_nav_sat = b_gps.getvalue()
data = reply.SerializeToString()
rospy.loginfo('%s: Reply size %s', self.commander.namespace, len(data))
self.socket.sendto(data, self.clientAddress)
def _recv_thread(self):
while not rospy.is_shutdown():
data = self.socket.recv(1024)
reply = messages_pb2.Reply()
try:
reply.ParseFromString(data)
self.recv_q.put(reply)
except Exception as e:
rospy.logerr(str(e) + ' data: ' + str(self.data))
return
def _recv_thread(self):
while not rospy.is_shutdown():
try:
data = self._recv_msg()
reply = messages_pb2.Reply()
reply.ParseFromString(data)
self.recv_q.put(reply)
except verify_data.VerifyError as e:
rospy.logerr(str(e))
except Exception as e:
rospy.logerr(str(e))
def heartbeat(hsock):
request = messages_pb2.Request()
request.command = messages_pb2.Command.HEART_BEAT
data = request.SerializeToString()
print(len(data))
hsock.sendto(data, ('127.0.0.1', 4444))
received = hsock._recv_msg(1024)
reply = messages_pb2.Reply()
reply.ParseFromString(received)
a = BytesIO()
a.write(reply.heartbeatData.mavrosState)
mavros_state = State()
mavros_state.deserialize(reply.heartbeatData.mavrosState)
print (reply)
print (mavros_state)
try:
t1.start()
except:
pass
clisock = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM)
request = messages_pb2.Request()
request.command = messages_pb2.Command.SET_OFFBOARD
data = request.SerializeToString()
print(len(data))
clisock.sendto(data, ('127.0.0.1', 4444))
received = clisock.recv(1024)
reply = messages_pb2.Reply()
reply.ParseFromString(received)
print (reply)
time.sleep(10)
request = messages_pb2.Request()
request.command = messages_pb2.Command.SET_ARM
data = request.SerializeToString()
print(len(data))
clisock.sendto(data, ('127.0.0.1', 4444))
received = clisock.recv(1024)
reply = messages_pb2.Reply()