FilesExpand file tree

master

/

srv_server_add2ints.c

Copy path

Blame

More file actions

Blame

More file actions

Latest commit

 

History

History

History

101 lines (87 loc) · 3.02 KB

master

/

srv_server_add2ints.c

Copy path

Top

File metadata and controls

• Code
• Blame

101 lines (87 loc) · 3.02 KB

Raw

Copy raw file

Download raw file

Open symbols panel

Edit and raw actions

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

86

87

88

89

90

91

92

93

94

95

96

97

98

99

100

101

/*******************************************************************************

* @file srv_server_add2ints.c

* @brief Example service server node for picoros

* @date 2025-May-27

*

* @details This example demonstrates a ROS service server implementation that

* provides an "add two integers" service functionality.

*

* @copyright Copyright (c) 2025 Ubiquity Robotics

*******************************************************************************/

#include <stdio.h>

#include <stdint.h>

#include <signal.h>

#include "picoros.h"

#include "picoserdes.h"

// Use command line arguments to change default values

#define MODE "client"

#define LOCATOR "tcp/192.168.1.16:7447"

// Common utils

extern int sys_parse_args(int argc, char **argv, picoros_interface_t* ifx);

extern volatile sig_atomic_t picoros_keep_running;

extern void sys_setup_sigint_handler(void);

// Service callback

picoros_service_reply_t add2_srv_cb(picoros_srv_server_t* server, uint8_t* request, size_t size);

// Static buffer for service reply serialization, used from zenoh threads

uint8_t srv_buf[1024];

// Example service

picoros_srv_server_t add2_srv = {

.topic = {

.name = "services/add2",

.type = ROSTYPE_NAME(srv_AddTwoInts),

.rihs_hash = ROSTYPE_HASH(srv_AddTwoInts),

},

.user_callback = add2_srv_cb,

};

// Example node

picoros_node_t node = {

.name = "picoros",

};

// Service callback

picoros_service_reply_t add2_srv_cb(picoros_srv_server_t* server, uint8_t* rx_data, size_t rx_size){

// request, response structs

request_srv_AddTwoInts request = {};

reply_srv_AddTwoInts response = {};

// deserialize request

ps_deserialize(rx_data, &request, rx_size);

// apply service

response.sum = request.a + request.b;

printf("Service add2(a:%ld, b:%ld) called. Sending reply sum:%ld\n", request.a, request.b, response.sum);

// serialize reply

size_t len = ps_serialize(srv_buf, &response , 1024);

// send reply

picoros_service_reply_t reply = {

.length = len,

.data = srv_buf,

.free_callback = NULL, // no need to free static buffer

};

return reply;

}

int main(int argc, char **argv){

picoros_interface_t ifx = {

.mode = MODE,

.locator = LOCATOR,

};

int ret = sys_parse_args(argc, argv , &ifx);

if(ret != 0){

return ret;

}

printf("Starting pico-ros interface %s %s\n", ifx.mode, ifx.locator );

while (picoros_interface_init(&ifx) == PICOROS_NOT_READY){

printf("Waiting RMW init...\n");

z_sleep_s(1);

}

printf("Starting Pico-ROS node %s domain:%d\n", node.name, node.domain_id);

picoros_node_init(&node);

printf("Declaring service on %s\n", add2_srv.topic.name);

picoros_service_declare(&node, &add2_srv);

sys_setup_sigint_handler();

while(picoros_keep_running){

z_sleep_s(1);

}

printf("Closing interface and cleaning up...\n");

picoros_service_drop(&add2_srv);

picoros_node_drop(&node);

picoros_interface_close();

return 0;

}