From 3b382ac98de843f2df6be695783615eb7bc6245e Mon Sep 17 00:00:00 2001 From: Serhii Matsyshyn Date: Wed, 12 Aug 2026 17:37:07 +0300 Subject: [PATCH] route targeted messages to all matching channels --- pkg/messageman/manager.go | 57 +++-- pkg/messageman/manager_test.go | 375 +++++++++++++++++++++++++++++++++ 2 files changed, 418 insertions(+), 14 deletions(-) diff --git a/pkg/messageman/manager.go b/pkg/messageman/manager.go index 7f74935..b7e58c1 100644 --- a/pkg/messageman/manager.go +++ b/pkg/messageman/manager.go @@ -90,22 +90,44 @@ func (m *Manager) run() { } } -func (m *Manager) findNodeBySystemID(systemID byte) *remoteNodeKey { +// appendChannel adds ch unless already present. +func appendChannel(channels []*gomavlib.Channel, ch *gomavlib.Channel) []*gomavlib.Channel { + for _, existing := range channels { + if existing == ch { + return channels + } + } + return append(channels, ch) +} + +// findChannelsBySystemID returns all channels of a given system. +func (m *Manager) findChannelsBySystemID(systemID byte) []*gomavlib.Channel { + // lock: the cleanup routine deletes from remoteNodes concurrently + m.remoteNodeMutex.Lock() + defer m.remoteNodeMutex.Unlock() + + var channels []*gomavlib.Channel for key := range m.remoteNodes { if key.systemID == systemID { - return &key + channels = appendChannel(channels, key.channel) } } - return nil + return channels } -func (m *Manager) findNodeBySystemAndComponentID(systemID byte, componentID byte) *remoteNodeKey { +// findChannelsBySystemAndComponentID returns all channels of a given system and component. +func (m *Manager) findChannelsBySystemAndComponentID(systemID byte, componentID byte) []*gomavlib.Channel { + // lock: the cleanup routine deletes from remoteNodes concurrently + m.remoteNodeMutex.Lock() + defer m.remoteNodeMutex.Unlock() + + var channels []*gomavlib.Channel for key := range m.remoteNodes { if key.systemID == systemID && key.componentID == componentID { - return &key + channels = appendChannel(channels, key.channel) } } - return nil + return channels } // ProcessFrame processes a EventFrame. @@ -137,20 +159,27 @@ func (m *Manager) ProcessFrame(evt *gomavlib.EventFrame) { // if message has a target, route only to it systemID, componentID, hasTarget := getTarget(evt.Message()) if hasTarget && systemID > 0 { - var key *remoteNodeKey + var channels []*gomavlib.Channel if componentID == 0 { - key = m.findNodeBySystemID(systemID) + channels = m.findChannelsBySystemID(systemID) } else { - key = m.findNodeBySystemAndComponentID(systemID, componentID) + channels = m.findChannelsBySystemAndComponentID(systemID, componentID) } - if key != nil { - if key.channel == evt.Channel { - log.Printf("Warning: channel %s attempted to send message to itself, discarding", key.channel) - } else { - m.Node.WriteFrameTo(key.channel, evt.Frame) //nolint:errcheck + if len(channels) != 0 { + // a target can be present on multiple channels; route to all of them + delivered := false + for _, channel := range channels { + if channel == evt.Channel { + continue + } + m.Node.WriteFrameTo(channel, evt.Frame) //nolint:errcheck + delivered = true + } + if delivered { return } + log.Printf("Warning: channel %s attempted to send message to itself, discarding", evt.Channel) } else { log.Printf( "Warning: received message addressed to unexistent node with systemID=%d and componentID=%d", diff --git a/pkg/messageman/manager_test.go b/pkg/messageman/manager_test.go index fde93d7..8f6a5ee 100644 --- a/pkg/messageman/manager_test.go +++ b/pkg/messageman/manager_test.go @@ -4,6 +4,7 @@ import ( "context" "sync" "testing" + "time" "github.com/bluenviron/gomavlib/v4" "github.com/bluenviron/gomavlib/v4/pkg/dialects/ardupilotmega" @@ -14,6 +15,101 @@ import ( "github.com/bluenviron/mavp2p/pkg/messageman" ) +// routedMessageID is the ID of MessageOsdParamConfig, used to tell routed +// frames apart from heartbeats. Payload fidelity is covered by TestRouteSingle. +const routedMessageID = 11033 + +// openChannels waits for count channels to open on n. +func openChannels(t *testing.T, n *gomavlib.Node, count int) []*gomavlib.Channel { + t.Helper() + + var channels []*gomavlib.Channel + timeout := time.After(5 * time.Second) + + for len(channels) < count { + select { + case evt := <-n.Events(): + if op, ok := evt.(*gomavlib.EventChannelOpen); ok { + channels = append(channels, op.Channel) + } + + case <-timeout: + t.Fatalf("timed out waiting for %d channels", count) + } + } + + return channels +} + +// requireRouted asserts that n receives the routed message. +func requireRouted(t *testing.T, n *gomavlib.Node) { + t.Helper() + + timeout := time.After(5 * time.Second) + + for { + select { + case evt := <-n.Events(): + if fr, ok := evt.(*gomavlib.EventFrame); ok { + if msg, ok2 := fr.Frame.GetMessage().(*message.MessageRaw); ok2 && msg.ID == routedMessageID { + return + } + } + + case <-timeout: + t.Fatal("timed out waiting for routed message") + } + } +} + +// targetedFrame returns a frame addressed to system 99, component targetComponent. +// +// FixFrame encodes the message into a MessageRaw, which would hide the +// TargetSystem/TargetComponent fields that getTarget reads by reflection and +// send the frame down the broadcast path instead. The typed message is put back +// afterwards, which leaves the checksum valid and the frame in the same shape as +// one decoded from an endpoint. +func targetedFrame(t *testing.T, n *gomavlib.Node, targetComponent byte) *frame.V2Frame { + t.Helper() + + msg := &ardupilotmega.MessageOsdParamConfig{ + TargetSystem: 99, + TargetComponent: targetComponent, + } + + fr := &frame.V2Frame{ + SequenceNumber: 127, + SystemID: 30, + ComponentID: 17, + Message: msg, + } + err := n.FixFrame(fr) + require.NoError(t, err) + fr.Message = msg + + return fr +} + +// announce registers a node on a channel. +func announce(m *messageman.Manager, n *gomavlib.Node, ch *gomavlib.Channel, systemID byte, componentID byte) error { + fr := &frame.V2Frame{ + SequenceNumber: 1, + SystemID: systemID, + ComponentID: componentID, + Message: &ardupilotmega.MessageHeartbeat{}, + } + if err := n.FixFrame(fr); err != nil { + return err + } + + m.ProcessFrame(&gomavlib.EventFrame{ + Frame: fr, + Channel: ch, + }) + + return nil +} + func TestRouteSingle(t *testing.T) { node := &gomavlib.Node{ Endpoints: []gomavlib.Endpoint{ @@ -178,3 +274,282 @@ func TestRouteAll(t *testing.T) { cancel() wg.Wait() } + +// countRouted counts the copies of the routed message that n receives within window. +func countRouted(t *testing.T, n *gomavlib.Node, window time.Duration) int { + t.Helper() + + count := 0 + deadline := time.After(window) + + for { + select { + case evt := <-n.Events(): + if fr, ok := evt.(*gomavlib.EventFrame); ok { + if msg, ok2 := fr.Frame.GetMessage().(*message.MessageRaw); ok2 && msg.ID == routedMessageID { + count++ + } + } + + case <-deadline: + return count + } + } +} + +func TestRouteMultipleChannels(t *testing.T) { + node := &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPServer{ + Address: "127.0.0.1:3346", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 22, + OutComponentID: 13, + Dialect: ardupilotmega.Dialect, + } + err := node.Initialize() + require.NoError(t, err) + defer node.Close() + + ctx, cancel := context.WithCancel(context.Background()) + var wg sync.WaitGroup + + m := &messageman.Manager{ + Ctx: ctx, + Wg: &wg, + StreamReqDisable: true, + Node: node, + } + err = m.Initialize() + require.NoError(t, err) + + clients := make([]*gomavlib.Node, 2) + for i := range clients { + clients[i] = &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPClient{ + Address: "127.0.0.1:3346", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 99, + OutComponentID: 34, + } + err = clients[i].Initialize() + require.NoError(t, err) + defer clients[i].Close() //nolint:gocritic + } + + channels := openChannels(t, node, 2) + + // both channels claim the same system and component + for _, ch := range channels { + err = announce(m, node, ch, 99, 34) + require.NoError(t, err) + } + + m.ProcessFrame(&gomavlib.EventFrame{ + Frame: targetedFrame(t, node, 34), + }) + + for _, client := range clients { + requireRouted(t, client) + } + + cancel() + wg.Wait() +} + +func TestRouteMultipleChannelsComponentZero(t *testing.T) { + node := &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPServer{ + Address: "127.0.0.1:3347", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 22, + OutComponentID: 13, + Dialect: ardupilotmega.Dialect, + } + err := node.Initialize() + require.NoError(t, err) + defer node.Close() + + ctx, cancel := context.WithCancel(context.Background()) + var wg sync.WaitGroup + + m := &messageman.Manager{ + Ctx: ctx, + Wg: &wg, + StreamReqDisable: true, + Node: node, + } + err = m.Initialize() + require.NoError(t, err) + + clients := make([]*gomavlib.Node, 2) + for i := range clients { + clients[i] = &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPClient{ + Address: "127.0.0.1:3347", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 99, + OutComponentID: byte(34 + i), + } + err = clients[i].Initialize() + require.NoError(t, err) + defer clients[i].Close() //nolint:gocritic + } + + channels := openChannels(t, node, 2) + + // same system, different components, one per channel + for i, ch := range channels { + err = announce(m, node, ch, 99, byte(34+i)) + require.NoError(t, err) + } + + // component 0 addresses every component of the system + m.ProcessFrame(&gomavlib.EventFrame{ + Frame: targetedFrame(t, node, 0), + }) + + for _, client := range clients { + requireRouted(t, client) + } + + cancel() + wg.Wait() +} + +func TestRouteSameChannelOnce(t *testing.T) { + node := &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPServer{ + Address: "127.0.0.1:3348", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 22, + OutComponentID: 13, + Dialect: ardupilotmega.Dialect, + } + err := node.Initialize() + require.NoError(t, err) + defer node.Close() + + ctx, cancel := context.WithCancel(context.Background()) + var wg sync.WaitGroup + + m := &messageman.Manager{ + Ctx: ctx, + Wg: &wg, + StreamReqDisable: true, + Node: node, + } + err = m.Initialize() + require.NoError(t, err) + + client := &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPClient{ + Address: "127.0.0.1:3348", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 99, + OutComponentID: 34, + } + err = client.Initialize() + require.NoError(t, err) + defer client.Close() + + channels := openChannels(t, node, 1) + + // two components of the same system on a single channel + err = announce(m, node, channels[0], 99, 34) + require.NoError(t, err) + err = announce(m, node, channels[0], 99, 35) + require.NoError(t, err) + + m.ProcessFrame(&gomavlib.EventFrame{ + Frame: targetedFrame(t, node, 0), + }) + + require.Equal(t, 1, countRouted(t, client, 500*time.Millisecond)) + + cancel() + wg.Wait() +} + +func TestRouteToItself(t *testing.T) { + node := &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPServer{ + Address: "127.0.0.1:3349", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 22, + OutComponentID: 13, + Dialect: ardupilotmega.Dialect, + } + err := node.Initialize() + require.NoError(t, err) + defer node.Close() + + ctx, cancel := context.WithCancel(context.Background()) + var wg sync.WaitGroup + + m := &messageman.Manager{ + Ctx: ctx, + Wg: &wg, + StreamReqDisable: true, + Node: node, + } + err = m.Initialize() + require.NoError(t, err) + + clients := make([]*gomavlib.Node, 2) + for i := range clients { + clients[i] = &gomavlib.Node{ + Endpoints: []gomavlib.Endpoint{ + &gomavlib.EndpointTCPClient{ + Address: "127.0.0.1:3349", + }, + }, + OutVersion: gomavlib.V1, + OutSystemID: 99, + OutComponentID: 34, + } + err = clients[i].Initialize() + require.NoError(t, err) + defer clients[i].Close() //nolint:gocritic + } + + channels := openChannels(t, node, 2) + + err = announce(m, node, channels[0], 99, 34) + require.NoError(t, err) + + // the only matching channel is the sender: fall back to broadcast, which + // delivers one copy, to the other channel. Which client owns which channel + // is not defined, so count across both. + m.ProcessFrame(&gomavlib.EventFrame{ + Frame: targetedFrame(t, node, 34), + Channel: channels[0], + }) + + total := countRouted(t, clients[0], 500*time.Millisecond) + + countRouted(t, clients[1], 500*time.Millisecond) + require.Equal(t, 1, total) + + cancel() + wg.Wait() +}