def client_connected():
while True:
time.sleep(1)
try:
#client.connect('47.100.92.173', 11883, 60) # 600 is keepalive
client.connect('47.110.250.141', 7200, 3600)
client.subscribe('as/wrc/upper/#', qos=0)
client.loop_forever() # keeplive
break
except Exception:
cmd = BoatCmd()
cmd.cmd = BoatCmd.STOP
cmd.data_float32 = []
cmd.data_int = []
pub_manual_cmd.publish(cmd)
pub_state.publish(str(0))
print("timeout,try again...")
def on_message(client, userdata, msg):
topic = msg.topic
data = json.loads(msg.payload)
cmd = BoatCmd()
print(topic)
print(data)
if topic == 'as/wrc/upper/move':
cmd.cmd = BoatCmd.SPEED
cmd.data_float32 = [
float(data['fbValue']),
float(data['lrValue'] / 10.0)
]
pub_manual_cmd.publish(cmd)
pub_state.publish(str(data['pilotMode']))
elif topic == 'as/wrc/upper/operate':
cmd.cmd = BoatCmd.LIGHT
cmd.data_int = [
int(data['resetLight']),
int(data['leftLight']),
int(data['rightLight']),
int(data['horn'])
]
pub_cmd.publish(cmd)
elif topic == 'as/wrc/upper/map':
#print("map")
#gps=BoatGpsPoint()
#set_gps = []
set_gps = BoatGpsPoints()
for i in range(len(data) / 2):
#print(i)
gps = BoatGpsPoint()
gps.longitude = float(data['point[%d]' % (i * 2)])
gps.latitude = float(data['point[%d]' % (i * 2 + 1)])
#print(gps)
set_gps.points.append(gps)
# info = json.dumps(GpsPointsToArray(set_gps))
# client.publish('as/wrc/under/map',info, qos=0)
print(set_gps)
#print("end map")
pub_gps.publish(set_gps)
pub_state.publish(str(1))
else:
print("error")
def on_message(client, userdata, msg):
topic = msg.topic
data = json.loads(msg.payload)
cmd = BoatCmd()
print(topic)
print(data)
if topic == 'as/wrc/upper/move':
cmd.cmd = BoatCmd.SPEED
cmd.data_float32 = [
float(data['fbValue']),
float(data['lrValue'] / 10.0)
]
pub_manual_cmd.publish(cmd)
pub_state.publish(str(data['pilotMode']))
elif topic == 'as/wrc/upper/operate':
cmd.cmd = BoatCmd.LIGHT
cmd.data_int = [
int(data['resetLight']),
int(data['leftLight']),
int(data['rightLight']),
int(data['horn'])
]
pub_cmd.publish(cmd)
elif topic == 'as/wrc/upper/map':
start_gps, end_gps = BoatGpsPoint(), BoatGpsPoint()
set_gps = BoatGpsPoints()
start_gps.latitude = float(data['startLat'])
start_gps.longitude = float(data['startLon'])
end_gps.latitude = float(data['endLat'])
end_gps.longitude = float(data['endLon'])
set_gps.points = [start_gps, end_gps]
# info = json.dumps(GpsPointsToArray(set_gps))
# client.publish('as/wrc/under/map',info, qos=0)
print("gps: ", set_gps)
pub_gps.publish(set_gps)
pub_state.publish(str(1))
# flag
index = 0
# avoid
# flag_avoid = 0
# avoid_direction = 0.0
# gps
latitude = 0
longitude = 0
heading = 0
flag_gps = 0
path = []
path_now = None
path_flag = 0
distance_to_current_goal = 0
distance_before = 0
time_now = 0
continue_flag = 1
cmd = Float32()
cmd1 = BoatCmd()
rospy.init_node('navigation', anonymous=True)
rospy.Subscriber("boatgps", BoatGps, gps_callback)
rospy.Subscriber("nodes", BoatGpsPoints, get_nodes)
rospy.Subscriber("hedao_direction", Float32, hedao_callback)
rospy.Subscriber("auto_state", String, callback_state)
pub2 = rospy.Publisher("auto_cmd", BoatCmd, queue_size=1)
pub1 = rospy.Publisher("global_direction", Float32, queue_size=1)
# rospy.Subscriber("avoid_msg", Float32, avoid_callback)
rospy.spin()
break
except Exception:
cmd = BoatCmd()
cmd.cmd = BoatCmd.STOP
cmd.data_float32 = []
cmd.data_int = []
pub_manual_cmd.publish(cmd)
pub_state.publish(str(0))
print("timeout,try again...")
def my_handler(signum, frame):
client.disconnect()
if __name__ == '__main__':
rospy.init_node('net2local_mqtt', anonymous=True)
pub_manual_cmd = rospy.Publisher('manual_cmd', BoatCmd, queue_size=10)
pub_cmd = rospy.Publisher('boatcmd', BoatCmd, queue_size=10)
pub_state = rospy.Publisher('auto_state', String, queue_size=1)
pub_gps = rospy.Publisher('set_gps', BoatGpsPoints, queue_size=2)
cmd = BoatCmd()
client = mqtt.Client()
client.on_connect = on_connect
client.on_message = on_message
client.on_disconnect = on_disconnect
signal.signal(signal.SIGINT, my_handler)
client_connected()