def run(self):
request_processors = {
messages_pb2.Command.SET_OFFBOARD: self.request_offboard,
messages_pb2.Command.SET_ARM: self.request_arm,
messages_pb2.Command.SET_RETURN_TO_HOME:
self.request_return_to_home,
messages_pb2.Command.GOTO: self.request_goto,
}
request = messages_pb2.Request()
try:
request.ParseFromString(self.data)
except Exception as e:
rospy.logerr(str(e) + ' data: ' + self.data)
return
print(request)
if request.command in request_processors.keys():
# Run one request at a time
if self.commander.cmd_server_lock.acquire(False):
self.commander.set_last_command(request.id, False)
request_processors[request.command](request)
self.commander.cmd_server_lock.release()
self.commander.set_last_command(request.id, True)
elif request.command == messages_pb2.Command.HEARTBEAT:
# Heartbeat can be asynchronous
self.request_heartbeat(request)
def _goto(param):
request = messages_pb2.Request()
request.command = messages_pb2.Command.GOTO
pose = param[0]
b_pose = BytesIO()
pose.serialize(b_pose)
request.req_data = b_pose.getvalue()
return request
def _send_heartbeat(self):
now = rospy.get_rostime()
if self.heartbeat_time is not None and now - self.heartbeat_time < rospy.Duration(
1):
return
request = messages_pb2.Request()
request.timestamp = int(now.to_sec())
request.command = messages_pb2.Command.HEARTBEAT
self.socket.send(request.SerializeToString())
self.heartbeat_time = now
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)
def thd():
hsock = socket.socket(family=socket.AF_INET, type=socket.SOCK_DGRAM)
while True:
time.sleep(2)
heartbeat(hsock)
t1 = threading.Thread(target=thd)
t1.daemon = True
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)
def _set_return_to_home():
request = messages_pb2.Request()
request.command = messages_pb2.Command.SET_RETURN_TO_HOME
return request
def _set_arm():
request = messages_pb2.Request()
request.command = messages_pb2.Command.SET_ARM
return request
def _set_offboard():
request = messages_pb2.Request()
request.command = messages_pb2.Command.SET_OFFBOARD
return request
def create_return_to_home_message():
request = messages_pb2.Request()
request.id = 1
request.command = messages_pb2.Command.SET_RETURN_TO_HOME
return request
