You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 

280 lines
11 KiB

const mqtt = require("mqtt");
const _ = require("lodash")
const proj4 = require("proj4")
module.exports = (agent) => {
agent.messenger.on("egg-ready", () => {
const client = mqtt.connect("mqtt://192.168.1.102:1883", {
// const client = mqtt.connect("mqtt://127.0.0.1:1883", {
clean: true,
connectTimeout: 4000,
clientId: 'map-editor-' + Math.random().toString(16).substr(2),
username: 'map-editor',
password: 'leador',
});
client.on("connect", function () {
console.log("agentmqtt服务器连接成功");
//本系统
client.subscribe(`/robot4inspection/fa49e558574df1ec/sensor/battery`, { qos: 0 }, (a, b, c) => {
}); // 订阅电池电量
client.subscribe(`/robot4inspection/fa49e558574df1ec/sensor/gps`, { qos: 0 }, (a, b, c) => {
}); // 订阅位置信息
client.subscribe(`/robot4inspection/fa49e558574df1ec/localization/pose`, { qos: 0 }, (a, b, c) => {
}); // 订阅位姿信息
client.subscribe(`/robot4inspection/fa49e558574df1ec/message/error`, { qos: 0 }, (a, b, c) => {
}); // 订阅错误消息
client.subscribe(`/robot4inspection/fa49e558574df1ec/message/warn`, { qos: 0 }, (a, b, c) => {
}); // 订阅警告消息
client.subscribe(`/robot4inspection/fa49e558574df1ec/message/info`, { qos: 0 }, (a, b, c) => {
}); // 订阅日志消息
client.subscribe(`/robot4inspection/fa49e558574df1ec/planning/service/plan/request`, { qos: 2 }, (a, b, c) => {
}); // 订阅路径规划
client.subscribe(`/robot4inspection/fa49e558574df1ec/planning/trajectory/2d/compact`, { qos: 2, retain: 0 }, (a, b, c) => {
}); // 订阅当前行驶轨迹
client.subscribe(`/robot4inspection/fa49e558574df1ec/task/target/action/goto`, { qos: 2, retain: 0 }, (a, b, c) => {
}); // 订阅前往目标点
//指控中心
client.subscribe(`/Plan`, { qos: 2 }, (a, b, c) => {
}); // 订阅生成任务
client.subscribe(`/startPlan`, { qos: 0 }, (a, b, c) => {
}); // 订阅开始任务
client.subscribe(`/pausePlan`, { qos: 0 }, (a, b, c) => {
}); // 订阅暂停任务
client.subscribe(`/overPlan`, { qos: 0 }, (a, b, c) => {
}); // 订阅取消任务
client.subscribe(`/delPlan`, { qos: 0 }, (a, b, c) => {
}); // 订阅删除任务
});
let current = {
lon: 0,
lat: 0,
x: 0,
y: 0,
}
const lonlatToxy = function (lon, lat) {
let detx = Number(lon - current.lon) * 100000 + Number(current.x)
let dety = Number(lat - current.lat) * 100000 + Number(current.y)
return [detx, dety]
}
const batteryThrottle = _.throttle(function (message) {
let resObj = JSON.parse(message.toString())
let retMessage = resObj.args[0]
let timestamp = resObj.timestamp
let postMessagr = {
message: {
capacity: retMessage.capacity,
charging: retMessage.charging,
current: retMessage.current,
temperature: retMessage.temperature,
voltage: retMessage.voltage,
},
timestamp,
}
let messStr = JSON.stringify(postMessagr)
// client.publish("/battery", messStr, {
// qos: 1,
// retain: true,
// })
}, 3000, { leading: true })
const temperatureThrottle = _.throttle(function (message) {
let resObj = JSON.parse(message.toString())
let retMessage = resObj.args[0]
let timestamp = resObj.timestamp
let postMessagr = {
message: {
humidity: retMessage.humidity,
env: retMessage.env,
cpu: retMessage.cpu,
},
timestamp,
}
let messStr = JSON.stringify(postMessagr)
// client.publish("/temperature", messStr, {
// qos: 1,
// retain: true,
// })
}, 3000, { leading: true })
const gpsThrottle = _.throttle(function (message) {
let resObj = JSON.parse(message.toString())
let retMessage = resObj.args[0]
let timestamp = resObj.timestamp
let postMessagr = {
message: {
lat: retMessage.blh[0],
lon: retMessage.blh[1],
height: retMessage.blh[2],
heading: retMessage.heading,
speed: retMessage.speed,
star: retMessage.star,
status: retMessage.status,
},
timestamp,
}
let messStr = JSON.stringify(postMessagr)
// client.publish("/location", messStr, {
// qos: 1,
// retain: true,
// })
}, 100, { leading: true })
const poseThrottle = _.throttle(function (message) {
let resObj = JSON.parse(message.toString())
let retMessage = resObj.args[0]
let timestamp = resObj.timestamp
let postMessagr = {
message: {
confidence: retMessage.confidence,
odometer: retMessage.odometer,
poseB: retMessage.pose.blh[0],
poseL: retMessage.pose.blh[1],
poseH: retMessage.pose.blh[2],
poseR: retMessage.pose.rpy[0],
poseP: retMessage.pose.rpy[1],
poseY: retMessage.pose.rpy[2],
posex1: retMessage.pose.xyz[0],
posey1: retMessage.pose.xyz[1],
posez1: retMessage.pose.xyz[2],
heading: retMessage.pose.heading,
replan: retMessage.replan,
star: retMessage.rtk.star,
status: retMessage.rtk.status,
run_state: retMessage.run_state,
score: retMessage.score,
sequence: retMessage.sequence,
enu: retMessage.vel.enu,
velheading: retMessage.vel.heading,
},
timestamp,
}
current = {
lon: retMessage.pose.blh[1],
lat: retMessage.pose.blh[0],
x: retMessage.pose.xyz[0],
y: retMessage.pose.xyz[1],
}
// client.publish("/pose", JSON.stringify(postMessagr), {
// qos: 1,
// retain: true,
// })
}, 100, { leading: true })
const PlanThrottle = _.throttle(function (message) {
let strJson = JSON.parse(message.toString()).data
let taskName = JSON.parse(message.toString()).handlerId
let mapName = JSON.parse(message.toString()).mapName
console.log(6666, strJson, taskName, mapName)
let params = {
map: mapName || "test",
path: taskName || "task1",
repeate: _.get(strJson, ["repeate"], 1) || 1,
coord_type: "local",
cron: _.get(strJson, ["cron"], "") || "",
coord: [],
plan: [],
"action": [
[]
],
}
let coord = []
let plan = []
if (_.get(strJson, ["type"], "xy") === "xy") {
if (strJson.start_lon && strJson.start_lat) {
coord = [[strJson.start_lon, strJson.start_lat]]
plan = ["route"]
}
let pointSort = _.sortBy(strJson.way_points, "id")
for (let index = 0; index < pointSort.length; index++) {
let element = pointSort[index];
coord.push([element.lon, element.lat])
plan.push("route")
}
if (strJson.end_lon && strJson.end_lat) {
coord.push([strJson.end_lon, strJson.end_lat])
plan.push("route")
}
} else if (_.get(strJson, ["type"], "xy") === "lonlat") {
if (strJson.start_lon && strJson.start_lat) {
coord = [lonlatToxy(strJson.start_lon, strJson.start_lat)]
plan = ["route"]
}
let pointSort = _.sortBy(strJson.way_points, "id")
for (let index = 0; index < pointSort.length; index++) {
let element = pointSort[index];
coord.push(lonlatToxy(element.lon, element.lat))
plan.push("route")
}
if (strJson.end_lon && strJson.end_lat) {
coord.push(lonlatToxy(strJson.end_lon, strJson.end_lat))
plan.push("route")
}
}
params.coord = coord
params.plan = plan
agent.messenger.sendToApp('plan_action', params);
}, 3000, { leading: true })
client.on("message", async function (top, message) {
if (_.endsWith(top, "/sensor/battery")) {
batteryThrottle(message)
} else if (_.endsWith(top, "/sensor/temperature")) {
temperatureThrottle(message)
} else if (_.endsWith(top, "/sensor/gps")) {
gpsThrottle(message)
} else if (_.endsWith(top, "/localization/pose")) {
poseThrottle(message)
} else if (_.endsWith(top, "/message/error")) {
let msg = JSON.parse(message.toString());
// client.publish("/errorMessage", JSON.stringify(msg), { qos: 1, })
} else if (_.endsWith(top, "/message/warn")) {
let msg = JSON.parse(message.toString());
// client.publish("/warnMessage", JSON.stringify(msg), { qos: 1, })
} else if (_.endsWith(top, "/message/info")) {
let msg = JSON.parse(message.toString());
// client.publish("/infoMessage", JSON.stringify(msg), { qos: 1, })
} else if (_.endsWith(top, "/Plan")) {
PlanThrottle(message)
} else if (_.endsWith(top, "/startPlan")) {
let strJson = JSON.parse(message.toString()).data
let job = {
args: [
{
roadmap: _.get(strJson, ["mapName"], "TestNJ1") || "TestNJ1",
task: _.get(strJson, ["taskName"], "task1") || "task1",
},
],
timestamp: Date.now()
}
console.log(66661,job,strJson)
// client.publish("/robot4inspection/fa49e558574df1ec/task/procedure/action/start", JSON.stringify(job), { qos: 2, retain: 0, })
} else if (_.endsWith(top, "/pausePlan")) {
let job = {
args: null,
timestamp: Date.now()
}
// client.publish("/robot4inspection/fa49e558574df1ec/task/procedure/action/pause", JSON.stringify(job), { qos: 1, retain: 0, })
} else if (_.endsWith(top, "/overPlan")) {
let job = {
args: null,
timestamp: Date.now()
}
// client.publish("/robot4inspection/fa49e558574df1ec/task/procedure/action/cancel", JSON.stringify(job), { qos: 2, retain: 0 })
} else if (_.endsWith(top, "/delPlan")) {
let strJson = JSON.parse(message.toString()).data
let params = {
map: _.get(strJson, ["mapName"], "TestNJ1") || "TestNJ1",
path: _.get(strJson, ["taskName"], "task1") || "task1",
}
agent.messenger.sendToApp('plan_delete', params);
} else if (_.endsWith(top, "/planning/service/plan/request")) {
let msg = JSON.parse(message.toString());
} else if (_.endsWith(top, "/trajectory/2d/compact")) {
let msg = JSON.parse(message.toString());
} else if (_.endsWith(top, "/task/target/action/goto")) {
let msg = JSON.parse(message.toString());
}
});
});
};