From 919dd636006ffaab6434b589a479d7e7dbb3825e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ern=C5=91=20Horv=C3=A1th?= Date: Mon, 23 May 2022 08:51:24 +0200 Subject: [PATCH] 21 euler issue (#23) * Orientation (quaternion) from euler #21 * Added euler_based_orientation param #21 --- CMakeLists.txt | 4 +++- launch/duro_example.launch | 5 +++++ src/duro.cpp | 43 +++++++++++++++++++++++++------------- 3 files changed, 37 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7094d84..bf4b8fd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,9 @@ cmake_minimum_required(VERSION 2.8.3) project(duro_ros) ## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +#add_compile_options(-std=c++11) +add_compile_options(-std=c++11 -O2) +#add_compile_options(-std=c++17 -O2) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/launch/duro_example.launch b/launch/duro_example.launch index eb1ef74..0c14f61 100644 --- a/launch/duro_example.launch +++ b/launch/duro_example.launch @@ -13,6 +13,11 @@ orig: the original Z provided by Duro / Piksi --> + + diff --git a/src/duro.cpp b/src/duro.cpp index 143a5e2..4e4fd75 100644 --- a/src/duro.cpp +++ b/src/duro.cpp @@ -46,6 +46,7 @@ std::string gps_receiver_frame_id; std::string imu_frame_id; std::string utm_frame_id; std::string z_coord_ref_switch; +bool euler_based_orientation; static sbp_msg_callbacks_node_t heartbeat_callback_node; static sbp_msg_callbacks_node_t pos_ll_callback_node; static sbp_msg_callbacks_node_t orientation_callback_node; @@ -227,26 +228,30 @@ void pos_ll_callback(u16 sender_id, u8 len, u8 msg[], void *context) status_stri_pub.publish(stflags); } + void orientation_callback(u16 sender_id, u8 len, u8 msg[], void *context) { // enable MSG ID 544 in swift console // the MSG ID comes from eg #define SBP_MSG_ORIENT_QUAT 0x0220 --> 544 - msg_orient_quat_t *orimsg = (msg_orient_quat_t *)msg; - - double w = orimsg->w * pow(2, -31); - double x = orimsg->x * pow(2, -31); - double y = orimsg->y * pow(2, -31); - double z = orimsg->z * pow(2, -31); - tf2::Quaternion tf_orig(x, y, z, w); - tf2::Quaternion tf_rot, tf_aligned; - tf_rot.setRPY(0.0, 0.0, -M_PI_2); // left-handerd / right handed rotation - tf_aligned = tf_rot * tf_orig; // left-handerd / right handed rotation - pose_msg.pose.orientation.w = tf_aligned.w() * -1; - pose_msg.pose.orientation.x = tf_aligned.y(); // left-handerd / right handed orientation - pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation - pose_msg.pose.orientation.z = tf_aligned.z(); // left-handerd / right handed orientation + if (!euler_based_orientation){ + msg_orient_quat_t *orimsg = (msg_orient_quat_t *)msg; + + double w = orimsg->w * pow(2, -31); + double x = orimsg->x * pow(2, -31); + double y = orimsg->y * pow(2, -31); + double z = orimsg->z * pow(2, -31); + tf2::Quaternion tf_orig(x, y, z, w); + tf2::Quaternion tf_rot, tf_aligned; + tf_rot.setRPY(0.0, 0.0, -M_PI_2); // left-handerd / right handed rotation + tf_aligned = tf_rot * tf_orig; // left-handerd / right handed rotation + pose_msg.pose.orientation.w = tf_aligned.w() * -1; + pose_msg.pose.orientation.x = tf_aligned.y(); // left-handerd / right handed orientation + pose_msg.pose.orientation.y = tf_aligned.x() * -1; // left-handerd / right handed orientation + pose_msg.pose.orientation.z = tf_aligned.z(); // left-handerd / right handed orientation + } } + void time_callback(u16 sender_id, u8 len, u8 msg[], void *context) { msg_gps_time_t *time_gps = (msg_gps_time_t*) msg; @@ -273,6 +278,14 @@ void orientation_euler_callback(u16 sender_id, u8 len, u8 msg[], void *context) eulervect.y = orimsg->pitch / 57292374.; eulervect.z = orimsg->yaw / 57292374.; euler_pub.publish(eulervect); + if (euler_based_orientation){ + tf2::Quaternion fromeuler; + fromeuler.setRPY(eulervect.x, eulervect.y, (eulervect.z * -1) + M_PI_2); // left-handerd / right handed orientation + pose_msg.pose.orientation.w = fromeuler.getW(); + pose_msg.pose.orientation.x = fromeuler.getX(); + pose_msg.pose.orientation.y = fromeuler.getY(); + pose_msg.pose.orientation.z = fromeuler.getZ(); + } } const double G_TO_M_S2 = 9.80665; // constans to convert g to m/s^2 @@ -413,6 +426,7 @@ int main(int argc, char **argv) n_private.param("imu_frame_id", imu_frame_id, gps_receiver_frame_id); n_private.param("utm_frame_id", utm_frame_id, "utm"); n_private.param("z_coord_ref_switch", z_coord_ref_switch, "zero"); + n_private.param("euler_based_orientation", euler_based_orientation, true); ROS_INFO("Connecting to duro on %s:%d", tcp_ip_addr.c_str(), tcp_ip_port); setup_socket(); @@ -424,6 +438,7 @@ int main(int argc, char **argv) sbp_register_callback(&s, SBP_MSG_IMU_AUX, &imu_aux_callback, NULL, &imu_aux_callback_node); sbp_register_callback(&s, SBP_MSG_MAG_RAW, &mag_callback, NULL, &mag_callback_node); sbp_register_callback(&s, SBP_MSG_GPS_TIME, &time_callback, NULL, &time_callback_node); + ROS_INFO("Success on %s:%d", tcp_ip_addr.c_str(), tcp_ip_port); while (ros::ok())