1+ #if !defined(ARDUINO_ARCH_OPENCR)
2+ #error This examples is for micro-ROS + OpenCR board
3+ #endif
4+
5+ #include < micro_ros_arduino.h>
6+ #include < IMU.h>
7+
8+ #include < stdio.h>
9+ #include < rcl/rcl.h>
10+ #include < rcl/error_handling.h>
11+ #include < rclc/rclc.h>
12+ #include < rclc/executor.h>
13+
14+ #include < geometry_msgs/msg/transform_stamped.h>
15+ #include < tf2_msgs/msg/tf_message.h>
16+
17+ rcl_publisher_t publisher;
18+ tf2_msgs__msg__TFMessage * tf_message;
19+ rclc_executor_t executor;
20+ rclc_support_t support;
21+ rcl_allocator_t allocator;
22+ rcl_node_t node;
23+ rcl_timer_t timer;
24+
25+ #define LED_PIN 13
26+
27+ #define RCCHECK (fn ) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)){error_loop ();}}
28+ #define RCSOFTCHECK (fn ) { rcl_ret_t temp_rc = fn; if ((temp_rc != RCL_RET_OK)){error_loop ();}}
29+
30+ extern " C" int clock_gettime (clockid_t unused, struct timespec *tp);
31+ cIMU IMU;
32+
33+ const void euler_to_quat (float x, float y, float z, double * q) {
34+ float c1 = cos (y/2 );
35+ float c2 = cos (z/2 );
36+ float c3 = cos (x/2 );
37+
38+ float s1 = sin (y/2 );
39+ float s2 = sin (z/2 );
40+ float s3 = sin (x/2 );
41+
42+ q[0 ] = c1 * c2 * c3 - s1 * s2 * s3;
43+ q[1 ] = s1 * s2 * c3 + c1 * c2 * s3;
44+ q[2 ] = s1 * c2 * c3 + c1 * s2 * s3;
45+ q[3 ] = c1 * s2 * c3 - s1 * c2 * s3;
46+ }
47+
48+ void error_loop (){
49+ while (1 ){
50+ digitalWrite (LED_PIN, !digitalRead (LED_PIN));
51+ delay (100 );
52+ }
53+ }
54+
55+ void timer_callback (rcl_timer_t * timer, int64_t last_call_time)
56+ {
57+ UNUSED (last_call_time);
58+ UNUSED (timer);
59+ }
60+
61+ void setup () {
62+
63+ IMU.begin ();
64+
65+ IMU.SEN .acc_cali_start ();
66+ while ( IMU.SEN .acc_cali_get_done () == false )
67+ {
68+ IMU.update ();
69+ }
70+
71+
72+ pinMode (LED_PIN, OUTPUT);
73+ digitalWrite (LED_PIN, HIGH);
74+
75+ delay (2000 );
76+
77+ allocator = rcl_get_default_allocator ();
78+
79+ // create init_options
80+ RCCHECK (rclc_support_init (&support, 0 , NULL , &allocator));
81+
82+ // create node
83+ node = rcl_get_zero_initialized_node ();
84+ RCCHECK (rclc_node_init_default (&node, " micro_ros_arduino_node" " "
85+
86+ // create publisher
87+ RCCHECK (rclc_publisher_init_default (
88+ &publisher,
89+ &node,
90+ ROSIDL_GET_MSG_TYPE_SUPPORT (tf2_msgs, msg, TFMessage),
91+ " /tf"
92+
93+ // create timer,
94+ timer = rcl_get_zero_initialized_timer ();
95+ const unsigned int timer_timeout = 1000 ;
96+ RCCHECK (rclc_timer_init_default (
97+ &timer,
98+ &support,
99+ RCL_MS_TO_NS (timer_timeout),
100+ timer_callback));
101+
102+ // create executor
103+ executor = rclc_executor_get_zero_initialized_executor ();
104+ RCCHECK (rclc_executor_init (&executor, &support.context , 1 , &allocator));
105+
106+ unsigned int rcl_wait_timeout = 100 ; // in ms
107+ RCCHECK (rclc_executor_set_timeout (&executor, RCL_MS_TO_NS (rcl_wait_timeout)));
108+
109+
110+ tf_message = tf2_msgs__msg__TFMessage__create ();
111+ geometry_msgs__msg__TransformStamped__Sequence__init (&tf_message->transforms , 1 );
112+
113+ tf_message->transforms .data [0 ].header .frame_id .data = (char *)malloc (100 *sizeof (char ));
114+ char string1[] = " /panda_link0"
115+ memcpy (tf_message->transforms .data [0 ].header .frame_id .data , string1, strlen (string1) + 1 );
116+ tf_message->transforms .data [0 ].header .frame_id .size = strlen (tf_message->transforms .data [0 ].header .frame_id .data );
117+ tf_message->transforms .data [0 ].header .frame_id .capacity = 100 ;
118+
119+ char string2[] = " /inertial_unit"
120+ tf_message->transforms .data [0 ].child_frame_id .data = (char *)malloc (100 *sizeof (char ));
121+ memcpy (tf_message->transforms .data [0 ].child_frame_id .data , string2, strlen (string2) + 1 );
122+ tf_message->transforms .data [0 ].child_frame_id .size = strlen (tf_message->transforms .data [0 ].child_frame_id .data );
123+ tf_message->transforms .data [0 ].child_frame_id .capacity = 100 ;
124+ }
125+
126+ void loop () {
127+
128+ struct timespec tv = {0 };
129+ clock_gettime (0 , &tv);
130+
131+ IMU.update ();
132+ double q[4 ];
133+ euler_to_quat (IMU.rpy [0 ], IMU.rpy [1 ], IMU.rpy [2 ], q);
134+
135+ tf_message->transforms .data [0 ].transform .rotation .x = (double ) q[1 ];
136+ tf_message->transforms .data [0 ].transform .rotation .y = (double ) q[2 ];
137+ tf_message->transforms .data [0 ].transform .rotation .z = (double ) q[3 ];
138+ tf_message->transforms .data [0 ].transform .rotation .w = (double ) q[0 ];
139+ tf_message->transforms .data [0 ].header .stamp .nanosec = tv.tv_nsec ;
140+ tf_message->transforms .data [0 ].header .stamp .sec = tv.tv_sec ;
141+
142+ RCSOFTCHECK (rcl_publish (&publisher, tf_message, NULL ));
143+
144+ // RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
145+ }
0 commit comments