github.com/bluenviron/gomavlib/v2@v2.2.1-0.20240308101627-2c07e3da629c/node_test.go (about)

     1  package gomavlib
     2  
     3  import (
     4  	"bytes"
     5  	"sync"
     6  	"testing"
     7  
     8  	"github.com/stretchr/testify/require"
     9  
    10  	"github.com/bluenviron/gomavlib/v2/pkg/dialect"
    11  	"github.com/bluenviron/gomavlib/v2/pkg/frame"
    12  	"github.com/bluenviron/gomavlib/v2/pkg/message"
    13  )
    14  
    15  type (
    16  	MAV_TYPE      uint64 //nolint:revive
    17  	MAV_AUTOPILOT uint64 //nolint:revive
    18  	MAV_MODE_FLAG uint64 //nolint:revive
    19  	MAV_STATE     uint64 //nolint:revive
    20  )
    21  
    22  type MessageHeartbeat struct {
    23  	Type           MAV_TYPE      `mavenum:"uint8"`
    24  	Autopilot      MAV_AUTOPILOT `mavenum:"uint8"`
    25  	BaseMode       MAV_MODE_FLAG `mavenum:"uint8"`
    26  	CustomMode     uint32
    27  	SystemStatus   MAV_STATE `mavenum:"uint8"`
    28  	MavlinkVersion uint8
    29  }
    30  
    31  func (*MessageHeartbeat) GetID() uint32 {
    32  	return 0
    33  }
    34  
    35  type MessageRequestDataStream struct {
    36  	TargetSystem    uint8
    37  	TargetComponent uint8
    38  	ReqStreamId     uint8 //nolint:revive
    39  	ReqMessageRate  uint16
    40  	StartStop       uint8
    41  }
    42  
    43  func (*MessageRequestDataStream) GetID() uint32 {
    44  	return 66
    45  }
    46  
    47  var testDialect = &dialect.Dialect{
    48  	Version:  3,
    49  	Messages: []message.Message{&MessageHeartbeat{}},
    50  }
    51  
    52  var testMessage = &MessageHeartbeat{
    53  	Type:           7,
    54  	Autopilot:      5,
    55  	BaseMode:       4,
    56  	CustomMode:     3,
    57  	SystemStatus:   2,
    58  	MavlinkVersion: 1,
    59  }
    60  
    61  func TestNodeError(t *testing.T) {
    62  	_, err := NewNode(NodeConf{
    63  		Dialect:     testDialect,
    64  		OutVersion:  V2,
    65  		OutSystemID: 11,
    66  		Endpoints: []EndpointConf{
    67  			EndpointUDPServer{"127.0.0.1:5600"},
    68  			EndpointUDPServer{"127.0.0.1:5600"},
    69  		},
    70  		HeartbeatDisable: true,
    71  	})
    72  	require.Error(t, err)
    73  }
    74  
    75  func TestNodeCloseInLoop(t *testing.T) {
    76  	node1, err := NewNode(NodeConf{
    77  		Dialect:     testDialect,
    78  		OutVersion:  V2,
    79  		OutSystemID: 11,
    80  		Endpoints: []EndpointConf{
    81  			EndpointUDPServer{"127.0.0.1:5600"},
    82  		},
    83  		HeartbeatDisable: true,
    84  	})
    85  	require.NoError(t, err)
    86  
    87  	node2, err := NewNode(NodeConf{
    88  		Dialect:     testDialect,
    89  		OutVersion:  V2,
    90  		OutSystemID: 11,
    91  		Endpoints: []EndpointConf{
    92  			EndpointUDPClient{"127.0.0.1:5600"},
    93  		},
    94  		HeartbeatDisable: true,
    95  	})
    96  	require.NoError(t, err)
    97  	defer node2.Close()
    98  
    99  	evt := <-node2.Events()
   100  	require.Equal(t, &EventChannelOpen{
   101  		Channel: evt.(*EventChannelOpen).Channel,
   102  	}, evt)
   103  
   104  	err = node2.WriteMessageAll(testMessage)
   105  	require.NoError(t, err)
   106  
   107  	for evt := range node1.Events() {
   108  		if _, ok := evt.(*EventChannelOpen); ok {
   109  			node1.Close()
   110  		}
   111  	}
   112  }
   113  
   114  func TestNodeWriteAll(t *testing.T) {
   115  	for _, ca := range []string{"message", "frame"} {
   116  		t.Run(ca, func(t *testing.T) {
   117  			server, err := NewNode(NodeConf{
   118  				Dialect:     testDialect,
   119  				OutVersion:  V2,
   120  				OutSystemID: 11,
   121  				Endpoints: []EndpointConf{
   122  					EndpointTCPServer{"127.0.0.1:5600"},
   123  				},
   124  				HeartbeatDisable: true,
   125  			})
   126  			require.NoError(t, err)
   127  			defer server.Close()
   128  
   129  			var wg sync.WaitGroup
   130  			wg.Add(5)
   131  
   132  			for i := 0; i < 5; i++ {
   133  				client, err := NewNode(NodeConf{
   134  					Dialect:     testDialect,
   135  					OutVersion:  V2,
   136  					OutSystemID: 11,
   137  					Endpoints: []EndpointConf{
   138  						EndpointTCPClient{"127.0.0.1:5600"},
   139  					},
   140  					HeartbeatDisable: true,
   141  				})
   142  				require.NoError(t, err)
   143  				defer client.Close()
   144  
   145  				go func() {
   146  					for evt := range client.Events() {
   147  						if fr, ok := evt.(*EventFrame); ok {
   148  							require.Equal(t, &EventFrame{
   149  								Frame: &frame.V2Frame{
   150  									SequenceID:  0,
   151  									SystemID:    11,
   152  									ComponentID: 1,
   153  									Message:     testMessage,
   154  									Checksum:    fr.Frame.GetChecksum(),
   155  								},
   156  								Channel: fr.Channel,
   157  							}, fr)
   158  							wg.Done()
   159  						}
   160  					}
   161  				}()
   162  			}
   163  
   164  			count := 0
   165  			for evt := range server.Events() {
   166  				if _, ok := evt.(*EventChannelOpen); ok {
   167  					count++
   168  					if count == 5 {
   169  						break
   170  					}
   171  				}
   172  			}
   173  
   174  			if ca == "message" {
   175  				err := server.WriteMessageAll(testMessage)
   176  				require.NoError(t, err)
   177  			} else {
   178  				err := server.WriteFrameAll(&frame.V2Frame{
   179  					SequenceID:  0,
   180  					SystemID:    11,
   181  					ComponentID: 1,
   182  					Message:     testMessage,
   183  					Checksum:    55967,
   184  				})
   185  				require.NoError(t, err)
   186  			}
   187  			wg.Wait()
   188  		})
   189  	}
   190  }
   191  
   192  func TestNodeWriteExcept(t *testing.T) {
   193  	for _, ca := range []string{"message", "frame"} {
   194  		t.Run(ca, func(t *testing.T) {
   195  			server, err := NewNode(NodeConf{
   196  				Dialect:     testDialect,
   197  				OutVersion:  V2,
   198  				OutSystemID: 11,
   199  				Endpoints: []EndpointConf{
   200  					EndpointTCPServer{"127.0.0.1:5600"},
   201  				},
   202  				HeartbeatDisable: true,
   203  			})
   204  			require.NoError(t, err)
   205  			defer server.Close()
   206  
   207  			var wg sync.WaitGroup
   208  			wg.Add(4)
   209  
   210  			for i := 0; i < 5; i++ {
   211  				client, err := NewNode(NodeConf{
   212  					Dialect:     testDialect,
   213  					OutVersion:  V2,
   214  					OutSystemID: 11,
   215  					Endpoints: []EndpointConf{
   216  						EndpointTCPClient{"127.0.0.1:5600"},
   217  					},
   218  					HeartbeatDisable: true,
   219  				})
   220  				require.NoError(t, err)
   221  				defer client.Close()
   222  
   223  				go func() {
   224  					for evt := range client.Events() {
   225  						if fr, ok := evt.(*EventFrame); ok {
   226  							require.Equal(t, &EventFrame{
   227  								Frame: &frame.V2Frame{
   228  									SequenceID:  0,
   229  									SystemID:    11,
   230  									ComponentID: 1,
   231  									Message:     testMessage,
   232  									Checksum:    fr.Frame.GetChecksum(),
   233  								},
   234  								Channel: fr.Channel,
   235  							}, fr)
   236  							wg.Done()
   237  						}
   238  					}
   239  				}()
   240  			}
   241  
   242  			count := 0
   243  			var except *Channel
   244  			for evt := range server.Events() {
   245  				if evt2, ok := evt.(*EventChannelOpen); ok {
   246  					if count == 1 {
   247  						except = evt2.Channel
   248  					}
   249  					count++
   250  					if count == 5 {
   251  						break
   252  					}
   253  				}
   254  			}
   255  
   256  			if ca == "message" {
   257  				err := server.WriteMessageExcept(except, testMessage)
   258  				require.NoError(t, err)
   259  			} else {
   260  				err := server.WriteFrameExcept(except, &frame.V2Frame{
   261  					SequenceID:  0,
   262  					SystemID:    11,
   263  					ComponentID: 1,
   264  					Message:     testMessage,
   265  					Checksum:    55967,
   266  				})
   267  				require.NoError(t, err)
   268  			}
   269  			wg.Wait()
   270  		})
   271  	}
   272  }
   273  
   274  func TestNodeWriteTo(t *testing.T) {
   275  	for _, ca := range []string{"message", "frame"} {
   276  		t.Run(ca, func(t *testing.T) {
   277  			server, err := NewNode(NodeConf{
   278  				Dialect:     testDialect,
   279  				OutVersion:  V2,
   280  				OutSystemID: 11,
   281  				Endpoints: []EndpointConf{
   282  					EndpointTCPServer{"127.0.0.1:5600"},
   283  				},
   284  				HeartbeatDisable: true,
   285  			})
   286  			require.NoError(t, err)
   287  			defer server.Close()
   288  
   289  			recv := make(chan struct{})
   290  
   291  			for i := 0; i < 5; i++ {
   292  				client, err := NewNode(NodeConf{
   293  					Dialect:     testDialect,
   294  					OutVersion:  V2,
   295  					OutSystemID: 11,
   296  					Endpoints: []EndpointConf{
   297  						EndpointTCPClient{"127.0.0.1:5600"},
   298  					},
   299  					HeartbeatDisable: true,
   300  				})
   301  				require.NoError(t, err)
   302  				defer client.Close()
   303  
   304  				go func() {
   305  					for evt := range client.Events() {
   306  						if fr, ok := evt.(*EventFrame); ok {
   307  							require.Equal(t, &EventFrame{
   308  								Frame: &frame.V2Frame{
   309  									SequenceID:  0,
   310  									SystemID:    11,
   311  									ComponentID: 1,
   312  									Message:     testMessage,
   313  									Checksum:    fr.Frame.GetChecksum(),
   314  								},
   315  								Channel: fr.Channel,
   316  							}, fr)
   317  							close(recv)
   318  						}
   319  					}
   320  				}()
   321  			}
   322  
   323  			count := 0
   324  			var except *Channel
   325  			for evt := range server.Events() {
   326  				if evt2, ok := evt.(*EventChannelOpen); ok {
   327  					if count == 1 {
   328  						except = evt2.Channel
   329  					}
   330  					count++
   331  					if count == 5 {
   332  						break
   333  					}
   334  				}
   335  			}
   336  
   337  			if ca == "message" {
   338  				err := server.WriteMessageTo(except, testMessage)
   339  				require.NoError(t, err)
   340  			} else {
   341  				err := server.WriteFrameTo(except, &frame.V2Frame{
   342  					SequenceID:  0,
   343  					SystemID:    11,
   344  					ComponentID: 1,
   345  					Message:     testMessage,
   346  					Checksum:    55967,
   347  				})
   348  				require.NoError(t, err)
   349  			}
   350  			<-recv
   351  		})
   352  	}
   353  }
   354  
   355  func TestNodeWriteMessageInLoop(t *testing.T) {
   356  	node1, err := NewNode(NodeConf{
   357  		Dialect:     testDialect,
   358  		OutVersion:  V2,
   359  		OutSystemID: 11,
   360  		Endpoints: []EndpointConf{
   361  			EndpointUDPServer{"127.0.0.1:5600"},
   362  		},
   363  		HeartbeatDisable: true,
   364  	})
   365  	require.NoError(t, err)
   366  	defer node1.Close()
   367  
   368  	node2, err := NewNode(NodeConf{
   369  		Dialect:     testDialect,
   370  		OutVersion:  V2,
   371  		OutSystemID: 11,
   372  		Endpoints: []EndpointConf{
   373  			EndpointUDPClient{"127.0.0.1:5600"},
   374  		},
   375  		HeartbeatDisable: true,
   376  	})
   377  	require.NoError(t, err)
   378  	defer node2.Close()
   379  
   380  	evt := <-node2.Events()
   381  	require.Equal(t, &EventChannelOpen{
   382  		Channel: evt.(*EventChannelOpen).Channel,
   383  	}, evt)
   384  
   385  	err = node2.WriteMessageAll(testMessage)
   386  	require.NoError(t, err)
   387  
   388  	for evt := range node1.Events() {
   389  		if _, ok := evt.(*EventChannelOpen); ok {
   390  			for i := 0; i < 10; i++ {
   391  				err := node1.WriteMessageAll(testMessage)
   392  				require.NoError(t, err)
   393  			}
   394  			break
   395  		}
   396  	}
   397  }
   398  
   399  func TestNodeSignature(t *testing.T) {
   400  	key1 := frame.NewV2Key(bytes.Repeat([]byte("\x4F"), 32))
   401  	key2 := frame.NewV2Key(bytes.Repeat([]byte("\xA8"), 32))
   402  
   403  	node1, err := NewNode(NodeConf{
   404  		Dialect: testDialect,
   405  		Endpoints: []EndpointConf{
   406  			EndpointUDPServer{"127.0.0.1:5600"},
   407  		},
   408  		HeartbeatDisable: true,
   409  		InKey:            key2,
   410  		OutVersion:       V2,
   411  		OutSystemID:      10,
   412  		OutKey:           key1,
   413  	})
   414  	require.NoError(t, err)
   415  	defer node1.Close()
   416  
   417  	node2, err := NewNode(NodeConf{
   418  		Dialect: testDialect,
   419  		Endpoints: []EndpointConf{
   420  			EndpointUDPClient{"127.0.0.1:5600"},
   421  		},
   422  		HeartbeatDisable: true,
   423  		InKey:            key1,
   424  		OutVersion:       V2,
   425  		OutSystemID:      11,
   426  		OutKey:           key2,
   427  	})
   428  	require.NoError(t, err)
   429  	defer node2.Close()
   430  
   431  	evt := <-node2.Events()
   432  	require.Equal(t, &EventChannelOpen{
   433  		Channel: evt.(*EventChannelOpen).Channel,
   434  	}, evt)
   435  
   436  	err = node2.WriteMessageAll(testMessage)
   437  	require.NoError(t, err)
   438  
   439  	<-node1.Events()
   440  	evt = <-node1.Events()
   441  	fr, ok := evt.(*EventFrame)
   442  	require.Equal(t, true, ok)
   443  	require.Equal(t, &EventFrame{
   444  		Frame: &frame.V2Frame{
   445  			SequenceID:          0,
   446  			SystemID:            11,
   447  			ComponentID:         1,
   448  			Message:             testMessage,
   449  			Checksum:            fr.Frame.GetChecksum(),
   450  			IncompatibilityFlag: 1,
   451  			SignatureLinkID:     fr.Frame.(*frame.V2Frame).SignatureLinkID,
   452  			Signature:           fr.Frame.(*frame.V2Frame).Signature,
   453  			SignatureTimestamp:  fr.Frame.(*frame.V2Frame).SignatureTimestamp,
   454  		},
   455  		Channel: fr.Channel,
   456  	}, evt)
   457  }
   458  
   459  func TestNodeRoute(t *testing.T) {
   460  	node1, err := NewNode(NodeConf{
   461  		Dialect:     testDialect,
   462  		OutVersion:  V2,
   463  		OutSystemID: 10,
   464  		Endpoints: []EndpointConf{
   465  			EndpointUDPClient{"127.0.0.1:5600"},
   466  		},
   467  		HeartbeatDisable: true,
   468  	})
   469  	require.NoError(t, err)
   470  	defer node1.Close()
   471  
   472  	node2, err := NewNode(NodeConf{
   473  		Dialect:     testDialect,
   474  		OutVersion:  V2,
   475  		OutSystemID: 11,
   476  		Endpoints: []EndpointConf{
   477  			EndpointUDPServer{"127.0.0.1:5600"},
   478  			EndpointUDPClient{"127.0.0.1:5601"},
   479  		},
   480  		HeartbeatDisable: true,
   481  	})
   482  	require.NoError(t, err)
   483  	defer node2.Close()
   484  
   485  	node3, err := NewNode(NodeConf{
   486  		Dialect:     testDialect,
   487  		OutVersion:  V2,
   488  		OutSystemID: 12,
   489  		Endpoints: []EndpointConf{
   490  			EndpointUDPServer{"127.0.0.1:5601"},
   491  		},
   492  		HeartbeatDisable: true,
   493  	})
   494  	require.NoError(t, err)
   495  	defer node3.Close()
   496  
   497  	evt := <-node1.Events()
   498  	_, ok := evt.(*EventChannelOpen)
   499  	require.Equal(t, true, ok)
   500  
   501  	err = node1.WriteMessageAll(testMessage)
   502  	require.NoError(t, err)
   503  
   504  	evt = <-node2.Events()
   505  	_, ok = evt.(*EventChannelOpen)
   506  	require.Equal(t, true, ok)
   507  
   508  	evt = <-node2.Events()
   509  	_, ok = evt.(*EventChannelOpen)
   510  	require.Equal(t, true, ok)
   511  
   512  	evt = <-node2.Events()
   513  	fr, ok := evt.(*EventFrame)
   514  	require.Equal(t, true, ok)
   515  
   516  	err = node2.WriteFrameExcept(fr.Channel, fr.Frame)
   517  	require.NoError(t, err)
   518  
   519  	evt = <-node3.Events()
   520  	_, ok = evt.(*EventChannelOpen)
   521  	require.Equal(t, true, ok)
   522  
   523  	evt = <-node3.Events()
   524  	fr, ok = evt.(*EventFrame)
   525  	require.Equal(t, true, ok)
   526  	require.Equal(t, &frame.V2Frame{
   527  		SystemID:    10,
   528  		ComponentID: 1,
   529  		Message:     testMessage,
   530  		Checksum:    fr.Frame.GetChecksum(),
   531  	}, fr.Frame)
   532  }
   533  
   534  func TestNodeFixFrame(t *testing.T) {
   535  	key := frame.NewV2Key(bytes.Repeat([]byte("\xB2"), 32))
   536  
   537  	node1, err := NewNode(NodeConf{
   538  		Dialect: testDialect,
   539  		Endpoints: []EndpointConf{
   540  			EndpointUDPServer{"127.0.0.1:5600"},
   541  		},
   542  		HeartbeatDisable: true,
   543  		InKey:            key,
   544  		OutVersion:       V2,
   545  		OutSystemID:      10,
   546  	})
   547  	require.NoError(t, err)
   548  	defer node1.Close()
   549  
   550  	node2, err := NewNode(NodeConf{
   551  		Dialect: testDialect,
   552  		Endpoints: []EndpointConf{
   553  			EndpointUDPClient{"127.0.0.1:5600"},
   554  		},
   555  		HeartbeatDisable: true,
   556  		OutVersion:       V2,
   557  		OutSystemID:      11,
   558  		OutKey:           key,
   559  	})
   560  	require.NoError(t, err)
   561  	defer node2.Close()
   562  
   563  	evt := <-node2.Events()
   564  	require.Equal(t, &EventChannelOpen{
   565  		Channel: evt.(*EventChannelOpen).Channel,
   566  	}, evt)
   567  
   568  	fra := &frame.V2Frame{
   569  		SequenceID:          13,
   570  		SystemID:            15,
   571  		ComponentID:         11,
   572  		Message:             testMessage,
   573  		IncompatibilityFlag: 1,
   574  		SignatureLinkID:     16,
   575  		SignatureTimestamp:  23434542,
   576  		Signature:           (*frame.V2Signature)(&[6]byte{0, 0, 0, 0, 0, 0}),
   577  	}
   578  
   579  	err = node2.FixFrame(fra)
   580  	require.NoError(t, err)
   581  
   582  	err = node2.WriteFrameAll(fra)
   583  	require.NoError(t, err)
   584  
   585  	<-node1.Events()
   586  	evt = <-node1.Events()
   587  	fr, ok := evt.(*EventFrame)
   588  	require.Equal(t, true, ok)
   589  	require.Equal(t, &EventFrame{
   590  		Frame: &frame.V2Frame{
   591  			SequenceID:          13,
   592  			SystemID:            15,
   593  			ComponentID:         11,
   594  			Message:             testMessage,
   595  			Checksum:            fr.Frame.GetChecksum(),
   596  			IncompatibilityFlag: 1,
   597  			SignatureLinkID:     16,
   598  			Signature:           fr.Frame.(*frame.V2Frame).Signature,
   599  			SignatureTimestamp:  23434542,
   600  		},
   601  		Channel: fr.Channel,
   602  	}, evt)
   603  }
   604  
   605  func TestNodeWriteSameToMultiple(t *testing.T) {
   606  	server, err := NewNode(NodeConf{
   607  		Dialect:     testDialect,
   608  		OutVersion:  V2,
   609  		OutSystemID: 11,
   610  		Endpoints: []EndpointConf{
   611  			EndpointTCPServer{"127.0.0.1:5600"},
   612  		},
   613  		HeartbeatDisable: true,
   614  	})
   615  	require.NoError(t, err)
   616  	defer server.Close()
   617  
   618  	client1, err := NewNode(NodeConf{
   619  		Dialect:     testDialect,
   620  		OutVersion:  V2,
   621  		OutSystemID: 11,
   622  		Endpoints: []EndpointConf{
   623  			EndpointTCPClient{"127.0.0.1:5600"},
   624  		},
   625  		HeartbeatDisable: true,
   626  	})
   627  	require.NoError(t, err)
   628  	defer client1.Close()
   629  
   630  	client2, err := NewNode(NodeConf{
   631  		Dialect:     testDialect,
   632  		OutVersion:  V2,
   633  		OutSystemID: 11,
   634  		Endpoints: []EndpointConf{
   635  			EndpointTCPClient{"127.0.0.1:5600"},
   636  		},
   637  		HeartbeatDisable: true,
   638  	})
   639  	require.NoError(t, err)
   640  	defer client2.Close()
   641  
   642  	evt := <-client1.Events()
   643  	_, ok := evt.(*EventChannelOpen)
   644  	require.Equal(t, true, ok)
   645  
   646  	evt = <-client2.Events()
   647  	_, ok = evt.(*EventChannelOpen)
   648  	require.Equal(t, true, ok)
   649  
   650  	evt = <-server.Events()
   651  	_, ok = evt.(*EventChannelOpen)
   652  	require.Equal(t, true, ok)
   653  
   654  	evt = <-server.Events()
   655  	_, ok = evt.(*EventChannelOpen)
   656  	require.Equal(t, true, ok)
   657  
   658  	fr := &frame.V2Frame{
   659  		SequenceID:  0,
   660  		SystemID:    11,
   661  		ComponentID: 1,
   662  		Message:     testMessage,
   663  		Checksum:    55967,
   664  	}
   665  
   666  	err = client1.WriteFrameAll(fr)
   667  	require.NoError(t, err)
   668  
   669  	err = client2.WriteFrameAll(fr)
   670  	require.NoError(t, err)
   671  
   672  	for i := 0; i < 2; i++ {
   673  		evt = <-server.Events()
   674  		fr, ok := evt.(*EventFrame)
   675  		require.Equal(t, true, ok)
   676  		require.Equal(t, &EventFrame{
   677  			Frame: &frame.V2Frame{
   678  				SequenceID:  0,
   679  				SystemID:    11,
   680  				ComponentID: 1,
   681  				Message:     testMessage,
   682  				Checksum:    55967,
   683  			},
   684  			Channel: fr.Channel,
   685  		}, evt)
   686  	}
   687  }