diff --git a/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino b/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino index 4b6b96b2..9d6ed25a 100755 --- a/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino +++ b/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino @@ -9,7 +9,6 @@ #include #include #include -#include #include #include @@ -18,61 +17,60 @@ #include rcl_publisher_t publisher; -tf2_msgs__msg__TFMessage * tf_message; -rclc_executor_t executor; +tf2_msgs__msg__TFMessage tf_message; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; -rcl_timer_t timer; #define LED_PIN 13 -#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}} -#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}} +#define RCCHECK(fn) \ + { \ + rcl_ret_t temp_rc = fn; \ + if ((temp_rc != RCL_RET_OK)) { error_loop(); } \ + } +#define RCSOFTCHECK(fn) \ + { \ + rcl_ret_t temp_rc = fn; \ + if ((temp_rc != RCL_RET_OK)) {} \ + } -extern "C" int clock_gettime(clockid_t unused, struct timespec *tp); -cIMU IMU; +extern "C" int clock_gettime(clockid_t unused, struct timespec* tp); +cIMU IMU; const void euler_to_quat(float x, float y, float z, double* q) { - float c1 = cos((y*3.14/180.0)/2); - float c2 = cos((z*3.14/180.0)/2); - float c3 = cos((x*3.14/180.0)/2); - - float s1 = sin((y*3.14/180.0)/2); - float s2 = sin((z*3.14/180.0)/2); - float s3 = sin((x*3.14/180.0)/2); - - q[0] = c1 * c2 * c3 - s1 * s2 * s3; - q[1] = s1 * s2 * c3 + c1 * c2 * s3; - q[2] = s1 * c2 * c3 + c1 * s2 * s3; - q[3] = c1 * s2 * c3 - s1 * c2 * s3; + float c1 = cos((y * 3.14 / 180.0) / 2); + float c2 = cos((z * 3.14 / 180.0) / 2); + float c3 = cos((x * 3.14 / 180.0) / 2); + + float s1 = sin((y * 3.14 / 180.0) / 2); + float s2 = sin((z * 3.14 / 180.0) / 2); + float s3 = sin((x * 3.14 / 180.0) / 2); + + q[0] = c1 * c2 * c3 - s1 * s2 * s3; + q[1] = s1 * s2 * c3 + c1 * c2 * s3; + q[2] = s1 * c2 * c3 + c1 * s2 * s3; + q[3] = c1 * s2 * c3 - s1 * c2 * s3; } -void error_loop(){ - while(1){ +void error_loop() { + while (1) { digitalWrite(LED_PIN, !digitalRead(LED_PIN)); delay(100); } } -void timer_callback(rcl_timer_t * timer, int64_t last_call_time) -{ - RCLC_UNUSED(last_call_time); - RCLC_UNUSED(timer); -} - void setup() { set_microros_transports(); + //can pass optional Hz value for reading - defaults to 200 low values delay the initialisation routine IMU.begin(); IMU.SEN.acc_cali_start(); - while( IMU.SEN.acc_cali_get_done() == false ) - { + while (IMU.SEN.acc_cali_get_done() == false) { IMU.update(); } - pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, HIGH); @@ -93,52 +91,44 @@ void setup() { ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), "/tf")); - // create timer, - const unsigned int timer_timeout = 1000; - RCCHECK(rclc_timer_init_default( - &timer, - &support, - RCL_MS_TO_NS(timer_timeout), - timer_callback)); - - // create executor - RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); - - if(!micro_ros_utilities_create_message_memory( - ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), - &tf_message, - (micro_ros_utilities_memory_conf_t) {}) - ) - { + + if (!micro_ros_utilities_create_message_memory( + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage), + &tf_message, + (micro_ros_utilities_memory_conf_t){})) { error_loop(); } - tf_message->transforms.size = 2; + tf_message.transforms.size = 1; - tf_message->transforms.data[0].header.frame_id = - micro_ros_string_utilities_set(tf_message->transforms.data[0].header.frame_id, "/panda_link0"); + tf_message.transforms.data[0].header.frame_id = + micro_ros_string_utilities_set(tf_message.transforms.data[0].header.frame_id, "/panda_link0"); - tf_message->transforms.data[1].header.frame_id = - micro_ros_string_utilities_set(tf_message->transforms.data[1].header.frame_id, "/inertial_unit"); + tf_message.transforms.data[0].child_frame_id = + micro_ros_string_utilities_set(tf_message.transforms.data[0].child_frame_id, "/inertial_unit"); } void loop() { - struct timespec tv = {0}; + struct timespec tv = { 0 }; clock_gettime(0, &tv); - - IMU.update(); - double q[4]; - euler_to_quat(IMU.rpy[0], IMU.rpy[1], IMU.rpy[2], q); - - tf_message->transforms.data[0].transform.rotation.x = (double) q[1]; - tf_message->transforms.data[0].transform.rotation.y = (double) q[2]; - tf_message->transforms.data[0].transform.rotation.z = (double) q[3]; - tf_message->transforms.data[0].transform.rotation.w = (double) q[0]; - tf_message->transforms.data[0].header.stamp.nanosec = tv.tv_nsec; - tf_message->transforms.data[0].header.stamp.sec = tv.tv_sec; - - RCSOFTCHECK(rcl_publish(&publisher, tf_message, NULL)); - - //RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); + + //only send message if imu has changed + if (IMU.update() > 0) { + double q[4]; + euler_to_quat(IMU.rpy[0], IMU.rpy[1], IMU.rpy[2], q); + + tf_message.transforms.data[0].transform.translation.x = (double)0; + tf_message.transforms.data[0].transform.translation.y = (double)0; + tf_message.transforms.data[0].transform.translation.z = (double)1; + + tf_message.transforms.data[0].transform.rotation.x = (double)q[1]; + tf_message.transforms.data[0].transform.rotation.y = (double)q[2]; + tf_message.transforms.data[0].transform.rotation.z = (double)q[3]; + tf_message.transforms.data[0].transform.rotation.w = (double)q[0]; + tf_message.transforms.data[0].header.stamp.nanosec = tv.tv_nsec; + tf_message.transforms.data[0].header.stamp.sec = tv.tv_sec; + + RCSOFTCHECK(rcl_publish(&publisher, &tf_message, NULL)); + } } \ No newline at end of file