From 586df2536055d33c8daa9fdfe1da0ffab37e1db9 Mon Sep 17 00:00:00 2001 From: nick weldin Date: Mon, 19 Feb 2024 17:37:01 +0000 Subject: [PATCH 1/4] changing message from pointer to get publishing to work Signed-off-by: nick weldin --- .../micro-ros_tf_publisher.ino | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) 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 4b6b96b2b..c10ea5e3a 100755 --- a/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino +++ b/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino @@ -18,7 +18,7 @@ #include rcl_publisher_t publisher; -tf2_msgs__msg__TFMessage * tf_message; +tf2_msgs__msg__TFMessage tf_message; rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; @@ -113,13 +113,13 @@ void setup() { error_loop(); } - tf_message->transforms.size = 2; + tf_message.transforms.size = 2; - 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[1].header.frame_id = + micro_ros_string_utilities_set(tf_message.transforms.data[1].header.frame_id, "/inertial_unit"); } void loop() { @@ -131,14 +131,14 @@ void loop() { 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; + 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)); + RCSOFTCHECK(rcl_publish(&publisher, &tf_message, NULL)); //RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); } \ No newline at end of file From d82a390af7f46c441a95adeb5bc0a0b45550d532 Mon Sep 17 00:00:00 2001 From: nick weldin Date: Mon, 19 Feb 2024 19:20:11 +0000 Subject: [PATCH 2/4] removed timer and executor, as not used by code --- .../micro-ros_tf_publisher.ino | 22 ------------------- 1 file changed, 22 deletions(-) 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 c10ea5e3a..cc43931e5 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 @@ -19,11 +18,9 @@ rcl_publisher_t publisher; tf2_msgs__msg__TFMessage tf_message; -rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; rcl_node_t node; -rcl_timer_t timer; #define LED_PIN 13 @@ -55,12 +52,6 @@ void error_loop(){ } } -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(); @@ -72,7 +63,6 @@ void setup() { IMU.update(); } - pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, HIGH); @@ -93,16 +83,6 @@ 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), @@ -139,6 +119,4 @@ void loop() { 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))); } \ No newline at end of file From 172da01a2005080134dfb0a3ec3c7eea15ec2c49 Mon Sep 17 00:00:00 2001 From: nick weldin Date: Mon, 19 Feb 2024 19:38:24 +0000 Subject: [PATCH 3/4] only publish if imu changed --- .../micro-ros_tf_publisher.ino | 88 ++++++++++--------- 1 file changed, 48 insertions(+), 40 deletions(-) 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 cc43931e5..6d0acd0f5 100755 --- a/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino +++ b/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino @@ -24,29 +24,37 @@ rcl_node_t node; #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); } @@ -55,11 +63,11 @@ void error_loop(){ 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(); } @@ -84,12 +92,10 @@ void setup() { "/tf")); - 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(); } @@ -104,19 +110,21 @@ void setup() { 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)); + + //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.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 From 2dd478b09db0c5bc7163f109a7b2372a2f4b7fb8 Mon Sep 17 00:00:00 2001 From: nick weldin Date: Mon, 19 Feb 2024 20:27:02 +0000 Subject: [PATCH 4/4] removed second transform element and included both frames in first one --- .../micro-ros_tf_publisher/micro-ros_tf_publisher.ino | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) 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 6d0acd0f5..9d6ed25a6 100755 --- a/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino +++ b/examples/micro-ros_tf_publisher/micro-ros_tf_publisher.ino @@ -99,13 +99,13 @@ void setup() { 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[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() { @@ -118,6 +118,10 @@ void loop() { 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];