MAVROS状态实时显示在阿里云物联网平台代码

在这里插入代码片`#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将订阅/turtle1/pose话题，消息类型turtlesim::Pose

import rospy
import datetime
import time
import hmac
import hashlib
import math
from turtlesim.msg import Pose
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import NavSatFix, BatteryState

try:
    import paho.mqtt.client as mqtt
except ImportError:
    print("MQTT client not find. Please install as follow:")
    print("pip install paho-mqtt")

# 设置连接信息
ProductKey = "********"  # 换成自己的
ClientId = "***********"  # 自定义clientId
DeviceName = "***********"  # 换成自己的
DeviceSecret = "************"  # 换成自己的
# 获取时间戳（当前时间毫秒值）
us = math.modf(time.time())[0]
ms = int(round(us * 1000))
timestamp = str(ms)


# 计算密码（签名值）
def calculation_sign(signmethod):
    data = "".join(("clientId", ClientId, "deviceName", DeviceName,
                    "productKey", ProductKey, "timestamp", timestamp))

    if "hmacsha1" == signmethod:
        ret = hmac.new(bytes(DeviceSecret),
                       bytes(data), hashlib.sha1).hexdigest()
    elif "hmacmd5" == signmethod:
        ret = hmac.new(bytes(DeviceSecret, encoding="utf-8"),
                       bytes(data, encoding="utf-8"), hashlib.md5).hexdigest()
    else:
        raise ValueError
    return ret


# ======================================================

strBroker = ProductKey + ".iot-as-mqtt.cn-shanghai.aliyuncs.com"
port = 1883

client_id = "".join((ClientId,
                     "|securemode=3",
                     ",signmethod=", "hmacsha1",
                     ",timestamp=", timestamp,
                     "|"))
username = "".join((DeviceName, "&", ProductKey))
password = calculation_sign("hmacsha1")
topic = "/sys/a1wly357T2l/python2/thing/event/property/post"

# -------------------------
#
i = 0
j = 0
k = 0
timeSpan1 = 8
timeSpan2 = 9
timeSpan3 = 10


# -------------------------

# 成功连接后的操作
def on_connect(mqttc, userdata, flags, rc):
    # rospy.loginfo("dh pose: x:%0.6f, y:%0.6f", pose_x, pose_y)
    # time.sleep(2)
    # pose_subscriber()
    print("OnConnect, rc: " + str(rc))
    send_mseg = '{"params":{"state:batpt": %f,   "state:local_x": %f,   "state:local_y":3, "state:local_z":6}}' % (
        0, 0)  # must json
    # print send_mseg
    mqttc.publish(topic, send_mseg, 1)  # 换成自己的
    print 'connect'


# 成功发布消息的操作
def on_publish(mqttc, msg, rc):
    if rc == 0:
        print("publish success, msg = " + msg)
    print 'publish'


# 成功订阅消息的操作
def on_subscribe(mqttc, obj, mid, granted_qos):
    print("Subscribed: " + str(mid) + " " + str(granted_qos))


def on_log(mqttc, obj, level, string):
    print("Log:" + string)


def on_message(mqttc, obj, msg):
    curtime = datetime.datetime.now()
    strcurtime = curtime.strftime("%Y-%m-%d %H:%M:%S")
    print(strcurtime + ": " + msg.topic + " " + str(msg.qos) + " " + str(msg.payload))
    on_exec(str(msg.payload))


def on_exec(strcmd):
    print("Exec:", strcmd)
    strExec = strcmd


# pose
def poseCallback(msg):
    time.sleep(1)
    global i, timeSpan1
    i = i + 1
    if i == timeSpan1:
        i = 0
        send_muskeg = '{"params":{"state:local_x": %f,   "state:local_y": %f, "state:local_z": ' \
                      '%f}}' % (msg.pose.position.x, msg.pose.position.y, msg.pose.position.z)  # must json
        toAliYun(send_muskeg)
        rospy.loginfo("UAV pose: x:%0.6f, y:%0.6f, z:%0.6f", msg.pose.position.x, msg.pose.position.y,
                      msg.pose.position.z)


# gps
def GpsCallback(msg):
    time.sleep(1)
    global j, timeSpan2
    j = j + 1
    if j == timeSpan2:
        j = 0
        send_muskeg = '{"params":{"state:Altitude": %f,   "state:Longitude": %f, ' \
                      '"state:Latitude": ' \
                      '%f}}' % (msg.altitude, msg.longitude, msg.latitude)  # must json
        toAliYun(send_muskeg)
        rospy.loginfo("UAV gps: Altitude:%0.6f, Longitude:%0.6f Latitude:%0.6f", msg.altitude, msg.latitude,
                      msg.longitude)


# battery
def BatteryCallback(msg):
    time.sleep(1)
    global k, timeSpan3
    k = k + 1
    if k == timeSpan3:
        k = 0
        send_muskeg = '{"params":{"state:batpt": %f,}}' % msg.voltage  # must json
        toAliYun(send_muskeg)
        rospy.loginfo("UAV battery: bpt:%0.6f", msg.voltage)


# ----------------mqtt-to-阿里云---------------
def toAliYun(send_muskeg):
    print send_muskeg
    mqttc.publish(topic, send_muskeg, 1)  # 换成自己的


if __name__ == '__main__':
    mqttc = mqtt.Client(client_id)
    mqttc.username_pw_set(username, password)
    mqttc.on_message = on_message
    mqttc.on_connect = on_connect
    mqttc.on_publish = on_publish
    mqttc.on_subscribe = on_subscribe
    # mqttc.on_log = on_log
    mqttc.connect(strBroker, port, 120)
    rospy.init_node('mavros_subscriber_gps', anonymous=True)
    # 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    rospy.Subscriber("/mavros/local_position/pose", PoseStamped, poseCallback)
    rospy.Subscriber("/mavros/global_position/global", NavSatFix, GpsCallback)
    rospy.Subscriber("/mavros/battery", BatteryState, BatteryCallback)
    mqttc.loop_forever()
`