Skip to content

Commit df6e137

Browse files
pablogs9pablogs9
authored
Add epoch example and param example (#45)
* Add epoch sync example * Parameter example * Update
1 parent 4d7a5fd commit df6e137

File tree

5 files changed

+212
-0
lines changed

5 files changed

+212
-0
lines changed

‎rclc/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,8 @@ export_executable(string_publisher)
5353
export_executable(string_subscriber)
5454
export_executable(autodiscover_agent)
5555
export_executable(static_type_handling)
56+
export_executable(epoch_synchronization)
57+
export_executable(parameter_server)
5658
# export_executable(configuration_example custom_transports)
5759

5860
if(EXISTS ${CMAKE_BINARY_DIR}/temp_install/)

‎rclc/epoch_synchronization/CMakeLists.txt‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(epoch_synchronization LANGUAGES C)
4+
5+
find_package(ament_cmake REQUIRED)
6+
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
8+
find_package(std_msgs REQUIRED)
9+
find_package(rmw_microxrcedds REQUIRED)
10+
11+
add_executable(${PROJECT_NAME} main.c)
12+
13+
ament_target_dependencies(${PROJECT_NAME}
14+
rcl
15+
rclc
16+
std_msgs
17+
rmw_microxrcedds
18+
)
19+
20+
install(TARGETS ${PROJECT_NAME}
21+
DESTINATION ${PROJECT_NAME}
22+
)

‎rclc/epoch_synchronization/main.c‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
#include <rcl/rcl.h>
2+
#include <rcl/error_handling.h>
3+
#include <rclc/rclc.h>
4+
#include <rclc/executor.h>
5+
#include <rmw_microros/rmw_microros.h>
6+
7+
#include <std_msgs/msg/int32.h>
8+
9+
#include <stdio.h>
10+
#include <unistd.h>
11+
12+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
13+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
14+
15+
rcl_publisher_t publisher;
16+
std_msgs__msg__Int32 msg;
17+
18+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
19+
{
20+
(void) last_call_time;
21+
(void) timer;
22+
23+
RCSOFTCHECK(rmw_uros_sync_session(1000));
24+
int64_t time = rmw_uros_epoch_millis();
25+
msg.data = time;
26+
27+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
28+
printf("UNIX time: %ld ms\n", time);
29+
}
30+
31+
int main()
32+
{
33+
rcl_allocator_t allocator = rcl_get_default_allocator();
34+
rclc_support_t support;
35+
36+
// create init_options
37+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
38+
39+
// create node
40+
rcl_node_t node;
41+
RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support));
42+
43+
// create publisher
44+
RCCHECK(rclc_publisher_init_default(
45+
&publisher,
46+
&node,
47+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
48+
"micro_ros_pub"));
49+
50+
// create timer,
51+
rcl_timer_t timer;
52+
const unsigned int timer_timeout = 1000;
53+
RCCHECK(rclc_timer_init_default(
54+
&timer,
55+
&support,
56+
RCL_MS_TO_NS(timer_timeout),
57+
timer_callback));
58+
59+
// create executor
60+
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
61+
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
62+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
63+
64+
rclc_executor_spin(&executor);
65+
66+
RCCHECK(rcl_publisher_fini(&publisher, &node));
67+
RCCHECK(rcl_node_fini(&node));
68+
}

‎rclc/parameter_server/CMakeLists.txt‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
cmake_minimum_required(VERSION 3.5)
2+
3+
project(parameter_server LANGUAGES C)
4+
5+
find_package(ament_cmake REQUIRED)
6+
find_package(rcl REQUIRED)
7+
find_package(rclc REQUIRED)
8+
find_package(std_msgs REQUIRED)
9+
find_package(rclc_parameter REQUIRED)
10+
find_package(rmw_microxrcedds REQUIRED)
11+
12+
add_executable(${PROJECT_NAME} main.c)
13+
14+
ament_target_dependencies(${PROJECT_NAME}
15+
rcl
16+
rclc
17+
rclc_parameter
18+
rmw_microxrcedds
19+
std_msgs
20+
)
21+
22+
install(TARGETS ${PROJECT_NAME}
23+
DESTINATION ${PROJECT_NAME}
24+
)

‎rclc/parameter_server/main.c‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
#include <rcl/rcl.h>
2+
#include <rcl/error_handling.h>
3+
#include <rclc/rclc.h>
4+
#include <rclc/executor.h>
5+
#include <rclc_parameter/rclc_parameter.h>
6+
7+
#include <std_msgs/msg/int32.h>
8+
9+
#include <stdio.h>
10+
#include <unistd.h>
11+
12+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Aborting.\n",__LINE__,(int)temp_rc); return 1;}}
13+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){printf("Failed status on line %d: %d. Continuing.\n",__LINE__,(int)temp_rc);}}
14+
15+
rcl_publisher_t publisher;
16+
rclc_parameter_server_t param_server;
17+
rcl_timer_t timer;
18+
bool publish = true;
19+
std_msgs__msg__Int32 msg;
20+
21+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
22+
{
23+
(void) last_call_time;
24+
(void) timer;
25+
26+
if (publish)
27+
{
28+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
29+
printf("Sent: %d\n", msg.data);
30+
31+
msg.data++;
32+
}
33+
}
34+
35+
void on_parameter_changed(Parameter * param)
36+
{
37+
if (strcmp(param->name.data, "publish_toogle") == 0 && param->value.type == RCLC_PARAMETER_BOOL)
38+
{
39+
publish = param->value.bool_value;
40+
printf("Publish %s\n", (publish) ? "ON" : "OFF");
41+
}
42+
else if (strcmp(param->name.data, "publish_rate_ms") == 0 && param->value.type == RCLC_PARAMETER_INT)
43+
{
44+
int64_t old;
45+
RCSOFTCHECK(rcl_timer_exchange_period(&timer, RCL_MS_TO_NS(param->value.integer_value), &old));
46+
printf("Publish rate %ld ms\n", param->value.integer_value);
47+
}
48+
}
49+
50+
int main()
51+
{
52+
rcl_allocator_t allocator = rcl_get_default_allocator();
53+
rclc_support_t support;
54+
55+
// create init_options
56+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
57+
58+
// create node
59+
rcl_node_t node;
60+
RCCHECK(rclc_node_init_default(&node, "micro_ros_node", "", &support));
61+
62+
// create publisher
63+
RCCHECK(rclc_publisher_init_default(
64+
&publisher,
65+
&node,
66+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
67+
"micro_ros_pub"));
68+
69+
// Create parameter service
70+
rclc_parameter_server_init_default(&param_server, &node);
71+
72+
// create timer,
73+
RCCHECK(rclc_timer_init_default(
74+
&timer,
75+
&support,
76+
RCL_MS_TO_NS(1000),
77+
timer_callback));
78+
79+
// create executor
80+
rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
81+
RCCHECK(rclc_executor_init(&executor, &support.context, RCLC_PARAMETER_EXECUTOR_HANDLES_NUMBER + 1, &allocator));
82+
RCCHECK(rclc_executor_add_parameter_server(&executor, &param_server, on_parameter_changed));
83+
RCCHECK(rclc_executor_add_timer(&executor, &timer));
84+
85+
// Add parameters
86+
rclc_add_parameter(&param_server, "publish_toogle", RCLC_PARAMETER_BOOL);
87+
rclc_add_parameter(&param_server, "publish_rate_ms", RCLC_PARAMETER_INT);
88+
89+
rclc_parameter_set_bool(&param_server, "publish_toogle", true);
90+
rclc_parameter_set_int(&param_server, "publish_rate_ms", 1000);
91+
92+
rclc_executor_spin(&executor);
93+
94+
RCCHECK(rcl_publisher_fini(&publisher, &node));
95+
RCCHECK(rcl_node_fini(&node));
96+
}

0 commit comments

Comments
 (0)