Add endpoint info methods

.golangci.yaml

@@ -41,6 +41,7 @@ linters:
     - nonamedreturns
     - nosnakecase
     - paralleltest
+    - tagliatelle
     - testableexamples
     - testpackage
     - varnamelen

pkg/rclgo/graph.go

@@ -6,7 +6,9 @@ package rclgo
 */
 import "C"
 
-import "unsafe"
+import (
+	"unsafe"
+)
 
 // GetTopicNamesAndTypes returns a map of all known topic names to corresponding
 // topic types. Note that multiple types may be associated with a single topic.
@@ -154,3 +156,66 @@ func (n *Node) getNamesAndTypes(
 	}
 	return namesAndTypes, nil
 }
+
+const GIDSize = 24
+
+type GID [GIDSize]byte
+
+type EndpointType int
+
+const (
+	EndpointInvalid EndpointType = iota
+	EndpointPublisher
+	EndpointSubscription
+)
+
+type TopicEndpointInfo struct {
+	NodeName      string
+	NodeNamespace string
+	TopicType     string
+	EndpointType  EndpointType
+	EndpointGID   GID
+	QosProfile    QosProfile
+}
+
+func (n *Node) GetPublishersInfoByTopic(topic string, mangle bool) ([]TopicEndpointInfo, error) {
+	return n.getInfoByTopic("publishers", topic, mangle, func(topic *C.char, noMangle C.bool, infoArray *C.rmw_topic_endpoint_info_array_t) C.int {
+		return C.rcl_get_publishers_info_by_topic(n.rcl_node_t, n.context.rcl_allocator_t, topic, noMangle, infoArray)
+	})
+}
+
+func (n *Node) GetSubscriptionsInfoByTopic(topic string, mangle bool) ([]TopicEndpointInfo, error) {
+	return n.getInfoByTopic("subscriptions", topic, mangle, func(topic *C.char, noMangle C.bool, infoArray *C.rmw_topic_endpoint_info_array_t) C.int {
+		return C.rcl_get_subscriptions_info_by_topic(n.rcl_node_t, n.context.rcl_allocator_t, topic, noMangle, infoArray)
+	})
+}
+
+func (n *Node) getInfoByTopic(kind, topic string, mangle bool, get func(
+	topic *C.char,
+	noMangle C.bool,
+	infoArray *C.rmw_topic_endpoint_info_array_t,
+) C.int) ([]TopicEndpointInfo, error) {
+	ctopic := C.CString(topic)
+	defer C.free(unsafe.Pointer(ctopic))
+	infoArray := C.rmw_get_zero_initialized_topic_endpoint_info_array()
+	defer C.rmw_topic_endpoint_info_array_fini(&infoArray, n.context.rcl_allocator_t)
+	rc := get(ctopic, !C.bool(mangle), &infoArray)
+	if rc != C.RCL_RET_OK {
+		return nil, errorsCastC(rc, "failed to get "+kind+" info by topic")
+	}
+	infoSlice := unsafe.Slice(infoArray.info_array, infoArray.size)
+	infos := make([]TopicEndpointInfo, len(infoSlice))
+	for i, info := range infoSlice {
+		infos[i] = TopicEndpointInfo{
+			NodeName:      C.GoString(info.node_name),
+			NodeNamespace: C.GoString(info.node_namespace),
+			TopicType:     C.GoString(info.topic_type),
+			EndpointType:  EndpointType(info.endpoint_type),
+		}
+		for j := range info.endpoint_gid {
+			infos[i].EndpointGID[j] = byte(info.endpoint_gid[j])
+		}
+		infos[i].QosProfile.fromCStruct(&info.qos_profile)
+	}
+	return infos, nil
+}

pkg/rclgo/qos.go

@@ -64,15 +64,15 @@ const (
 const LivelinessLeaseDurationDefault = DurationUnspecified
 
 type QosProfile struct {
-	History                      HistoryPolicy
-	Depth                        int
-	Reliability                  ReliabilityPolicy
-	Durability                   DurabilityPolicy
-	Deadline                     time.Duration
-	Lifespan                     time.Duration
-	Liveliness                   LivelinessPolicy
-	LivelinessLeaseDuration      time.Duration
-	AvoidRosNamespaceConventions bool
+	History                      HistoryPolicy     `yaml:"history"`
+	Depth                        int               `yaml:"depth"`
+	Reliability                  ReliabilityPolicy `yaml:"reliability"`
+	Durability                   DurabilityPolicy  `yaml:"durability"`
+	Deadline                     time.Duration     `yaml:"deadline"`
+	Lifespan                     time.Duration     `yaml:"lifespan"`
+	Liveliness                   LivelinessPolicy  `yaml:"liveliness"`
+	LivelinessLeaseDuration      time.Duration     `yaml:"liveliness_lease_duration"`
+	AvoidRosNamespaceConventions bool              `yaml:"avoid_ros_namespace_conventions"`
 }
 
 func NewDefaultQosProfile() QosProfile {
@@ -104,3 +104,15 @@ func (p *QosProfile) asCStruct(dst *C.rmw_qos_profile_t) {
 	dst.liveliness_lease_duration = C.rmw_time_t{nsec: C.ulong(p.LivelinessLeaseDuration)}
 	dst.avoid_ros_namespace_conventions = C.bool(p.AvoidRosNamespaceConventions)
 }
+
+func (p *QosProfile) fromCStruct(src *C.rmw_qos_profile_t) {
+	p.History = HistoryPolicy(src.history)
+	p.Depth = int(src.depth)
+	p.Reliability = ReliabilityPolicy(src.reliability)
+	p.Durability = DurabilityPolicy(src.durability)
+	p.Deadline = time.Duration(src.deadline.sec)*time.Second + time.Duration(src.deadline.nsec)
+	p.Lifespan = time.Duration(src.lifespan.sec)*time.Second + time.Duration(src.lifespan.nsec)
+	p.Liveliness = LivelinessPolicy(src.liveliness)
+	p.LivelinessLeaseDuration = time.Duration(src.liveliness_lease_duration.sec)*time.Second + time.Duration(src.liveliness_lease_duration.nsec)
+	p.AvoidRosNamespaceConventions = bool(src.avoid_ros_namespace_conventions)
+}