commit

trigger/pji/diagnostics/errorcode#1#21031701/main/main.go

@@ -1,25 +1,68 @@
 package main
 
 import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
-	"math"
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/diagnostic_msgs"
+	"strconv"
 )
 
 /*
 输入电源欠压
 */
 
+var (
+	errorType       = "1"
+	errorCode       = "21031701"
+	code12          = "21"
+	hardwareIdIndex = map[string]string{
+		"batter":        "00",
+		"cloud":         "01",
+		"gateway":       "02",
+		"mcu":           "03",
+		"imu":           "04",
+		"odom":          "05",
+		"motor":         "06",
+		"preventFall":   "07",
+		"sonar":         "08",
+		"locate":        "09",
+		"laserLadar":    "10",
+		"electricDrive": "11",
+		"infrared":      "12",
+		"livoxLidar":    "13",
+		"camera":        "14",
+		"navigation":    "15",
+		"system":        "16",
+	}
+)
+
 func Topic() string {
 	return "/diagnostics"
 }
 
 func Label() string {
-	return "errorcode#1#21031701"
+	return "errorcode#" + errorType + "#" + errorCode
 }
 
-func Rule(data *sensor_msgs.Imu) string {
-	if math.Abs(data.LinearAcceleration.X) >= 0.7 && math.Abs(data.LinearAcceleration.Y) >= 0.7 {
-		return "unstabledriving"
+func Rule(data *diagnostic_msgs.DiagnosticArray) string {
+	if len(data.Status) == 0 {
+		return ""
+	}
+	for _, status := range data.Status {
+		if status.Level != 5 || len(status.Values) == 0 {
+			continue
+		}
+
+		code34 := fmt.Sprintf("%02d", status.Level)
+		code56 := hardwareIdIndex[status.HardwareId]
+		intVal, err := strconv.Atoi(status.Values[1].Value)
+		if err != nil {
+			fmt.Println("字段 【status.Values[1].Value】转整数报错：", err)
+			return ""
+		}
+		code78 := fmt.Sprintf("%02d", intVal)
+		if errorCode == code12+code34+code56+code78 {
+			return Label()
+		}
 	}
 	return ""
 }

trigger/pji/diagnostics/errorcode#1#21031702/main/main.go

@@ -1,8 +1,38 @@
 package main
 
 import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
-	"math"
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/diagnostic_msgs"
+	"strconv"
+)
+
+/*
+输入电源过压
+*/
+
+var (
+	errorType       = "1"
+	errorCode       = "21031702"
+	code12          = "21"
+	hardwareIdIndex = map[string]string{
+		"batter":        "00",
+		"cloud":         "01",
+		"gateway":       "02",
+		"mcu":           "03",
+		"imu":           "04",
+		"odom":          "05",
+		"motor":         "06",
+		"preventFall":   "07",
+		"sonar":         "08",
+		"locate":        "09",
+		"laserLadar":    "10",
+		"electricDrive": "11",
+		"infrared":      "12",
+		"livoxLidar":    "13",
+		"camera":        "14",
+		"navigation":    "15",
+		"system":        "16",
+	}
 )
 
 func Topic() string {
@@ -10,12 +40,29 @@ func Topic() string {
 }
 
 func Label() string {
-	return "errorcode#1#21031702"
+	return "errorcode#" + errorType + "#" + errorCode
 }
 
-func Rule(data *sensor_msgs.Imu) string {
-	if math.Abs(data.LinearAcceleration.X) >= 0.7 && math.Abs(data.LinearAcceleration.Y) >= 0.7 {
-		return "unstabledriving"
+func Rule(data *diagnostic_msgs.DiagnosticArray) string {
+	if len(data.Status) == 0 {
+		return ""
+	}
+	for _, status := range data.Status {
+		if status.Level != 5 || len(status.Values) == 0 {
+			continue
+		}
+
+		code34 := fmt.Sprintf("%02d", status.Level)
+		code56 := hardwareIdIndex[status.HardwareId]
+		intVal, err := strconv.Atoi(status.Values[1].Value)
+		if err != nil {
+			fmt.Println("字段 【status.Values[1].Value】转整数报错：", err)
+			return ""
+		}
+		code78 := fmt.Sprintf("%02d", intVal)
+		if errorCode == code12+code34+code56+code78 {
+			return Label()
+		}
 	}
 	return ""
 }

trigger/pji/diagnostics/errorcode#2#21020001/main/main.go

@@ -1,8 +1,38 @@
 package main
 
 import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
-	"math"
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/diagnostic_msgs"
+	"strconv"
+)
+
+/*
+电池数据通讯轻微丢失
+*/
+
+var (
+	errorType       = "2"
+	errorCode       = "21020001"
+	code12          = "21"
+	hardwareIdIndex = map[string]string{
+		"batter":        "00",
+		"cloud":         "01",
+		"gateway":       "02",
+		"mcu":           "03",
+		"imu":           "04",
+		"odom":          "05",
+		"motor":         "06",
+		"preventFall":   "07",
+		"sonar":         "08",
+		"locate":        "09",
+		"laserLadar":    "10",
+		"electricDrive": "11",
+		"infrared":      "12",
+		"livoxLidar":    "13",
+		"camera":        "14",
+		"navigation":    "15",
+		"system":        "16",
+	}
 )
 
 func Topic() string {
@@ -10,12 +40,29 @@ func Topic() string {
 }
 
 func Label() string {
-	return "errorcode#1#21031702"
+	return "errorcode#" + errorType + "#" + errorCode
 }
 
-func Rule(data *sensor_msgs.Imu) string {
-	if math.Abs(data.LinearAcceleration.X) >= 0.7 && math.Abs(data.LinearAcceleration.Y) >= 0.7 {
-		return "unstabledriving"
+func Rule(data *diagnostic_msgs.DiagnosticArray) string {
+	if len(data.Status) == 0 {
+		return ""
+	}
+	for _, status := range data.Status {
+		if status.Level != 5 || len(status.Values) == 0 {
+			continue
+		}
+
+		code34 := fmt.Sprintf("%02d", status.Level)
+		code56 := hardwareIdIndex[status.HardwareId]
+		intVal, err := strconv.Atoi(status.Values[1].Value)
+		if err != nil {
+			fmt.Println("字段 【status.Values[1].Value】转整数报错：", err)
+			return ""
+		}
+		code78 := fmt.Sprintf("%02d", intVal)
+		if errorCode == code12+code34+code56+code78 {
+			return Label()
+		}
 	}
 	return ""
 }

trigger/pji/diagnostics/errorcode#2#21020401/main/main.go

@@ -1,8 +1,38 @@
 package main
 
 import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
-	"math"
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/diagnostic_msgs"
+	"strconv"
+)
+
+/*
+imu通讯轻微丢失
+*/
+
+var (
+	errorType       = "2"
+	errorCode       = "21020401"
+	code12          = "21"
+	hardwareIdIndex = map[string]string{
+		"batter":        "00",
+		"cloud":         "01",
+		"gateway":       "02",
+		"mcu":           "03",
+		"imu":           "04",
+		"odom":          "05",
+		"motor":         "06",
+		"preventFall":   "07",
+		"sonar":         "08",
+		"locate":        "09",
+		"laserLadar":    "10",
+		"electricDrive": "11",
+		"infrared":      "12",
+		"livoxLidar":    "13",
+		"camera":        "14",
+		"navigation":    "15",
+		"system":        "16",
+	}
 )
 
 func Topic() string {
@@ -10,12 +40,29 @@ func Topic() string {
 }
 
 func Label() string {
-	return "errorcode#1#21031702"
+	return "errorcode#" + errorType + "#" + errorCode
 }
 
-func Rule(data *sensor_msgs.Imu) string {
-	if math.Abs(data.LinearAcceleration.X) >= 0.7 && math.Abs(data.LinearAcceleration.Y) >= 0.7 {
-		return "unstabledriving"
+func Rule(data *diagnostic_msgs.DiagnosticArray) string {
+	if len(data.Status) == 0 {
+		return ""
+	}
+	for _, status := range data.Status {
+		if status.Level != 5 || len(status.Values) == 0 {
+			continue
+		}
+
+		code34 := fmt.Sprintf("%02d", status.Level)
+		code56 := hardwareIdIndex[status.HardwareId]
+		intVal, err := strconv.Atoi(status.Values[1].Value)
+		if err != nil {
+			fmt.Println("字段 【status.Values[1].Value】转整数报错：", err)
+			return ""
+		}
+		code78 := fmt.Sprintf("%02d", intVal)
+		if errorCode == code12+code34+code56+code78 {
+			return Label()
+		}
 	}
 	return ""
 }

trigger/pji/diagnostics/errorcode#2#21020501/main/main.go

@@ -1,8 +1,38 @@
 package main
 
 import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
-	"math"
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/diagnostic_msgs"
+	"strconv"
+)
+
+/*
+odom通讯轻微丢失
+*/
+
+var (
+	errorType       = "2"
+	errorCode       = "21020501"
+	code12          = "21"
+	hardwareIdIndex = map[string]string{
+		"batter":        "00",
+		"cloud":         "01",
+		"gateway":       "02",
+		"mcu":           "03",
+		"imu":           "04",
+		"odom":          "05",
+		"motor":         "06",
+		"preventFall":   "07",
+		"sonar":         "08",
+		"locate":        "09",
+		"laserLadar":    "10",
+		"electricDrive": "11",
+		"infrared":      "12",
+		"livoxLidar":    "13",
+		"camera":        "14",
+		"navigation":    "15",
+		"system":        "16",
+	}
 )
 
 func Topic() string {
@@ -10,12 +40,29 @@ func Topic() string {
 }
 
 func Label() string {
-	return "errorcode#1#21031702"
+	return "errorcode#" + errorType + "#" + errorCode
 }
 
-func Rule(data *sensor_msgs.Imu) string {
-	if math.Abs(data.LinearAcceleration.X) >= 0.7 && math.Abs(data.LinearAcceleration.Y) >= 0.7 {
-		return "unstabledriving"
+func Rule(data *diagnostic_msgs.DiagnosticArray) string {
+	if len(data.Status) == 0 {
+		return ""
+	}
+	for _, status := range data.Status {
+		if status.Level != 5 || len(status.Values) == 0 {
+			continue
+		}
+
+		code34 := fmt.Sprintf("%02d", status.Level)
+		code56 := hardwareIdIndex[status.HardwareId]
+		intVal, err := strconv.Atoi(status.Values[1].Value)
+		if err != nil {
+			fmt.Println("字段 【status.Values[1].Value】转整数报错：", err)
+			return ""
+		}
+		code78 := fmt.Sprintf("%02d", intVal)
+		if errorCode == code12+code34+code56+code78 {
+			return Label()
+		}
 	}
 	return ""
 }

trigger/pji/diagnostics/errorcode#2#21020601/main/main.go

@@ -1,8 +1,38 @@
 package main
 
 import (
-	"github.com/bluenviron/goroslib/v2/pkg/msgs/sensor_msgs"
-	"math"
+	"fmt"
+	"github.com/bluenviron/goroslib/v2/pkg/msgs/diagnostic_msgs"
+	"strconv"
+)
+
+/*
+motor通讯轻微丢失
+*/
+
+var (
+	errorType       = "2"
+	errorCode       = "21020601"
+	code12          = "21"
+	hardwareIdIndex = map[string]string{
+		"batter":        "00",
+		"cloud":         "01",
+		"gateway":       "02",
+		"mcu":           "03",
+		"imu":           "04",
+		"odom":          "05",
+		"motor":         "06",
+		"preventFall":   "07",
+		"sonar":         "08",
+		"locate":        "09",
+		"laserLadar":    "10",
+		"electricDrive": "11",
+		"infrared":      "12",
+		"livoxLidar":    "13",
+		"camera":        "14",
+		"navigation":    "15",
+		"system":        "16",
+	}
 )
 
 func Topic() string {
@@ -10,12 +40,29 @@ func Topic() string {
 }
 
 func Label() string {
-	return "errorcode#1#21031702"
+	return "errorcode#" + errorType + "#" + errorCode
 }
 
-func Rule(data *sensor_msgs.Imu) string {
-	if math.Abs(data.LinearAcceleration.X) >= 0.7 && math.Abs(data.LinearAcceleration.Y) >= 0.7 {
-		return "unstabledriving"
+func Rule(data *diagnostic_msgs.DiagnosticArray) string {
+	if len(data.Status) == 0 {
+		return ""
+	}
+	for _, status := range data.Status {
+		if status.Level != 5 || len(status.Values) == 0 {
+			continue
+		}
+
+		code34 := fmt.Sprintf("%02d", status.Level)
+		code56 := hardwareIdIndex[status.HardwareId]
+		intVal, err := strconv.Atoi(status.Values[1].Value)
+		if err != nil {
+			fmt.Println("字段 【status.Values[1].Value】转整数报错：", err)
+			return ""
+		}
+		code78 := fmt.Sprintf("%02d", intVal)
+		if errorCode == code12+code34+code56+code78 {
+			return Label()
+		}
 	}
 	return ""
 }