FilesExpand file tree

master

/

params_server.c

Copy path

Blame

More file actions

Blame

More file actions

Latest commit

 

History

History

History

227 lines (199 loc) · 6.46 KB

master

/

params_server.c

Copy path

Top

File metadata and controls

• Code
• Blame

227 lines (199 loc) · 6.46 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

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

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

* @file params_server.c

* @brief Example parameter server node for picoros

* @date 2025-May-27

*

* @details This example demonstrates a ROS parameter server implementation

* that handles needed requests to support configuration with rqt_reconfigure.

*

* @copyright Copyright (c) 2025 Ubiquity Robotics

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

#include <stdio.h>

#include <stdint.h>

#include <string.h>

#include <signal.h>

#include "picoros.h"

#include "picoparams.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);

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

#define STATIC_BUF_SIZE 4096

uint8_t srv_buf[STATIC_BUF_SIZE];

// Example node

picoros_node_t node = {

.name = "picoros",

};

// Example parameters

// Should probably be implemented as different structure in your application.

typedef struct{

pp_ParameterDescriptor desc;

pp_ParameterValue val;

}parameter_t;

#define N_PARAMS 3

parameter_t params[N_PARAMS] = {

{

.desc = {

.name = "example.param1",

.description = "Super important parameter",

.additional_constraints = "Must not be 0",

.type = PARAMETER_INTEGER,

.int_range.min = -50,

.int_range.max = 50,

},

.val.type = PARAMETER_INTEGER,

.val.val_int = 10,

.val.length = 1,

},{

.desc = {

.name = "example.param2",

.description = "Don't touch!",

.type = PARAMETER_DOUBLE,

.float_range.min = -3.14,

.float_range.max = 3.14,

},

.val.type = PARAMETER_DOUBLE,

.val.val_double = 1.2525,

.val.length = 1,

},{

.desc = {

.name = "example.param3",

.type = PARAMETER_BYTE_ARRAY,

},

.val.type = PARAMETER_BYTE_ARRAY,

.val.val_bytearray = (uint8_t[]){1,2,3,4,5,6,7,8,9,10},

.val.length = 10,

},

};

// Parameters server api functions declaration

void* api_param_ref(char* name);

pp_ParameterDescriptor api_param_describe(void* param);

pp_ParameterValue api_param_get(void* param);

pp_ParameterType api_param_type(void* param);

bool api_param_set(void* param, pp_ParameterValue* value, char** error_msg);

int api_param_list(char* prefix, void (*write_next)(char* param_name) );

int api_prefix_list(char* prefix, void (*write_next)(char* prefix_name) );

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);

picoparams_interface_t params_ifx = {

.f_ref = api_param_ref,

.f_get = api_param_get,

.f_set = api_param_set,

.f_describe = api_param_describe,

.f_type = api_param_type,

.f_list = api_param_list,

.f_prefixes = api_prefix_list,

.reply_buf = srv_buf,

.reply_buf_size = STATIC_BUF_SIZE,

};

if (picoparams_init(&node, params_ifx) != PICOROS_OK){

printf("Parameters server init failed.\n");

exit(-1);

}

printf("Parameter server started\n");

sys_setup_sigint_handler();

while(picoros_keep_running){

z_sleep_s(1);

}

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

picoparams_stop();

picoros_interface_close();

return 0;

}

void* api_param_ref(char* name){

for (int i = 0; i < N_PARAMS; i++){

if(strcmp(name, params[i].desc.name) == 0){

return &params[i];

}

}

printf("No param found: %s\n", name);

return NULL;

}

pp_ParameterDescriptor api_param_describe(void* param){

parameter_t* p = (parameter_t*)param;

return p->desc;

}

pp_ParameterValue api_param_get(void* param){

parameter_t* p = (parameter_t*)param;

return p->val;

}

pp_ParameterType api_param_type(void* param){

parameter_t* p = (parameter_t*)param;

return p->desc.type;

}

size_t get_param_value_size(parameter_t* p){

size_t ret = sizeof(uint8_t);

switch(p->val.type){

case PARAMETER_INTEGER:

case PARAMETER_INTEGER_ARRAY:

ret = sizeof(int64_t);

break;

case PARAMETER_DOUBLE:

case PARAMETER_DOUBLE_ARRAY:

ret = sizeof(double);

break;

case PARAMETER_BOOL:

case PARAMETER_BOOL_ARRAY:

ret = sizeof(bool);

break;

default:

break;

}

return ret * p->val.length;

}

bool api_param_set(void* param, pp_ParameterValue* value, char** error_msg){

parameter_t* p = (parameter_t*)param;

if (p->desc.type != value->type){

*error_msg = "Error wrong value";

return false;

}

if (p->val.length != value->length){

*error_msg = "Error wrong length";

return false;

}

switch(p->val.type){

case PARAMETER_BOOL:

case PARAMETER_INTEGER:

case PARAMETER_DOUBLE:

memcpy(&p->val.val_bool, &value->val_bool, get_param_value_size(p));

break;

case PARAMETER_BYTE_ARRAY:

case PARAMETER_BOOL_ARRAY:

case PARAMETER_INTEGER_ARRAY:

case PARAMETER_DOUBLE_ARRAY:

memcpy(p->val.val_bytearray, value->val_bytearray, get_param_value_size(p));

break;

default:

*error_msg = "Set operation not supported";

return false;

}

return true;

}

int api_param_list(char* prefix, void (*write_next)(char* param_name) ){

(void)prefix; // not used

for (int i = 0; i < N_PARAMS; i++){

write_next(params[i].desc.name);

}

return N_PARAMS;

}

int api_prefix_list(char* prefix, void (*write_next)(char* prefix_name) ){

return 0; // not used

}