From c3a2d744436ad4767b0aad3b855a367bbf8842fb Mon Sep 17 00:00:00 2001 From: horverno Date: Mon, 23 May 2022 14:59:49 +0200 Subject: [PATCH] Inintial verison of fake orientation #22 --- CMakeLists.txt | 2 +- include/fake_orientation.hpp | 23 +++ src/duro.cpp | 269 +++++++++++++++++++---------------- src/fake_orientation.cpp | 75 ++++++++++ 4 files changed, 248 insertions(+), 121 deletions(-) create mode 100644 include/fake_orientation.hpp create mode 100644 src/fake_orientation.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bf4b8fd..6fa1ddc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,7 +69,7 @@ link_directories("/usr/local/lib/") ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -add_executable(duronode src/duro.cpp src/utm.cpp) +add_executable(duronode src/duro.cpp src/utm.cpp src/fake_orientation.cpp) #add_executable(libsbp_tcp_example src/tcp_example.c) ## Rename C++ executable without prefix diff --git a/include/fake_orientation.hpp b/include/fake_orientation.hpp new file mode 100644 index 0000000..6616724 --- /dev/null +++ b/include/fake_orientation.hpp @@ -0,0 +1,23 @@ +#ifndef FAKE_ORI_HPP +#define FAKE_ORI_HPP +#include +#include + + +class FakeOri{ + public: + double fake_orientation; + int status; + std::vector x_vect; + std::vector y_vect; + std::vector ori_vect; + void addXY(double x, double y); + void setStatus(int fix_mode); + void printAll(); + double calcOrientation(int p1, int p2) const; + double getDistance() const; + double getOri() const; + +}; + +#endif \ No newline at end of file diff --git a/src/duro.cpp b/src/duro.cpp index 4e4fd75..6157ee5 100644 --- a/src/duro.cpp +++ b/src/duro.cpp @@ -28,6 +28,7 @@ #include // include folder headers #include "UTM.h" +#include "fake_orientation.hpp" sensor_msgs::NavSatFix fix; ros::Publisher nav_fix_pub; @@ -35,7 +36,9 @@ ros::Publisher odom_pub; ros::Publisher imu_pub; ros::Publisher mag_pub; ros::Publisher euler_pub; +ros::Publisher euler_pub_fake; ros::Publisher pose_pub; +ros::Publisher fake_pub; ros::Publisher status_flag_pub; ros::Publisher status_stri_pub; ros::Publisher time_ref_pub; @@ -57,11 +60,13 @@ static sbp_msg_callbacks_node_t imu_aux_callback_node; static sbp_msg_callbacks_node_t mag_callback_node; nav_msgs::Odometry odom; geometry_msgs::PoseStamped pose_msg; +geometry_msgs::PoseStamped fake_pose_msg; CoordinateTransition coordinate_transition; +FakeOri fake_ori; int socket_desc = -1; -double linear_acc_conf = -1.0; //4096; // default acc_range 8g +double linear_acc_conf = -1.0; //4096; // default acc_range 8g double angular_vel_conf = -1.0; //262.4; // default gyro_range 125 bool first_run_imu_conf = true; bool first_run_z_coord = true; @@ -98,16 +103,16 @@ const u8 FIX_MODE_MASK = 7; namespace fix_modes { -enum FIX_MODE -{ - INVALID = 0, - SINGLE_POINT_POSITION, - DIFFERENTIAL_GNSS, - FLOAT_RTK, - FIXED_RTK, - DEAD_RECKONING, - SBAS_POSITION -}; + enum FIX_MODE + { + INVALID = 0, + SINGLE_POINT_POSITION, + DIFFERENTIAL_GNSS, + FLOAT_RTK, + FIXED_RTK, + DEAD_RECKONING, + SBAS_POSITION + }; } // next two bits are internal navigation system mode @@ -116,11 +121,11 @@ const u8 INS_MODE_MASK = 3 << INS_MODE_POSITION; namespace ins_modes { -enum INS_MODE -{ - NONE = 0, - INS_USED -}; + enum INS_MODE + { + NONE = 0, + INS_USED + }; } void pos_ll_callback(u16 sender_id, u8 len, u8 msg[], void *context) @@ -128,9 +133,9 @@ void pos_ll_callback(u16 sender_id, u8 len, u8 msg[], void *context) msg_pos_llh_t *latlonmsg = (msg_pos_llh_t *)msg; // nav fix (latlon) message over ROS fix.header.stamp = ros::Time::now(); - fix.header.frame_id = gps_receiver_frame_id; + fix.header.frame_id = gps_receiver_frame_id; - int ins_mode = (latlonmsg->flags & INS_MODE_MASK) >> INS_MODE_POSITION; // INS mode seems to remain 0... + int ins_mode = (latlonmsg->flags & INS_MODE_MASK) >> INS_MODE_POSITION; // INS mode seems to remain 0... int fix_mode = (latlonmsg->flags & FIX_MODE_MASK) >> FIX_MODE_POSITION; std_msgs::String stflags; @@ -138,19 +143,19 @@ void pos_ll_callback(u16 sender_id, u8 len, u8 msg[], void *context) std_msgs::UInt8 fix_mode_msg; fix_mode_msg.data = fix_mode; - status_flag_pub.publish(fix_mode_msg); // 0: Invalid 1: Single Point Position (SPP) 2: Differential GNSS (DGNSS) 3: - // Float RTK 4: Fixed RTK 5: Dead Reckoning 6: SBAS Position + status_flag_pub.publish(fix_mode_msg); // 0: Invalid 1: Single Point Position (SPP) 2: Differential GNSS (DGNSS) 3: + // Float RTK 4: Fixed RTK 5: Dead Reckoning 6: SBAS Position if (fix_mode > fix_modes::INVALID) { fix.latitude = latlonmsg->lat; fix.longitude = latlonmsg->lon; // covariance matrix - double h_covariance = pow(latlonmsg->h_accuracy * 1e-3, 2); // Convert mm to m and take the ^2 for going from std to cov - double v_covariance = pow(latlonmsg->v_accuracy * 1e-3, 2); // Convert mm to m and take the ^2 for going from std to cov - fix.position_covariance[0] = h_covariance; // x = 0, 0 - fix.position_covariance[4] = h_covariance; // y = 1, 1 - fix.position_covariance[8] = v_covariance; // z = 2, 2 + double h_covariance = pow(latlonmsg->h_accuracy * 1e-3, 2); // Convert mm to m and take the ^2 for going from std to cov + double v_covariance = pow(latlonmsg->v_accuracy * 1e-3, 2); // Convert mm to m and take the ^2 for going from std to cov + fix.position_covariance[0] = h_covariance; // x = 0, 0 + fix.position_covariance[4] = h_covariance; // y = 1, 1 + fix.position_covariance[8] = v_covariance; // z = 2, 2 fix.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; double x = 0, y = 0; @@ -169,58 +174,76 @@ void pos_ll_callback(u16 sender_id, u8 len, u8 msg[], void *context) pose_msg.pose.position.x = x; pose_msg.pose.position.y = y; - if (first_run_z_coord){ + fake_ori.addXY(x, y); + //fake_ori.printAll(); + //ROS_INFO_STREAM(fake_ori.getOri()); + + if (first_run_z_coord) + { z_coord_start = latlonmsg->height; first_run_z_coord = false; } - // z_coord_ref_switch can be zero / zero_based / orig - if (z_coord_ref_switch.compare("zero") == 0){ + // z_coord_ref_switch can be zero / zero_based / orig + if (z_coord_ref_switch.compare("zero") == 0) + { pose_msg.pose.position.z = 0; } - else if (z_coord_ref_switch.compare("zero_based") == 0){ + else if (z_coord_ref_switch.compare("zero_based") == 0) + { pose_msg.pose.position.z = latlonmsg->height - z_coord_start; } - else if (z_coord_ref_switch.compare("orig") == 0){ + else if (z_coord_ref_switch.compare("orig") == 0) + { pose_msg.pose.position.z = latlonmsg->height; } - pose_pub.publish(pose_msg); // - + pose_pub.publish(pose_msg); + fake_pose_msg.header = pose_msg.header; + fake_pose_msg.pose.position = pose_msg.pose.position; + tf2::Quaternion fake_quat; + fake_quat.setRPY(0.0, 0.0, fake_ori.getOri() + M_PI); + fake_pose_msg.pose.orientation.w = fake_quat.getW(); + fake_pose_msg.pose.orientation.x = fake_quat.getX(); + fake_pose_msg.pose.orientation.y = fake_quat.getY(); + fake_pose_msg.pose.orientation.z = fake_quat.getZ(); + fake_pub.publish(fake_pose_msg); + + fake_ori.setStatus(fix_mode); switch (fix_mode) { - case fix_modes::INVALID: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; - stflags.data = "Invalid"; - break; - case fix_modes::SINGLE_POINT_POSITION: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; - stflags.data = "Single Point Position (SPP)"; - break; - case fix_modes::DIFFERENTIAL_GNSS: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; - stflags.data = "Differential GNSS (DGNSS)"; - break; - case fix_modes::FLOAT_RTK: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; - stflags.data = "Float RTK"; - break; - case fix_modes::FIXED_RTK: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; - stflags.data = "Fixed RTK"; - break; - case fix_modes::DEAD_RECKONING: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; - stflags.data = "Dead Reckoning (DR)"; - break; - case fix_modes::SBAS_POSITION: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX; - stflags.data = "SBAS Position"; - break; - default: - fix.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; - ROS_WARN_STREAM("Acquired a fix with a mode that's not implemented. You are likely" - "using an unsupported version of libsbp."); - stflags.data = "Not implemented"; + case fix_modes::INVALID: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; + stflags.data = "Invalid"; + break; + case fix_modes::SINGLE_POINT_POSITION: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_FIX; + stflags.data = "Single Point Position (SPP)"; + break; + case fix_modes::DIFFERENTIAL_GNSS: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; + stflags.data = "Differential GNSS (DGNSS)"; + break; + case fix_modes::FLOAT_RTK: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; + stflags.data = "Float RTK"; + break; + case fix_modes::FIXED_RTK: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; + stflags.data = "Fixed RTK"; + break; + case fix_modes::DEAD_RECKONING: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; + stflags.data = "Dead Reckoning (DR)"; + break; + case fix_modes::SBAS_POSITION: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_SBAS_FIX; + stflags.data = "SBAS Position"; + break; + default: + fix.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; + ROS_WARN_STREAM("Acquired a fix with a mode that's not implemented. You are likely" + "using an unsupported version of libsbp."); + stflags.data = "Not implemented"; } nav_fix_pub.publish(fix); @@ -228,12 +251,12 @@ 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 - if (!euler_based_orientation){ + if (!euler_based_orientation) + { msg_orient_quat_t *orimsg = (msg_orient_quat_t *)msg; double w = orimsg->w * pow(2, -31); @@ -251,22 +274,21 @@ void orientation_callback(u16 sender_id, u8 len, u8 msg[], void *context) } } - void time_callback(u16 sender_id, u8 len, u8 msg[], void *context) { - msg_gps_time_t *time_gps = (msg_gps_time_t*) msg; - sensor_msgs::TimeReference time_msg; - - time_msg.header.frame_id = "ros_time"; - time_msg.header.stamp = ros::Time::now(); - - //rounded msec + residual nsec -> truncated sec + remainder nsec - long long int ttemp = (time_gps->tow * 1000000 + time_gps->ns_residual) % 1000000000; - time_msg.time_ref.nsec = ttemp; - time_msg.time_ref.sec = time_gps->tow / 1000; - time_msg.source = "gps_duro"; - - time_ref_pub.publish(time_msg); + msg_gps_time_t *time_gps = (msg_gps_time_t *)msg; + sensor_msgs::TimeReference time_msg; + + time_msg.header.frame_id = "ros_time"; + time_msg.header.stamp = ros::Time::now(); + + //rounded msec + residual nsec -> truncated sec + remainder nsec + long long int ttemp = (time_gps->tow * 1000000 + time_gps->ns_residual) % 1000000000; + time_msg.time_ref.nsec = ttemp; + time_msg.time_ref.sec = time_gps->tow / 1000; + time_msg.source = "gps_duro"; + + time_ref_pub.publish(time_msg); } void orientation_euler_callback(u16 sender_id, u8 len, u8 msg[], void *context) @@ -274,13 +296,17 @@ void orientation_euler_callback(u16 sender_id, u8 len, u8 msg[], void *context) // enable MSG ID 545 in swift console msg_orient_euler_t *orimsg = (msg_orient_euler_t *)msg; geometry_msgs::Vector3 eulervect; + geometry_msgs::Vector3 eulervect_fake; eulervect.x = orimsg->roll / 57292374.; // 57292374: raw > microdegrees > rad constant eulervect.y = orimsg->pitch / 57292374.; eulervect.z = orimsg->yaw / 57292374.; + eulervect_fake.z = fake_ori.getOri(); euler_pub.publish(eulervect); - if (euler_based_orientation){ + euler_pub_fake.publish(eulervect_fake); + if (euler_based_orientation) + { tf2::Quaternion fromeuler; - fromeuler.setRPY(eulervect.x, eulervect.y, (eulervect.z * -1) + M_PI_2); // left-handerd / right handed orientation + 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(); @@ -288,11 +314,12 @@ void orientation_euler_callback(u16 sender_id, u8 len, u8 msg[], void *context) } } -const double G_TO_M_S2 = 9.80665; // constans to convert g to m/s^2 +const double G_TO_M_S2 = 9.80665; // constans to convert g to m/s^2 const double GRAD_TO_RAD_ACC = 0.01745; // constans to convert to rad/sec void imu_callback(u16 sender_id, u8 len, u8 msg[], void *context) { - if(linear_acc_conf > 0){ + if (linear_acc_conf > 0) + { msg_imu_raw_t *imumsg = (msg_imu_raw_t *)msg; sensor_msgs::Imu imu_ros_msg; imu_ros_msg.header.stamp = ros::Time::now(); @@ -308,7 +335,7 @@ void imu_callback(u16 sender_id, u8 len, u8 msg[], void *context) imu_ros_msg.orientation.w = pose_msg.pose.orientation.w; imu_ros_msg.orientation.x = pose_msg.pose.orientation.x; imu_ros_msg.orientation.y = pose_msg.pose.orientation.y; - imu_ros_msg.orientation.z = pose_msg.pose.orientation.z; + imu_ros_msg.orientation.z = pose_msg.pose.orientation.z; imu_pub.publish(imu_ros_msg); } } @@ -333,11 +360,11 @@ namespace gyro_conf_modes { enum GYRO_CONF_MODE { - DEG_S2000 = 0, // +/- 2000 deg / s - DEG_S1000, // +/- 1000 deg / s - DEG_S500, // +/- 500 deg / s - DEG_S250, // +/- 250 deg / s - DEG_S125 // +/- 125 deg / s + DEG_S2000 = 0, // +/- 2000 deg / s + DEG_S1000, // +/- 1000 deg / s + DEG_S500, // +/- 500 deg / s + DEG_S250, // +/- 250 deg / s + DEG_S125 // +/- 125 deg / s }; } @@ -349,38 +376,39 @@ void imu_aux_callback(u16 sender_id, u8 len, u8 msg[], void *context) //ROS_INFO("IMU %x gyro:%d acc:%d", imuauxmsg->imu_conf, gyro_mode, acc_mode); switch (acc_mode) { - case acc_conf_modes::G2: - linear_acc_conf = 16384; - break; - case acc_conf_modes::G4: - linear_acc_conf = 8192; - break; - case acc_conf_modes::G8: - linear_acc_conf = 4096; - break; - case acc_conf_modes::G16: - linear_acc_conf = 2048; - break; + case acc_conf_modes::G2: + linear_acc_conf = 16384; + break; + case acc_conf_modes::G4: + linear_acc_conf = 8192; + break; + case acc_conf_modes::G8: + linear_acc_conf = 4096; + break; + case acc_conf_modes::G16: + linear_acc_conf = 2048; + break; } switch (gyro_mode) { - case gyro_conf_modes::DEG_S2000: - angular_vel_conf = 16.4; - break; - case gyro_conf_modes::DEG_S1000: - angular_vel_conf = 32.8; - break; - case gyro_conf_modes::DEG_S500: - angular_vel_conf = 65.6; - break; - case gyro_conf_modes::DEG_S250: - angular_vel_conf = 131.2; - break; - case gyro_conf_modes::DEG_S125: - angular_vel_conf = 262.4; - break; + case gyro_conf_modes::DEG_S2000: + angular_vel_conf = 16.4; + break; + case gyro_conf_modes::DEG_S1000: + angular_vel_conf = 32.8; + break; + case gyro_conf_modes::DEG_S500: + angular_vel_conf = 65.6; + break; + case gyro_conf_modes::DEG_S250: + angular_vel_conf = 131.2; + break; + case gyro_conf_modes::DEG_S125: + angular_vel_conf = 262.4; + break; } - if (first_run_imu_conf){ + if (first_run_imu_conf) + { ROS_INFO("Duro IMU initalized"); first_run_imu_conf = false; } @@ -411,10 +439,12 @@ int main(int argc, char **argv) ros::NodeHandle n; odom_pub = n.advertise("odom", 100); pose_pub = n.advertise("current_pose", 100); + fake_pub = n.advertise("current_pose_fake_orientation", 100); nav_fix_pub = n.advertise("fix", 100); mag_pub = n.advertise("mag", 100); imu_pub = n.advertise("imu", 100); euler_pub = n.advertise("rollpitchyaw", 100); + euler_pub_fake = n.advertise("rollpitchyaw_fake", 100); status_flag_pub = n.advertise("status_flag", 100); status_stri_pub = n.advertise("status_string", 100); time_ref_pub = n.advertise("time_ref", 100); @@ -440,7 +470,6 @@ int main(int argc, char **argv) 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()) { sbp_process(&s, &socket_read); diff --git a/src/fake_orientation.cpp b/src/fake_orientation.cpp new file mode 100644 index 0000000..05bb5a8 --- /dev/null +++ b/src/fake_orientation.cpp @@ -0,0 +1,75 @@ +#include "fake_orientation.hpp" +#include +#include +#include + +namespace fix_modes +{ +enum FIX_MODE +{ + INVALID = 0, + SINGLE_POINT_POSITION, + DIFFERENTIAL_GNSS, + FLOAT_RTK, + FIXED_RTK, + DEAD_RECKONING, + SBAS_POSITION +}; +} + +void FakeOri::addXY(double x, double y) { + if(x_vect.size() >= 10){ + x_vect.erase(x_vect.begin()); + y_vect.erase(y_vect.begin()); + } + x_vect.push_back(x); + y_vect.push_back(y); +} + +void FakeOri::setStatus(int fix_mode) { + status = fix_mode; +} + +void FakeOri::printAll() { + for (int i = 0; i < x_vect.size(); i++) { + std::cout << x_vect[i] - x_vect[0] << " " << y_vect[i] - y_vect[0] << ", "; + } + std::cout << "\n"; + std::cout << "\n --" << x_vect.size() << "-- "; +} + +double FakeOri::calcOrientation(int p1, int p2) const { + return atan2(y_vect[p1] - y_vect[p2], x_vect[p1] - x_vect[p2]); +} + +double FakeOri::getDistance() const { + double dist = abs(sqrt(pow(y_vect[0] - y_vect[9], 2) + pow(x_vect[0] - x_vect[9], 2))); + //std::cout << "\n **" << dist << "** "; + return dist; +} + +double FakeOri::getOri() const { + double ret = fake_orientation; + // if there is not enough mesaurement + if (x_vect.size() < 10 ){ + return ret; + } + if(getDistance() < 0.1){ + return ret; + //std::cout << "aaa\n"; + } + if(status == fix_modes::FIXED_RTK or status == fix_modes::FLOAT_RTK){ + ret = calcOrientation(0, 4); + } + else if(status == fix_modes::INVALID){ + return ret; + } + else{ + ret = calcOrientation(0, 9); + } + return ret; +} + + + +