main.go 2.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869
  1. package main
  2. import (
  3. "cicv-data-closedloop/common/util"
  4. masterConfig "cicv-data-closedloop/kinglong/master/pkg/cfg"
  5. "cicv-data-closedloop/kinglong_msgs"
  6. "fmt"
  7. "github.com/bluenviron/goroslib/v2"
  8. "os"
  9. )
  10. var (
  11. TopicOfNodeFaultInfo = "/nodefault_info"
  12. TopicOfCicvLocation = "/cicv_location"
  13. TopicOfTpperception = "/tpperception"
  14. TopicOfFaultInfo = "/fault_info"
  15. TopicOfDataRead = "/data_read"
  16. )
  17. func main() {
  18. rosNode, _ := goroslib.NewNode(goroslib.NodeConf{
  19. Name: "cicvDataClosedloopTopicEcho",
  20. MasterAddress: "127.0.0.1:11311",
  21. })
  22. topic := util.ToString(os.Args[1])
  23. if topic == TopicOfNodeFaultInfo {
  24. _, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
  25. Node: rosNode,
  26. Topic: masterConfig.TopicOfNodeFaultInfo,
  27. Callback: func(data *kinglong_msgs.FaultInfo) {
  28. fmt.Println("收到话题", topic, "的数据", data)
  29. }})
  30. }
  31. if topic == TopicOfCicvLocation {
  32. _, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
  33. Node: rosNode,
  34. Topic: masterConfig.TopicOfCicvLocation,
  35. Callback: func(data *kinglong_msgs.PerceptionLocalization) {
  36. fmt.Println("收到话题", topic, "的数据", data)
  37. }})
  38. }
  39. if topic == TopicOfTpperception {
  40. _, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
  41. Node: rosNode,
  42. Topic: masterConfig.TopicOfTpperception,
  43. Callback: func(data *kinglong_msgs.PerceptionObjects) {
  44. fmt.Println("收到话题", topic, "的数据", data)
  45. }})
  46. }
  47. if topic == TopicOfFaultInfo {
  48. _, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
  49. Node: rosNode,
  50. Topic: masterConfig.TopicOfFaultInfo,
  51. Callback: func(data *kinglong_msgs.FaultVec) {
  52. fmt.Println("收到话题", topic, "的数据", data)
  53. }})
  54. }
  55. if topic == TopicOfDataRead {
  56. _, _ = goroslib.NewSubscriber(goroslib.SubscriberConf{
  57. Node: rosNode,
  58. Topic: masterConfig.TopicOfDataRead,
  59. Callback: func(data *kinglong_msgs.Retrieval) {
  60. fmt.Println("收到话题", topic, "的数据", data)
  61. }})
  62. }
  63. select {}
  64. }