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