FilesExpand file tree

master

/

jointState_publisher.cpp

Copy path

Blame

More file actions

Blame

More file actions

Latest commit

 

History

History

History

110 lines (94 loc) · 3.23 KB

master

/

jointState_publisher.cpp

Copy path

Top

File metadata and controls

• Code
• Blame

110 lines (94 loc) · 3.23 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

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

* @file jointState_publisher.cpp

* @brief Example cpp jointState publisher node for picoros

* @date 2025-Avg-19

*

* @details This example demonstrates a simple ROS publisher node that

* publishes jointState messages on the "picoros/joint" topic.

*

* @copyright Copyright (c) 2025 Ubiquity Robotics

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

#include <cstdio>

#include <cstdint>

#include <csignal>

#include "picoros.h"

#include "picoserdes.h"

#include "zenoh-pico/system/common/platform.h"

#include "zenoh-pico/system/platform/unix.h"

// Use command line arguments to change default values

constexpr const char* MODE = "client";

constexpr const char* LOCATOR = "tcp/192.168.1.16:7447";

// Common utils

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

extern "C" volatile sig_atomic_t picoros_keep_running;

extern "C" void sys_setup_sigint_handler(void);

// Example Publisher

picoros_publisher_t publisher = {

.topic = {

.name = "picoros/joint",

.type = ROSTYPE_NAME(ros_JointState),

.rihs_hash = ROSTYPE_HASH(ros_JointState),

},

};

// Example node

picoros_node_t node = {

.name = "talker",

};

// Buffer for publication, used from this thread

uint8_t pub_buf[1024];

void publish_jointState() {

double positions[] = {-1, 0, 1};

double velocities[] = {0.5, 0, -0.1};

double efforst[] = {0.5, 0, 0.2};

const char* names[] = {"joint1", "joint2", "joint3"};

z_clock_t now = z_clock_now();

ros_JointState joint = {

.header = {

.stamp = {

.sec = (int32_t)now.tv_sec,

.nanosec = (uint32_t)now.tv_nsec,

},

},

.name = {.data = (char**)names, .n_elements = 3},

.position = {.data = positions, .n_elements = 3},

.velocity = {.data = velocities, .n_elements = 3},

.effort = {.data = efforst, .n_elements = 3},

};

printf("Publishing JointState...\n");

size_t len = ps_serialize(pub_buf, &joint, 1024);

if (len > 0){

picoros_publish(&publisher, pub_buf, len);

}

else{

printf("Message serialization error.");

}

}

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

picoros_interface_t ifx = {

.mode = const_cast<char*>(MODE),

.locator = const_cast<char*>(LOCATOR),

};

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

if (ret != 0) {

return ret;

}

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

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

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

z_sleep_s(1);

}

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

picoros_node_init(&node);

std::printf("Declaring publisher on %s\n", publisher.topic.name);

picoros_publisher_declare(&node, &publisher);

sys_setup_sigint_handler();

while (picoros_keep_running) {

publish_jointState();

z_sleep_s(1);

}

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

picoros_publisher_drop(&publisher);

picoros_node_drop(&node);

picoros_interface_close();

return 0;

}