diff --git a/.gitmodules b/.gitmodules index 398d5702..0c316ce5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -5,3 +5,6 @@ path = eagleye_pp/common/multi_rosbag_controller url = https://github.com/MapIV/multi_rosbag_controller.git +[submodule "eagleye_util/llh_converter"] + path = eagleye_util/llh_converter + url = https://github.com/MapIV/llh_converter diff --git a/README.md b/README.md index cc55ce65..421f4f95 100755 --- a/README.md +++ b/README.md @@ -57,6 +57,10 @@ Clone and build the necessary packages for Eagleye. git clone https://github.com/MapIV/nmea_ros_bridge.git git clone https://github.com/MapIV/gnss_compass_ros.git git clone https://github.com/MapIV/llh_converter.git + sudo apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc + sudo geographiclib-get-geoids best + sudo mkdir /usr/share/GSIGEO + sudo cp eagleye/eagleye_util/llh_converter/data/gsigeo2011_ver2_1.asc /usr/share/GSIGEO/ cd .. rosdep install --from-paths src --ignore-src -r -y catkin_make -DCMAKE_BUILD_TYPE=Release diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt index 5ebe0130..658b2d8f 100755 --- a/eagleye_core/navigation/CMakeLists.txt +++ b/eagleye_core/navigation/CMakeLists.txt @@ -4,6 +4,9 @@ project(eagleye_navigation) find_package(PkgConfig REQUIRED) pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +find_package(Eigen3 QUIET) +set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) + find_package(catkin REQUIRED COMPONENTS roscpp std_msgs @@ -23,6 +26,7 @@ include_directories( include ${PROJECT_SOURCE_DIR}/include ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIRS} ) diff --git a/eagleye_core/navigation/include/navigation/navigation.hpp b/eagleye_core/navigation/include/navigation/navigation.hpp index d12d4fd4..febfd7c8 100644 --- a/eagleye_core/navigation/include/navigation/navigation.hpp +++ b/eagleye_core/navigation/include/navigation/navigation.hpp @@ -24,10 +24,12 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include #include "sensor_msgs/Imu.h" #include "sensor_msgs/NavSatFix.h" #include "geometry_msgs/TwistStamped.h" +#include "geometry_msgs/TwistWithCovarianceStamped.h" #include "geometry_msgs/Vector3Stamped.h" #include "nmea_msgs/Gpgga.h" #include "nmea_msgs/Gprmc.h" @@ -136,6 +138,7 @@ struct HeadingParameter double outlier_threshold; double outlier_ratio_threshold; double curve_judgment_threshold; + double init_STD; }; struct HeadingStatus @@ -194,6 +197,7 @@ struct HeadingInterpolateParameter double imu_rate; double stop_judgment_threshold; double sync_search_period; + double proc_noise; }; struct HeadingInterpolateStatus @@ -206,6 +210,7 @@ struct HeadingInterpolateStatus double provisional_heading_angle; std::vector provisional_heading_angle_buffer; std::vector imu_stamp_buffer; + double heading_variance_last; }; struct PositionParameter @@ -321,6 +326,10 @@ struct TrajectoryParameter { double stop_judgment_threshold; double curve_judgment_threshold; + double sensor_noise_velocity; + double sensor_scale_noise_velocity; + double sensor_noise_yawrate; + double sensor_bias_noise_yawrate; }; struct TrajectoryStatus @@ -411,6 +420,8 @@ struct RtkDeadreckoningParameter double tf_gnss_rotation_w; std::string tf_gnss_parent_frame; std::string tf_gnss_child_frame; + double rtk_fix_STD; + double proc_noise; }; struct RtkDeadreckoningStatus @@ -428,6 +439,7 @@ struct RtkDeadreckoningStatus std::vector provisional_enu_pos_y_buffer; std::vector provisional_enu_pos_z_buffer; std::vector imu_stamp_buffer; + Eigen::MatrixXd position_covariance_last; }; struct EnableAdditionalRollingParameter @@ -504,7 +516,7 @@ extern void smoothing_estimate(const rtklib_msgs::RtklibNav,const geometry_msgs: eagleye_msgs::Position*); extern void trajectory_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::StatusStamped,const eagleye_msgs::Heading,const eagleye_msgs::YawrateOffset, const eagleye_msgs::YawrateOffset,const TrajectoryParameter,TrajectoryStatus*,geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*, - geometry_msgs::TwistStamped*); + geometry_msgs::TwistStamped*, geometry_msgs::TwistWithCovarianceStamped*); extern void heading_interpolate_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::YawrateOffset, const eagleye_msgs::YawrateOffset,const eagleye_msgs::Heading,const eagleye_msgs::SlipAngle,const HeadingInterpolateParameter,HeadingInterpolateStatus*, eagleye_msgs::Heading*); @@ -514,7 +526,7 @@ extern void pitching_estimate(const sensor_msgs::Imu,const nmea_msgs::Gpgga,cons const HeightParameter,HeightStatus*,eagleye_msgs::Height*,eagleye_msgs::Pitching*,eagleye_msgs::AccXOffset*,eagleye_msgs::AccXScaleFactor*); extern void trajectory3d_estimate(const sensor_msgs::Imu,const geometry_msgs::TwistStamped,const eagleye_msgs::StatusStamped,const eagleye_msgs::Heading, const eagleye_msgs::YawrateOffset,const eagleye_msgs::YawrateOffset,const eagleye_msgs::Pitching,const TrajectoryParameter,TrajectoryStatus*, - geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,geometry_msgs::TwistStamped*); + geometry_msgs::Vector3Stamped*,eagleye_msgs::Position*,geometry_msgs::TwistStamped*, geometry_msgs::TwistWithCovarianceStamped*); extern void angular_velocity_offset_stop_estimate(const geometry_msgs::TwistStamped, const sensor_msgs::Imu, const AngularVelocityOffsetStopParameter, AngularVelocityOffsetStopStatus*, eagleye_msgs::AngularVelocityOffset*); extern void rtk_deadreckoning_estimate(const rtklib_msgs::RtklibNav,const geometry_msgs::Vector3Stamped,const nmea_msgs::Gpgga, const eagleye_msgs::Heading, diff --git a/eagleye_core/navigation/src/heading.cpp b/eagleye_core/navigation/src/heading.cpp index a21c5df1..478a3aa3 100644 --- a/eagleye_core/navigation/src/heading.cpp +++ b/eagleye_core/navigation/src/heading.cpp @@ -141,10 +141,10 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity std::vector base_heading_angle_buffer2; std::vector diff_buffer; - if(!heading_interpolate.status.enabled_status) - { - heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]]; - } + if(!heading_interpolate.status.enabled_status) + { + heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]]; + } int ref_cnt; std::vector heading_angle_buffer2; @@ -229,6 +229,9 @@ void heading_estimate_(sensor_msgs::Imu imu,geometry_msgs::TwistStamped velocity provisional_heading_angle_buffer[index[index_length-1]]); } heading->status.estimate_status = true; + + double heading_STD = heading_parameter.init_STD; + heading->variance = heading_STD*heading_STD; } } } diff --git a/eagleye_core/navigation/src/heading_interpolate.cpp b/eagleye_core/navigation/src/heading_interpolate.cpp index 08781a5b..4bc381ae 100644 --- a/eagleye_core/navigation/src/heading_interpolate.cpp +++ b/eagleye_core/navigation/src/heading_interpolate.cpp @@ -43,6 +43,8 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg bool heading_estimate_status = false; std::size_t imu_stamp_buffer_length; + double proc_noise = heading_interpolate_parameter.proc_noise; + double search_buffer_number = heading_interpolate_parameter.sync_search_period * heading_interpolate_parameter.imu_rate; yawrate = imu.angular_velocity.z; @@ -77,6 +79,7 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg { double dt = imu.header.stamp.toSec() - heading_interpolate_status->time_last; heading_interpolate_status->provisional_heading_angle += yawrate * dt; + heading_interpolate_status->heading_variance_last += proc_noise*proc_noise; } // data buffer generate @@ -107,22 +110,26 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg if (heading_estimate_status && estimate_index > 0 && heading_interpolate_status->number_buffer >= estimate_index && heading_interpolate_status->heading_estimate_status_count > 1) { + double heading_variance = heading.variance; double diff_estimate_heading_angle = (heading_interpolate_status->provisional_heading_angle_buffer[estimate_index-1] - heading.heading_angle); for (int i = estimate_index; i <= heading_interpolate_status->number_buffer; i++) { heading_interpolate_status->provisional_heading_angle_buffer[i-1] -= diff_estimate_heading_angle; + heading_variance += proc_noise*proc_noise; } heading_interpolate_status->provisional_heading_angle = heading_interpolate_status->provisional_heading_angle_buffer[heading_interpolate_status->number_buffer-1]; heading_interpolate->status.enabled_status = true; heading_interpolate->status.estimate_status = true; + heading_interpolate_status->heading_variance_last = heading_variance; } else if (heading_interpolate_status->heading_estimate_status_count == 1) { heading_interpolate_status->provisional_heading_angle = heading.heading_angle; heading_interpolate->status.enabled_status = true; heading_interpolate->status.estimate_status = false; + heading_interpolate_status->heading_variance_last = heading.variance; } else { @@ -133,6 +140,7 @@ void heading_interpolate_estimate(const sensor_msgs::Imu imu, const geometry_msg if (heading_interpolate_status->heading_estimate_start_status) { heading_interpolate->heading_angle = heading_interpolate_status->provisional_heading_angle + slip_angle.slip_angle; + heading_interpolate->variance = heading_interpolate_status->heading_variance_last; } else { diff --git a/eagleye_core/navigation/src/rtk_deadreckoning.cpp b/eagleye_core/navigation/src/rtk_deadreckoning.cpp index db11f729..6970662b 100755 --- a/eagleye_core/navigation/src/rtk_deadreckoning.cpp +++ b/eagleye_core/navigation/src/rtk_deadreckoning.cpp @@ -41,7 +41,7 @@ void rtk_deadreckoning_estimate_(geometry_msgs::Vector3Stamped enu_vel, nmea_msg double ecef_rtk[3]; double llh_pos[3],llh_rtk[3]; - if(rtk_deadreckoning_status->position_estimate_start_status) + if(rtk_deadreckoning_status->position_estimate_start_status && heading.status.enabled_status) { ecef_base_pos[0] = enu_absolute_rtk_deadreckoning->ecef_base_pos.x; ecef_base_pos[1] = enu_absolute_rtk_deadreckoning->ecef_base_pos.y; @@ -97,6 +97,10 @@ void rtk_deadreckoning_estimate_(geometry_msgs::Vector3Stamped enu_vel, nmea_msg enu_absolute_rtk_deadreckoning->status.enabled_status = true; enu_absolute_rtk_deadreckoning->status.estimate_status = false; } + else if(!enu_absolute_rtk_deadreckoning->status.enabled_status) + { + return; + } enu_pos[0] = rtk_deadreckoning_status->provisional_enu_pos_x; enu_pos[1] = rtk_deadreckoning_status->provisional_enu_pos_y; @@ -104,17 +108,65 @@ void rtk_deadreckoning_estimate_(geometry_msgs::Vector3Stamped enu_vel, nmea_msg enu2llh(enu_pos, ecef_base_pos, llh_pos); + double rtk_fix_STD = rtk_deadreckoning_parameter.rtk_fix_STD; + Eigen::MatrixXd init_covariance; + init_covariance = Eigen::MatrixXd::Zero(6, 6); + init_covariance(0,0) = rtk_fix_STD * rtk_fix_STD; + init_covariance(1,1) = rtk_fix_STD * rtk_fix_STD; + init_covariance(2,2) = rtk_fix_STD * rtk_fix_STD; + init_covariance(5,5) = heading.variance; + + double proc_noise = rtk_deadreckoning_parameter.proc_noise; + Eigen::MatrixXd proc_covariance; + proc_covariance = Eigen::MatrixXd::Zero(6, 6); + proc_covariance(0,0) = proc_noise * proc_noise; + proc_covariance(1,1) = proc_noise * proc_noise; + proc_covariance(2,2) = proc_noise * proc_noise; + + Eigen::MatrixXd position_covariance; + position_covariance = Eigen::MatrixXd::Zero(6, 6); + + + if(enu_absolute_rtk_deadreckoning->status.estimate_status) + { + position_covariance = init_covariance; + rtk_deadreckoning_status->position_covariance_last = position_covariance; + } + else + { + Eigen::MatrixXd jacobian; + jacobian = Eigen::MatrixXd::Zero(6, 6); + jacobian(0,0) = 1; + jacobian(1,1) = 1; + jacobian(2,2) = 1; + jacobian(3,3) = 1; + jacobian(4,4) = 1; + jacobian(5,5) = 1; + jacobian(0,5) = enu_vel.vector.y*(enu_vel.header.stamp.toSec() - rtk_deadreckoning_status->time_last); + jacobian(1,5) = -enu_vel.vector.x*(enu_vel.header.stamp.toSec() - rtk_deadreckoning_status->time_last); + + // MEMO: Jacobean not included + // position_covariance = rtk_deadreckoning_status->position_covariance_last + proc_covariance; + + // MEMO: Jacobean not included + position_covariance = jacobian * rtk_deadreckoning_status->position_covariance_last * (jacobian.transpose()) + proc_covariance; + + rtk_deadreckoning_status->position_covariance_last = position_covariance; + } + eagleye_fix->latitude = llh_pos[0] * 180/M_PI; eagleye_fix->longitude = llh_pos[1] * 180/M_PI; eagleye_fix->altitude = llh_pos[2]; - // TODO(Map IV): temporary covariance value - eagleye_fix->position_covariance[0] = (enu_absolute_rtk_deadreckoning->status.estimate_status) ? 0.02 * 0.02 : 1.5 * 1.5; // [m^2] - eagleye_fix->position_covariance[4] = (enu_absolute_rtk_deadreckoning->status.estimate_status) ? 0.02 * 0.02 : 1.5 * 1.5; // [m^2] - eagleye_fix->position_covariance[8] = (enu_absolute_rtk_deadreckoning->status.estimate_status) ? 0.02 * 0.02 : 1.5 * 1.5; // [m^2] + eagleye_fix->position_covariance[0] = position_covariance(0,0); // [m^2] + eagleye_fix->position_covariance[4] = position_covariance(1,1); // [m^2] + eagleye_fix->position_covariance[8] = position_covariance(2,2); // [m^2] enu_absolute_rtk_deadreckoning->enu_pos.x = enu_pos[0]; enu_absolute_rtk_deadreckoning->enu_pos.y = enu_pos[1]; enu_absolute_rtk_deadreckoning->enu_pos.z = enu_pos[2]; + enu_absolute_rtk_deadreckoning->covariance[0] = position_covariance(0,0); // [m^2] + enu_absolute_rtk_deadreckoning->covariance[4] = position_covariance(1,1); // [m^2] + enu_absolute_rtk_deadreckoning->covariance[8] = position_covariance(2,2); // [m^2] rtk_deadreckoning_status->time_last = enu_vel.header.stamp.toSec(); rtk_deadreckoning_status->position_stamp_last = gga.header.stamp.toSec(); diff --git a/eagleye_core/navigation/src/trajectory.cpp b/eagleye_core/navigation/src/trajectory.cpp index b442dd7e..a4689fa3 100755 --- a/eagleye_core/navigation/src/trajectory.cpp +++ b/eagleye_core/navigation/src/trajectory.cpp @@ -31,11 +31,44 @@ #include "coordinate/coordinate.hpp" #include "navigation/navigation.hpp" + +void calculate_covariance(const geometry_msgs::TwistStamped velocity, const eagleye_msgs::StatusStamped velocity_status, + const eagleye_msgs::YawrateOffset yawrate_offset_stop, const TrajectoryParameter trajectory_parameter, + geometry_msgs::TwistWithCovarianceStamped* eagleye_twist_with_covariance) +{ + double noise_velocity; + double noise_yawrate; + + if(velocity_status.status.enabled_status) + { + noise_velocity = trajectory_parameter.sensor_noise_velocity * trajectory_parameter.sensor_noise_velocity; + } + else + { + noise_velocity = trajectory_parameter.sensor_noise_velocity * trajectory_parameter.sensor_noise_velocity + + (velocity.twist.linear.x*trajectory_parameter.sensor_scale_noise_velocity)*(velocity.twist.linear.x*trajectory_parameter.sensor_scale_noise_velocity); + } + + if(yawrate_offset_stop.status.enabled_status) + { + noise_yawrate = trajectory_parameter.sensor_noise_yawrate * trajectory_parameter.sensor_noise_yawrate; + } + else + { + noise_yawrate = trajectory_parameter.sensor_noise_yawrate * trajectory_parameter.sensor_noise_yawrate + + trajectory_parameter.sensor_bias_noise_yawrate * trajectory_parameter.sensor_bias_noise_yawrate; + } + + eagleye_twist_with_covariance->twist.covariance[0] = noise_velocity; + eagleye_twist_with_covariance->twist.covariance[35] = noise_yawrate; + +} + void trajectory_estimate(const sensor_msgs::Imu imu, const geometry_msgs::TwistStamped velocity, const eagleye_msgs::StatusStamped velocity_status, const eagleye_msgs::Heading heading_interpolate_3rd, const eagleye_msgs::YawrateOffset yawrate_offset_stop, const eagleye_msgs::YawrateOffset yawrate_offset_2nd, const TrajectoryParameter trajectory_parameter, TrajectoryStatus* trajectory_status, geometry_msgs::Vector3Stamped* enu_vel, - eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist) + eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist, geometry_msgs::TwistWithCovarianceStamped* eagleye_twist_with_covariance) { if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold && @@ -43,14 +76,19 @@ void trajectory_estimate(const sensor_msgs::Imu imu, const geometry_msgs::TwistS { // Inverted because the coordinate system is reversed eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); } else { // Inverted because the coordinate system is reversed eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); } eagleye_twist->twist.linear.x = velocity.twist.linear.x; + eagleye_twist_with_covariance->twist.twist.linear.x = velocity.twist.linear.x; + + calculate_covariance(velocity, velocity_status, yawrate_offset_stop, trajectory_parameter, eagleye_twist_with_covariance); if (trajectory_status->estimate_status_count == 0 && velocity_status.status.enabled_status == true && heading_interpolate_3rd.status.enabled_status == true) @@ -107,7 +145,8 @@ void trajectory3d_estimate(const sensor_msgs::Imu imu, const geometry_msgs::Twis const eagleye_msgs::StatusStamped velocity_status, const eagleye_msgs::Heading heading_interpolate_3rd, const eagleye_msgs::YawrateOffset yawrate_offset_stop, const eagleye_msgs::YawrateOffset yawrate_offset_2nd, const eagleye_msgs::Pitching pitching, const TrajectoryParameter trajectory_parameter, TrajectoryStatus* trajectory_status, - geometry_msgs::Vector3Stamped* enu_vel, eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist) + geometry_msgs::Vector3Stamped* enu_vel, eagleye_msgs::Position* enu_relative_pos, geometry_msgs::TwistStamped* eagleye_twist, + geometry_msgs::TwistWithCovarianceStamped* eagleye_twist_with_covariance) { if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold && @@ -115,14 +154,19 @@ void trajectory3d_estimate(const sensor_msgs::Imu imu, const geometry_msgs::Twis { //Inverted because the coordinate system is reversed eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); } else { //Inverted because the coordinate system is reversed eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); } eagleye_twist->twist.linear.x = velocity.twist.linear.x; + eagleye_twist_with_covariance->twist.twist.linear.x = velocity.twist.linear.x; + + calculate_covariance(velocity, velocity_status, yawrate_offset_stop, trajectory_parameter, eagleye_twist_with_covariance); if (trajectory_status->estimate_status_count == 0 && velocity_status.status.enabled_status == true && heading_interpolate_3rd.status.enabled_status == true) @@ -166,6 +210,8 @@ void trajectory3d_estimate(const sensor_msgs::Imu imu, const geometry_msgs::Twis enu_relative_pos->status.enabled_status = enu_relative_pos->status.estimate_status = true; } + + trajectory_status->heading_last = heading_interpolate_3rd.heading_angle; trajectory_status->time_last = imu.header.stamp.toSec(); } diff --git a/eagleye_msgs/msg/Heading.msg b/eagleye_msgs/msg/Heading.msg index 8effcce6..aabbdb08 100644 --- a/eagleye_msgs/msg/Heading.msg +++ b/eagleye_msgs/msg/Heading.msg @@ -1,4 +1,5 @@ Header header float64 heading_angle +float64 variance eagleye_msgs/Status status diff --git a/eagleye_msgs/msg/Position.msg b/eagleye_msgs/msg/Position.msg index 75ea0e50..1c75f971 100644 --- a/eagleye_msgs/msg/Position.msg +++ b/eagleye_msgs/msg/Position.msg @@ -2,4 +2,5 @@ Header header geometry_msgs/Point enu_pos geometry_msgs/Point ecef_base_pos +float64[9] covariance eagleye_msgs/Status status diff --git a/eagleye_pp/common/multi_rosbag_controller b/eagleye_pp/common/multi_rosbag_controller index b7e32ffa..242fd5a2 160000 --- a/eagleye_pp/common/multi_rosbag_controller +++ b/eagleye_pp/common/multi_rosbag_controller @@ -1 +1 @@ -Subproject commit b7e32ffa03e34e948a1cd4b2d2a32fe4e5fd9790 +Subproject commit 242fd5a2dc9403fa11920b35abca114269887bc3 diff --git a/eagleye_pp/eagleye_pp/CMakeLists.txt b/eagleye_pp/eagleye_pp/CMakeLists.txt index 1d16f917..28ad1c79 100755 --- a/eagleye_pp/eagleye_pp/CMakeLists.txt +++ b/eagleye_pp/eagleye_pp/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(catkin REQUIRED COMPONENTS eagleye_msgs eagleye_navigation eagleye_coordinate - eagleye_nmea2fix + eagleye_gnss_converter multi_rosbag_controller kml_generator tf2 @@ -34,7 +34,7 @@ catkin_package( eagleye_msgs eagleye_navigation eagleye_coordinate - eagleye_nmea2fix + eagleye_gnss_converter multi_rosbag_controller kml_generator tf2 diff --git a/eagleye_pp/eagleye_pp/config/README.md b/eagleye_pp/eagleye_pp/config/README.md index a1c00134..5b0cdb8d 100644 --- a/eagleye_pp/eagleye_pp/config/README.md +++ b/eagleye_pp/eagleye_pp/config/README.md @@ -103,14 +103,14 @@ Figure shows the relationship between these parameters. | outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0524 (3 deg) | | outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | | curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0873 (5 deg/s) | - +| init_STD | double | Standard deviation of Doppler azimuth angle [rad] | 0.0035 (0.2 deg) | ### heading_interpolate | Name | Type | Description | Default value | | :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | | sync_search_period | double | Synchronous search time for delay interpolation [s] | 2 | - +| proc_noise | double | Process Noise [rad] | 0.0005 (0.03 deg) | ### slip_angle @@ -143,7 +143,10 @@ Figure shows the relationship between these parameters. | curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0174 (1 deg/s) | | timer_updata_rate | double | Self-diagnostic cycle [Hz] | 10 | | deadlock_threshold | double | Allowable communication deadlock time for error output [s] | 1 | - +| sensor_noise_velocity | double | Sensor velocity noise | 0.05 | +| sensor_scale_noise_velocity | double | Sensor velocity scale noise | 0.02 | +| sensor_noise_yawrate | double | Sensor yaw rate noise | 0.01 | +| sensor_bias_noise_yawrate | double | Sensor yaw rate bias noise | 0.01 | ### smoothing @@ -193,6 +196,15 @@ Figure shows the relationship between these parameters. | outlier_threshold | double | Allowable gap between current outlier estimate and previous estimate [rad] | 0.002 | +### rtk_deadreckoning + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| rtk_fix_STD | double | RTK-FIX position covariance + [m] | 0.3 | +| proc_noise | double | Process Noise + [m] | 0.05 | + ### rtk_heading | Name | Type | Description | Default value | diff --git a/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml b/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml index 889e2aa8..929aa42d 100644 --- a/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml +++ b/eagleye_pp/eagleye_pp/config/eagleye_pp_config.yaml @@ -105,9 +105,11 @@ heading: outlier_threshold: 0.0524 outlier_ratio_threshold: 0.5 curve_judgment_threshold: 0.0873 + init_STD: 0.0035 #[rad] (= 0.2 [deg]) heading_interpolate: sync_search_period: 2 + proc_noise: 0.0005 #[rad] (= 0.03 [deg]) slip_angle: manual_coefficient: 0.0 @@ -126,6 +128,10 @@ trajectory: curve_judgment_threshold: 0.017453 timer_updata_rate: 10 deadlock_threshold: 1 + sensor_noise_velocity: 0.05 + sensor_scale_noise_velocity: 0.02 + sensor_noise_yawrate: 0.01 + sensor_bias_noise_yawrate: 0.1 smoothing: moving_average_time: 3 @@ -155,6 +161,10 @@ angular_velocity_offset_stop: estimated_interval: 4 outlier_threshold: 0.002 +rtk_deadreckoning: + rtk_fix_STD: 0.3 #[m] + proc_noise: 0.05 #[m] + rtk_heading: update_distance: 0.3 estimated_minimum_interval: 10 diff --git a/eagleye_pp/eagleye_pp/include/eagleye_pp.hpp b/eagleye_pp/eagleye_pp/include/eagleye_pp.hpp index 1f2bab36..c5d0a56b 100644 --- a/eagleye_pp/eagleye_pp/include/eagleye_pp.hpp +++ b/eagleye_pp/eagleye_pp/include/eagleye_pp.hpp @@ -41,7 +41,7 @@ #include "coordinate/coordinate.hpp" #include "navigation/navigation.hpp" -#include "nmea2fix/nmea2fix.hpp" +#include "gnss_converter/nmea2fix.hpp" #include #include @@ -99,6 +99,7 @@ class eagleye_pp std::vector rolling; std::vector eagleye_fix; std::vector eagleye_twist; + std::vector eagleye_twist_with_covariance; }; EagleyeStates eagleye_state_forward_, eagleye_state_backward_; diff --git a/eagleye_pp/eagleye_pp/package.xml b/eagleye_pp/eagleye_pp/package.xml index aece907c..7a8805fd 100755 --- a/eagleye_pp/eagleye_pp/package.xml +++ b/eagleye_pp/eagleye_pp/package.xml @@ -18,7 +18,7 @@ eagleye_msgs eagleye_coordinate eagleye_navigation - eagleye_nmea2fix + eagleye_gnss_converter multi_rosbag_controller yaml-cpp kml_generator @@ -33,7 +33,7 @@ eagleye_msgs eagleye_coordinate eagleye_navigation - eagleye_nmea2fix + eagleye_gnss_converter multi_rosbag_controller yaml-cpp kml_generator @@ -48,7 +48,7 @@ eagleye_msgs eagleye_coordinate eagleye_navigation - eagleye_nmea2fix + eagleye_gnss_converter multi_rosbag_controller yaml-cpp kml_generator diff --git a/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp b/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp index 7d95731f..17984bfe 100644 --- a/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp +++ b/eagleye_pp/eagleye_pp/src/eagleye_pp.cpp @@ -25,7 +25,7 @@ #include "coordinate/coordinate.hpp" #include "navigation/navigation.hpp" -#include "nmea2fix/nmea2fix.hpp" +#include "gnss_converter/nmea2fix.hpp" #include "eagleye_pp.hpp" #include diff --git a/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp b/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp index c5db596e..bd11e90f 100644 --- a/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp +++ b/eagleye_pp/eagleye_pp/src/eagleye_pp_core.cpp @@ -145,6 +145,7 @@ void eagleye_pp::setParam(std::string arg_config_file, std::string *arg_twist_to heading_interpolate_parameter_.imu_rate = conf["common"]["imu_rate"].as(); heading_interpolate_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); heading_interpolate_parameter_.sync_search_period = conf["heading_interpolate"]["sync_search_period"].as(); + heading_interpolate_parameter_.proc_noise = conf["heading_interpolate"]["proc_noise"].as(); heading_parameter_.imu_rate = conf["common"]["imu_rate"].as(); heading_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); @@ -156,6 +157,7 @@ void eagleye_pp::setParam(std::string arg_config_file, std::string *arg_twist_to heading_parameter_.outlier_threshold = conf["heading"]["outlier_threshold"].as(); heading_parameter_.outlier_ratio_threshold = conf["heading"]["outlier_ratio_threshold"].as(); heading_parameter_.curve_judgment_threshold = conf["heading"]["curve_judgment_threshold"].as(); + heading_parameter_.init_STD = conf["heading"]["init_STD"].as(); position_interpolate_parameter_.imu_rate = conf["common"]["imu_rate"].as(); position_interpolate_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); @@ -182,6 +184,8 @@ void eagleye_pp::setParam(std::string arg_config_file, std::string *arg_twist_to // rtk_deadreckoning_parameter_.tf_gnss_parent_frame = conf["tf_gnss_frame"]["parent"].as(); // rtk_deadreckoning_parameter_.tf_gnss_child_frame = conf["tf_gnss_frame"]["child"].as(); rtk_deadreckoning_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + rtk_deadreckoning_parameter_.rtk_fix_STD = conf["rtk_deadreckoning"]["rtk_fix_STD"].as(); + rtk_deadreckoning_parameter_.proc_noise = conf["rtk_deadreckoning"]["proc_noise"].as(); slip_angle_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); slip_angle_parameter_.manual_coefficient = conf["slip_angle"]["manual_coefficient"].as(); @@ -196,6 +200,10 @@ void eagleye_pp::setParam(std::string arg_config_file, std::string *arg_twist_to trajectory_parameter_.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); trajectory_parameter_.curve_judgment_threshold = conf["trajectory"]["curve_judgment_threshold"].as(); + trajectory_parameter_.sensor_noise_velocity = conf["trajectory"]["sensor_noise_velocity"].as(); + trajectory_parameter_.sensor_scale_noise_velocity = conf["trajectory"]["sensor_scale_noise_velocity"].as(); + trajectory_parameter_.sensor_noise_yawrate = conf["trajectory"]["sensor_noise_yawrate"].as(); + trajectory_parameter_.sensor_bias_noise_yawrate = conf["trajectory"]["sensor_bias_noise_yawrate"].as(); velocity_scale_factor_parameter_.imu_rate = conf["common"]["imu_rate"].as(); velocity_scale_factor_parameter_.gnss_rate = conf["common"]["gnss_rate"].as(); @@ -614,6 +622,7 @@ void eagleye_pp::estimatingEagleye(bool arg_forward_flag) eagleye_msgs::Position _gnss_smooth_pos_enu; sensor_msgs::NavSatFix _eagleye_fix; geometry_msgs::TwistStamped _eagleye_twist; + geometry_msgs::TwistWithCovarianceStamped _eagleye_twist_with_covariance; eagleye_msgs::StatusStamped _velocity_status; VelocityEstimator velocity_estimator; @@ -735,7 +744,8 @@ void eagleye_pp::estimatingEagleye(bool arg_forward_flag) _enu_vel.header = imu_[i].header; _enu_relative_pos.header = imu_[i].header; _eagleye_twist.header = imu_[i].header; - trajectory_estimate(imu_[i], _correction_velocity, _velocity_status, _heading_interpolate_3rd, _yawrate_offset_stop, _yawrate_offset_2nd, trajectory_parameter_, &trajectory_status, &_enu_vel, &_enu_relative_pos, &_eagleye_twist); + trajectory_estimate(imu_[i], _correction_velocity, _velocity_status, _heading_interpolate_3rd, _yawrate_offset_stop, _yawrate_offset_2nd, trajectory_parameter_, + &trajectory_status, &_enu_vel, &_enu_relative_pos, &_eagleye_twist, &_eagleye_twist_with_covariance); _enu_absolute_pos.header = imu_[i].header; _enu_absolute_pos_interpolate.header = imu_[i].header; @@ -789,6 +799,7 @@ void eagleye_pp::estimatingEagleye(bool arg_forward_flag) eagleye_state_forward_.gnss_smooth_pos_enu.push_back(_gnss_smooth_pos_enu); eagleye_state_forward_.eagleye_fix.push_back(_eagleye_fix); eagleye_state_forward_.eagleye_twist.push_back(_eagleye_twist); + eagleye_state_forward_.eagleye_twist_with_covariance.push_back(_eagleye_twist_with_covariance); eagleye_state_forward_.flag_reliability_buffer.push_back(height_status.flag_reliability); } else @@ -818,6 +829,7 @@ void eagleye_pp::estimatingEagleye(bool arg_forward_flag) eagleye_state_backward_.gnss_smooth_pos_enu.push_back(_gnss_smooth_pos_enu); eagleye_state_backward_.eagleye_fix.push_back(_eagleye_fix); eagleye_state_backward_.eagleye_twist.push_back(_eagleye_twist); + eagleye_state_backward_.eagleye_twist_with_covariance.push_back(_eagleye_twist_with_covariance); eagleye_state_backward_.flag_reliability_buffer.push_back(height_status.flag_reliability); } @@ -1468,7 +1480,7 @@ void eagleye_pp::smoothingTrajectory(void) trajectory_estimate(imu_[i], eagleye_state_forward_.correction_velocity[i], velocity_enable_status, eagleye_state_forward_.heading_interpolate_3rd[i], eagleye_state_forward_.yawrate_offset_stop[i], eagleye_state_forward_.yawrate_offset_2nd[i], trajectory_parameter_, &trajectory_status, &eagleye_state_forward_.enu_vel[i], &eagleye_state_forward_.enu_relative_pos[i], - &eagleye_state_forward_.eagleye_twist[i]); + &eagleye_state_forward_.eagleye_twist[i], &eagleye_state_forward_.eagleye_twist_with_covariance[i]); } gga_length = std::distance(gga_.begin(), gga_.end()); diff --git a/eagleye_pp/eagleye_pp/src/eagleye_pp_output.cpp b/eagleye_pp/eagleye_pp/src/eagleye_pp_output.cpp index 217bf30c..b6614735 100644 --- a/eagleye_pp/eagleye_pp/src/eagleye_pp_output.cpp +++ b/eagleye_pp/eagleye_pp/src/eagleye_pp_output.cpp @@ -189,12 +189,12 @@ output_csv_file << "timestamp,eagleye_llh.latitude,eagleye_llh.longitude,eagleye output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_fix[i].position_covariance[7] << ","; // eagleye_llh.position_covariance[7] output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_fix[i].position_covariance[8] << ","; // eagleye_llh.position_covariance[8] output_csv_file << bool(eagleye_state_forward_.enu_absolute_pos_interpolate[i].status.estimate_status) << ","; // eagleye_llh.status - output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist[i].twist.linear.x << ","; // eagleye_twist.linear.x - output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist[i].twist.linear.y << ","; // eagleye_twist.linear.y - output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist[i].twist.linear.z << ","; // eagleye_twist.linear.z - output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist[i].twist.angular.x << ","; // eagleye_twist.angular.x - output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist[i].twist.angular.y << ","; // eagleye_twist.angular.y - output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist[i].twist.angular.z << ","; // eagleye_twist.angular.z + output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist_with_covariance[i].twist.twist.linear.x << ","; // eagleye_twist.linear.x + output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist_with_covariance[i].twist.twist.linear.y << ","; // eagleye_twist.linear.y + output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist_with_covariance[i].twist.twist.linear.z << ","; // eagleye_twist.linear.z + output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist_with_covariance[i].twist.twist.angular.x << ","; // eagleye_twist.angular.x + output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist_with_covariance[i].twist.twist.angular.y << ","; // eagleye_twist.angular.y + output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state_forward_.eagleye_twist_with_covariance[i].twist.twist.angular.z << ","; // eagleye_twist.angular.z output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; // eagleye_twist.covariance[0] output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; // eagleye_twist.covariance[1] output_csv_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; // eagleye_twist.covariance[2] @@ -302,19 +302,23 @@ void eagleye_pp::writeDetailCSVOneWay(std::ofstream* output_log_csv_file, const ,velocity.twist.linear.x,velocity.twist.linear.y,velocity.twist.linear.z,velocity.twist.angular.x,velocity.twist.angular.y,velocity.twist.angular.z\ ,velocity_scale_factor.scale_factor,correction_velocity.twist.linear.x,correction_velocity.twist.linear.y,correction_velocity.twist.linear.z,correction_velocity.twist.angular.x,correction_velocity.twist.angular.y,correction_velocity.twist.angular.z,velocity_scale_factor.status.enabled_status,velocity_scale_factor.status.estimate_status\ ,distance.distance,distance.status.enabled_status,distance.status.estimate_status\ -,heading_1st.heading_angle,heading_1st.status.enabled_status,heading_1st.status.estimate_status\ -,heading_interpolate_1st.heading_angle,heading_interpolate_1st.status.enabled_status,heading_interpolate_1st.status.estimate_status\ -,heading_2nd.heading_angle,heading_2nd.status.enabled_status,heading_2nd.status.estimate_status\ -,heading_interpolate_2nd.heading_angle,heading_interpolate_2nd.status.enabled_status,heading_interpolate_2nd.status.estimate_status\ -,heading_3rd.heading_angle,heading_3rd.status.enabled_status,heading_3rd.status.estimate_status\ -,heading_interpolate_3rd.heading_angle,heading_interpolate_3rd.status.enabled_status,heading_interpolate_3rd.status.estimate_status\ +,heading_1st.heading_angle,heading_1st.variance,heading_1st.status.enabled_status,heading_1st.status.estimate_status\ +,heading_interpolate_1st.heading_angle,heading_interpolate_1st.variance,heading_interpolate_1st.status.enabled_status,heading_interpolate_1st.status.estimate_status\ +,heading_2nd.heading_angle,heading_2nd.variance,heading_2nd.status.enabled_status,heading_2nd.status.estimate_status\ +,heading_interpolate_2nd.heading_angle,heading_interpolate_2nd.variance,heading_interpolate_2nd.status.enabled_status,heading_interpolate_2nd.status.estimate_status\ +,heading_3rd.heading_angle,heading_3rd.variance,heading_3rd.status.enabled_status,heading_3rd.status.estimate_status\ +,heading_interpolate_3rd.heading_angle,heading_interpolate_3rd.variance,heading_interpolate_3rd.status.enabled_status,heading_interpolate_3rd.status.estimate_status\ ,yawrate_offset_stop.yawrate_offset,yawrate_offset_stop.status.enabled_status,yawrate_offset_stop.status.estimate_status\ ,yawrate_offset_1st.yawrate_offset,yawrate_offset_1st.status.enabled_status,yawrate_offset_1st.status.estimate_status\ ,yawrate_offset_2nd.yawrate_offset,yawrate_offset_2nd.status.enabled_status,yawrate_offset_2nd.status.estimate_status\ ,slip_angle.coefficient,slip_angle.slip_angle,slip_angle.status.enabled_status,slip_angle.status.estimate_status\ ,enu_vel.vector.x,enu_vel.vector.y,enu_vel.vector.z\ -,enu_absolute_pos.enu_pos.x,enu_absolute_pos.enu_pos.y,enu_absolute_pos.enu_pos.z,enu_absolute_pos.ecef_base_pos.x,enu_absolute_pos.ecef_base_pos.y,enu_absolute_pos.ecef_base_pos.z,enu_absolute_pos.status.enabled_status,enu_absolute_pos.status.estimate_status\ -,enu_absolute_pos_interpolate.enu_pos.x,enu_absolute_pos_interpolate.enu_pos.y,enu_absolute_pos_interpolate.enu_pos.z,enu_absolute_pos_interpolate.ecef_base_pos.x,enu_absolute_pos_interpolate.ecef_base_pos.y,enu_absolute_pos_interpolate.ecef_base_pos.z,enu_absolute_pos_interpolate.status.enabled_status,enu_absolute_pos_interpolate.status.estimate_status\ +,enu_absolute_pos.enu_pos.x,enu_absolute_pos.enu_pos.y,enu_absolute_pos.enu_pos.z,enu_absolute_pos.ecef_base_pos.x,enu_absolute_pos.ecef_base_pos.y,enu_absolute_pos.ecef_base_pos.z\ +,enu_absolute_pos.covariance[0],enu_absolute_pos.covariance[1],enu_absolute_pos.covariance[2],enu_absolute_pos.covariance[3],enu_absolute_pos.covariance[4],enu_absolute_pos.covariance[5],enu_absolute_pos.covariance[6],enu_absolute_pos.covariance[7],enu_absolute_pos.covariance[8]\ +,enu_absolute_pos.status.enabled_status,enu_absolute_pos.status.estimate_status\ +,enu_absolute_pos_interpolate.enu_pos.x,enu_absolute_pos_interpolate.enu_pos.y,enu_absolute_pos_interpolate.enu_pos.z,enu_absolute_pos_interpolate.ecef_base_pos.x,enu_absolute_pos_interpolate.ecef_base_pos.y,enu_absolute_pos_interpolate.ecef_base_pos.z\ +,enu_absolute_pos_interpolate.covariance[0],enu_absolute_pos_interpolate.covariance[1],enu_absolute_pos_interpolate.covariance[2],enu_absolute_pos_interpolate.covariance[3],enu_absolute_pos_interpolate.covariance[4],enu_absolute_pos_interpolate.covariance[5],enu_absolute_pos_interpolate.covariance[6],enu_absolute_pos_interpolate.covariance[7],enu_absolute_pos_interpolate.covariance[8]\ +,enu_absolute_pos_interpolate.status.enabled_status,enu_absolute_pos_interpolate.status.estimate_status\ ,height.height,height.status.enabled_status,height.status.estimate_status\ ,pitching.pitching_angle,pitching.status.enabled_status,pitching.status.estimate_status\ ,acc_x_offset.acc_x_offset,acc_x_offset.status.enabled_status,acc_x_offset.status.estimate_status\ @@ -371,21 +375,27 @@ void eagleye_pp::writeDetailCSVOneWay(std::ofstream* output_log_csv_file, const *output_log_csv_file << (eagleye_state.distance[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.distance[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_1st[i].heading_angle << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_1st[i].variance << ","; *output_log_csv_file << (eagleye_state.heading_1st[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.heading_1st[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_interpolate_1st[i].heading_angle << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_interpolate_1st[i].variance << ","; *output_log_csv_file << (eagleye_state.heading_interpolate_1st[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.heading_interpolate_1st[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_2nd[i].heading_angle << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_2nd[i].variance << ","; *output_log_csv_file << (eagleye_state.heading_2nd[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.heading_2nd[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_interpolate_2nd[i].heading_angle << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_interpolate_2nd[i].variance << ","; *output_log_csv_file << (eagleye_state.heading_interpolate_2nd[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.heading_interpolate_2nd[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_3rd[i].heading_angle << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_3rd[i].variance << ","; *output_log_csv_file << (eagleye_state.heading_3rd[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.heading_3rd[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_interpolate_3rd[i].heading_angle << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.heading_interpolate_3rd[i].variance << ","; *output_log_csv_file << (eagleye_state.heading_interpolate_3rd[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.heading_interpolate_3rd[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.yawrate_offset_stop[i].yawrate_offset << ","; @@ -410,6 +420,15 @@ void eagleye_pp::writeDetailCSVOneWay(std::ofstream* output_log_csv_file, const *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].ecef_base_pos.x << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].ecef_base_pos.y << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].ecef_base_pos.z << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[0] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[1] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[2] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[3] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[4] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[5] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[6] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[7] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos[i].covariance[8] << ","; *output_log_csv_file << (eagleye_state.enu_absolute_pos[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.enu_absolute_pos[i].status.estimate_status ? "1" : "0") << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].enu_pos.x << ","; @@ -418,6 +437,15 @@ void eagleye_pp::writeDetailCSVOneWay(std::ofstream* output_log_csv_file, const *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].ecef_base_pos.x << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].ecef_base_pos.y << ","; *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].ecef_base_pos.z << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[0] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[1] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[2] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[3] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[4] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[5] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[6] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[7] << ","; + *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.enu_absolute_pos_interpolate[i].covariance[8] << ","; *output_log_csv_file << (eagleye_state.enu_absolute_pos_interpolate[i].status.enabled_status ? "1" : "0") << ","; *output_log_csv_file << (eagleye_state.enu_absolute_pos_interpolate[i].status.estimate_status ? "1" : "0") << ","; // *output_log_csv_file << std::setprecision(std::numeric_limits::max_digits10) << eagleye_state.angular_velocity_offset_stop.rollrate_offset << ","; diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt index a929590e..1a819325 100644 --- a/eagleye_rt/CMakeLists.txt +++ b/eagleye_rt/CMakeLists.txt @@ -53,6 +53,10 @@ include_directories( ${YAML_CPP_INCLUDE_DIRS} ) +add_executable(twist_relay src/twist_relay_node.cpp) +target_link_libraries(twist_relay ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) +add_dependencies(twist_relay ${catkin_EXPORTED_TARGETS}) + add_executable(velocity_scale_factor src/velocity_scale_factor_node.cpp) target_link_libraries(velocity_scale_factor ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) add_dependencies(velocity_scale_factor ${catkin_EXPORTED_TARGETS}) diff --git a/eagleye_rt/config/README.md b/eagleye_rt/config/README.md index ae26fa3b..be8325b8 100644 --- a/eagleye_rt/config/README.md +++ b/eagleye_rt/config/README.md @@ -15,13 +15,12 @@ The parameters for estimation in Eagleye can be set in the `config/eagleye_confi | Name | Type | Description | Default value | | :---------------------------- | :----- | :---------------------------------------------------------------------- | :----------------------- | | imu_topic | string | Topic name to be subscribed to in node (sensor_msgs/Imu.msg) | /imu/data_raw | -| twist_topic | string | Topic name to be subscribed to in node (geometry_msgs/TwistStamped.msg) | /can_twist | -| rtklib_nav_topic | string | Topic name to be subscribed to in node (rtklib_msgs/RtklibNav.msg) | /rtklib_nav_topic | -| nmea_sentence_topic | string | Topic name to be subscribed to in node (nmea_msgs/Sentence.msg) | /nmea_sentence_topic | -| gga_topic | string | Topic name to be subscribed to in node (nmea_msgs/Gpgga.msg) | /navsat/gga | -| rmc_topic | string | Topic name to be subscribed to in node (nmea_msgs/Gprmc.msg) | /navsat/rmc | -| localization_pose_topic | string | Topic name to be subscribed to in node (geometry_msgs/PoseStamped.msg) | /localization_pose_topic | - +| twist.twist_type | int | Topic type to be subscribed to in node (TwistStamped : 0, TwistWithCovarianceStamped: 1) | 0 | +| twist.twist_topic | string | Topic name to be subscribed to in node | /can_twist | +| gnss.velocity_source_type | int | Topic type to be subscribed to in node (rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3) | 0 | +| gnss.velocity_source_topic | string | Topic name to be subscribed to in node | /rtklib_nav | +| gnss.llh_source_type | int | Topic type to be subscribed to in node (rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2) | 0 | +| gnss.llh_source_topic | string | Topic name to be subscribed to in node | /rtklib_nav | ## TF @@ -111,14 +110,14 @@ Figure shows the relationship between these parameters. | outlier_threshold | double | Outlier threshold due to GNSS multipath [rad] | 0.0524 (3 deg) | | outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | | curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0873 (5 deg/s) | - +| init_STD | double | Standard deviation of Doppler azimuth angle [rad] | 0.0035 (0.2 deg) | ### heading_interpolate | Name | Type | Description | Default value | | :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | | sync_search_period | double | Synchronous search time for delay interpolation [s] | 2 | - +| proc_noise | double | Process Noise [rad] | 0.0005 (0.03 deg) | ### slip_angle @@ -151,7 +150,10 @@ Figure shows the relationship between these parameters. | curve_judgment_threshold | double | Yaw rate threshold for curve determination [rad/s] | 0.0174 (1 deg/s) | | timer_updata_rate | double | Self-diagnostic cycle [Hz] | 10 | | deadlock_threshold | double | Allowable communication deadlock time for error output [s] | 1 | - +| sensor_noise_velocity | double | Sensor velocity noise | 0.05 | +| sensor_scale_noise_velocity | double | Sensor velocity scale noise | 0.02 | +| sensor_noise_yawrate | double | Sensor yaw rate noise | 0.01 | +| sensor_bias_noise_yawrate | double | Sensor yaw rate bias noise | 0.01 | ### smoothing @@ -217,6 +219,15 @@ Figure shows the relationship between these parameters. | outlier_threshold | double | Allowable gap between current outlier estimate and previous estimate [rad] | 0.002 | +### rtk_deadreckoning + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| rtk_fix_STD | double | RTK-FIX position covariance + [m] | 0.3 | +| proc_noise | double | Process Noise + [m] | 0.05 | + ### rtk_heading | Name | Type | Description | Default value | diff --git a/eagleye_rt/config/eagleye_config.yaml b/eagleye_rt/config/eagleye_config.yaml index 8c18ba14..c6289ac2 100644 --- a/eagleye_rt/config/eagleye_config.yaml +++ b/eagleye_rt/config/eagleye_config.yaml @@ -4,13 +4,15 @@ use_gnss_mode: RTKLIB use_canless_mode: false # Topic -twist_topic: /can_twist +twist: + twist_type: 0 # TwistStamped : 0, TwistWithCovarianceStamped: 1 + twist_topic: /can_twist imu_topic: /imu/data_raw -rtklib_nav_topic: /rtklib_nav -nmea_sentence_topic: /nmea_sentence -gga_topic: /navsat/gga -rmc_topic: /navsat/rmc -localization_pose_topic: /localization/pose +gnss: + velocity_source_type: 0 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3 + velocity_source_topic: /rtklib_nav + llh_source_type: 0 # rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2 + llh_source_topic: /rtklib_nav # TF tf_gnss_frame: @@ -62,9 +64,11 @@ heading: outlier_threshold: 0.0524 outlier_ratio_threshold: 0.5 curve_judgment_threshold: 0.0873 + init_STD: 0.0035 #[rad] (= 0.2 [deg]) heading_interpolate: sync_search_period: 2 + proc_noise: 0.0005 #[rad] (= 0.03 [deg]) slip_angle: manual_coefficient: 0.0 @@ -83,6 +87,10 @@ trajectory: curve_judgment_threshold: 0.017453 timer_updata_rate: 10 deadlock_threshold: 1 + sensor_noise_velocity: 0.05 + sensor_scale_noise_velocity: 0.02 + sensor_noise_yawrate: 0.01 + sensor_bias_noise_yawrate: 0.1 smoothing: moving_average_time: 3 @@ -123,6 +131,10 @@ angular_velocity_offset_stop: estimated_interval: 4 outlier_threshold: 0.002 +rtk_deadreckoning: + rtk_fix_STD: 0.3 #[m] + proc_noise: 0.05 #[m] + rtk_heading: update_distance: 0.3 estimated_minimum_interval: 10 diff --git a/eagleye_rt/launch/eagleye_calibration.launch b/eagleye_rt/launch/eagleye_calibration.launch index a6b0f9b1..6c8d0188 100644 --- a/eagleye_rt/launch/eagleye_calibration.launch +++ b/eagleye_rt/launch/eagleye_calibration.launch @@ -12,6 +12,7 @@ + @@ -43,6 +44,8 @@ + + diff --git a/eagleye_rt/launch/eagleye_rt.launch b/eagleye_rt/launch/eagleye_rt.launch index 3a4d2cd3..68bf70b8 100644 --- a/eagleye_rt/launch/eagleye_rt.launch +++ b/eagleye_rt/launch/eagleye_rt.launch @@ -18,6 +18,7 @@ + @@ -32,8 +33,8 @@ - - + + @@ -48,7 +49,7 @@ - + diff --git a/eagleye_rt/launch/eagleye_rt_canless.launch b/eagleye_rt/launch/eagleye_rt_canless.launch index 130ed7ea..7aad2ed4 100644 --- a/eagleye_rt/launch/eagleye_rt_canless.launch +++ b/eagleye_rt/launch/eagleye_rt_canless.launch @@ -18,6 +18,7 @@ + @@ -43,6 +44,7 @@ + diff --git a/eagleye_rt/launch/eagleye_rt_dualantenna.launch b/eagleye_rt/launch/eagleye_rt_dualantenna.launch index 804a9c95..839da648 100644 --- a/eagleye_rt/launch/eagleye_rt_dualantenna.launch +++ b/eagleye_rt/launch/eagleye_rt_dualantenna.launch @@ -18,6 +18,7 @@ + diff --git a/eagleye_rt/launch/navpvt_rt.launch b/eagleye_rt/launch/navpvt_rt.launch deleted file mode 100644 index 6e3b021c..00000000 --- a/eagleye_rt/launch/navpvt_rt.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - - - - - - - - - \ No newline at end of file diff --git a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp index f13875fc..51e46b94 100644 --- a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp +++ b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp @@ -59,7 +59,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "angular_velocity_offset_stop"); ros::NodeHandle nh; - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string yaml_file; nh.getParam("yaml_file",yaml_file); @@ -69,8 +69,6 @@ int main(int argc, char** argv) { YAML::Node conf = YAML::LoadFile(yaml_file); - subscribe_twist_topic_name = conf["twist_topic"].as(); - _angular_velocity_offset_stop_parameter.imu_rate = conf["common"]["imu_rate"].as(); _angular_velocity_offset_stop_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); _angular_velocity_offset_stop_parameter.estimated_interval = conf["angular_velocity_offset_stop"]["estimated_interval"].as(); diff --git a/eagleye_rt/src/heading_interpolate_node.cpp b/eagleye_rt/src/heading_interpolate_node.cpp index cd6bb9e0..91bc3c52 100644 --- a/eagleye_rt/src/heading_interpolate_node.cpp +++ b/eagleye_rt/src/heading_interpolate_node.cpp @@ -107,10 +107,12 @@ int main(int argc, char** argv) _heading_interpolate_parameter.imu_rate = conf["common"]["imu_rate"].as(); _heading_interpolate_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); _heading_interpolate_parameter.sync_search_period = conf["heading_interpolate"]["sync_search_period"].as(); + _heading_interpolate_parameter.proc_noise = conf["heading_interpolate"]["proc_noise"].as(); std::cout << "imu_rate " << _heading_interpolate_parameter.imu_rate << std::endl; std::cout << "stop_judgment_threshold " << _heading_interpolate_parameter.stop_judgment_threshold << std::endl; std::cout << "sync_search_period " << _heading_interpolate_parameter.sync_search_period << std::endl; + std::cout << "proc_noise " << _heading_interpolate_parameter.proc_noise << std::endl; } catch (YAML::Exception& e) { diff --git a/eagleye_rt/src/heading_node.cpp b/eagleye_rt/src/heading_node.cpp index ef5c6a3d..c8f382f7 100644 --- a/eagleye_rt/src/heading_node.cpp +++ b/eagleye_rt/src/heading_node.cpp @@ -151,7 +151,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "heading"); ros::NodeHandle nh; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "navsat/rtklib_nav"; std::string subscribe_rmc_topic_name = "navsat/rmc"; std::string yaml_file; @@ -164,8 +164,6 @@ int main(int argc, char** argv) _use_gnss_mode = conf["use_gnss_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); - _heading_parameter.imu_rate = conf["common"]["imu_rate"].as(); _heading_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); _heading_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); @@ -176,6 +174,7 @@ int main(int argc, char** argv) _heading_parameter.outlier_threshold = conf["heading"]["outlier_threshold"].as(); _heading_parameter.outlier_ratio_threshold = conf["heading"]["outlier_ratio_threshold"].as(); _heading_parameter.curve_judgment_threshold = conf["heading"]["curve_judgment_threshold"].as(); + _heading_parameter.init_STD = conf["heading"]["init_STD"].as(); std::cout<< "use_gnss_mode " << _use_gnss_mode << std::endl; @@ -193,6 +192,7 @@ int main(int argc, char** argv) std::cout << "outlier_threshold " << _heading_parameter.outlier_threshold << std::endl; std::cout << "outlier_ratio_threshold " << _heading_parameter.outlier_ratio_threshold << std::endl; std::cout << "curve_judgment_threshold " << _heading_parameter.curve_judgment_threshold << std::endl; + std::cout << "init_STD " << _heading_parameter.init_STD << std::endl; } catch (YAML::Exception& e) { diff --git a/eagleye_rt/src/height_node.cpp b/eagleye_rt/src/height_node.cpp index aabc46f7..e207d27b 100644 --- a/eagleye_rt/src/height_node.cpp +++ b/eagleye_rt/src/height_node.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "height"); ros::NodeHandle nh; - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_gga_topic_name = "navsat/gga"; std::string yaml_file; nh.getParam("yaml_file",yaml_file); diff --git a/eagleye_rt/src/monitor_node.cpp b/eagleye_rt/src/monitor_node.cpp index f80720b6..5e9d53d3 100755 --- a/eagleye_rt/src/monitor_node.cpp +++ b/eagleye_rt/src/monitor_node.cpp @@ -1095,9 +1095,9 @@ int main(int argc, char** argv) ros::NodeHandle n; - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string subscribe_imu_topic_name = "/imu/data_raw"; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "navsat/rtklib_nav"; std::string subscribe_gga_topic_name = "navsat/gga"; std::string comparison_twist_topic_name = "/calculated_twist"; @@ -1110,10 +1110,7 @@ int main(int argc, char** argv) YAML::Node conf = YAML::LoadFile(yaml_file); n.getParam("use_rtk_deadreckoning",_use_rtk_deadreckoning); - - subscribe_twist_topic_name = conf["twist_topic"].as(); subscribe_imu_topic_name = conf["imu_topic"].as(); - subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); comparison_twist_topic_name = conf["monitor"]["comparison_twist_topic"].as(); _print_status = conf["monitor"]["print_status"].as(); diff --git a/eagleye_rt/src/position_node.cpp b/eagleye_rt/src/position_node.cpp index 7619410b..6091238f 100644 --- a/eagleye_rt/src/position_node.cpp +++ b/eagleye_rt/src/position_node.cpp @@ -147,7 +147,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "position"); ros::NodeHandle nh; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "navsat/rtklib_nav"; std::string subscribe_gga_topic_name = "navsat/gga"; std::string yaml_file; diff --git a/eagleye_rt/src/rtk_deadreckoning_node.cpp b/eagleye_rt/src/rtk_deadreckoning_node.cpp index 1b88e8fb..a1aa262c 100644 --- a/eagleye_rt/src/rtk_deadreckoning_node.cpp +++ b/eagleye_rt/src/rtk_deadreckoning_node.cpp @@ -126,7 +126,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "rtk_deadreckoning"); ros::NodeHandle nh; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "navsat/rtklib_nav"; std::string subscribe_gga_topic_name = "navsat/gga"; std::string yaml_file; @@ -138,7 +138,6 @@ int main(int argc, char** argv) YAML::Node conf = YAML::LoadFile(yaml_file); _use_gnss_mode = conf["use_gnss_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); _rtk_deadreckoning_parameter.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); _rtk_deadreckoning_parameter.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); @@ -148,6 +147,9 @@ int main(int argc, char** argv) _rtk_deadreckoning_parameter.tf_gnss_child_frame = conf["tf_gnss_frame"]["child"].as(); _rtk_deadreckoning_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); + _rtk_deadreckoning_parameter.rtk_fix_STD = conf["rtk_deadreckoning"]["rtk_fix_STD"].as(); + _rtk_deadreckoning_parameter.proc_noise = conf["rtk_deadreckoning"]["proc_noise"].as(); + std::cout << "use_gnss_mode " << _use_gnss_mode << std::endl; std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; @@ -158,6 +160,8 @@ int main(int argc, char** argv) std::cout << "tf_gnss_frame/parent " << _rtk_deadreckoning_parameter.tf_gnss_parent_frame << std::endl; std::cout << "tf_gnss_frame/child " << _rtk_deadreckoning_parameter.tf_gnss_child_frame << std::endl; std::cout << "stop_judgment_threshold " << _rtk_deadreckoning_parameter.stop_judgment_threshold << std::endl; + std::cout << "rtk_fix_STD " << _rtk_deadreckoning_parameter.rtk_fix_STD << std::endl; + std::cout << "proc_noise " << _rtk_deadreckoning_parameter.proc_noise << std::endl; } catch (YAML::Exception& e) { @@ -170,8 +174,8 @@ int main(int argc, char** argv) ros::Subscriber sub3 = nh.subscribe(subscribe_gga_topic_name, 1000, gga_callback, ros::TransportHints().tcpNoDelay()); ros::Subscriber sub4 = nh.subscribe("heading_interpolate_3rd", 1000, heading_interpolate_3rd_callback, ros::TransportHints().tcpNoDelay()); - _pub1 = nh.advertise("enu_absolute_rtk_deadreckoning", 1000); - _pub2 = nh.advertise("rtk_fix", 1000); + _pub1 = nh.advertise("enu_absolute_pos_interpolate", 1000); + _pub2 = nh.advertise("fix", 1000); tf2_ros::Buffer tf_buffer; tf2_ros::TransformListener tf_listener(tf_buffer); diff --git a/eagleye_rt/src/rtk_heading_node.cpp b/eagleye_rt/src/rtk_heading_node.cpp index d45df6a8..2d9b9f94 100644 --- a/eagleye_rt/src/rtk_heading_node.cpp +++ b/eagleye_rt/src/rtk_heading_node.cpp @@ -112,7 +112,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "rtk_heading"); ros::NodeHandle nh; - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_gga_topic_name = "navsat/gga"; std::string yaml_file; nh.getParam("yaml_file",yaml_file); diff --git a/eagleye_rt/src/slip_coefficient_node.cpp b/eagleye_rt/src/slip_coefficient_node.cpp index d6042a21..e1d95036 100644 --- a/eagleye_rt/src/slip_coefficient_node.cpp +++ b/eagleye_rt/src/slip_coefficient_node.cpp @@ -106,7 +106,7 @@ int main(int argc, char** argv) ros::NodeHandle nh; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "navsat/rtklib_nav"; std::string yaml_file; nh.getParam("yaml_file",yaml_file); diff --git a/eagleye_rt/src/smoothing_node.cpp b/eagleye_rt/src/smoothing_node.cpp index 7297d597..9bfa074e 100644 --- a/eagleye_rt/src/smoothing_node.cpp +++ b/eagleye_rt/src/smoothing_node.cpp @@ -69,7 +69,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "smoothing"); ros::NodeHandle nh; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "navsat/rtklib_nav"; std::string yaml_file; nh.getParam("yaml_file",yaml_file); @@ -81,8 +81,6 @@ int main(int argc, char** argv) _use_canless_mode = conf["use_canless_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); - _smoothing_parameter.ecef_base_pos_x = conf["ecef_base_pos"]["x"].as(); _smoothing_parameter.ecef_base_pos_y = conf["ecef_base_pos"]["y"].as(); _smoothing_parameter.ecef_base_pos_z = conf["ecef_base_pos"]["z"].as(); diff --git a/eagleye_rt/src/trajectory_node.cpp b/eagleye_rt/src/trajectory_node.cpp index 2e77211d..e3dec865 100644 --- a/eagleye_rt/src/trajectory_node.cpp +++ b/eagleye_rt/src/trajectory_node.cpp @@ -45,9 +45,11 @@ static eagleye_msgs::Pitching _pitching; static geometry_msgs::Vector3Stamped _enu_vel; static eagleye_msgs::Position _enu_relative_pos; static geometry_msgs::TwistStamped _eagleye_twist; +static geometry_msgs::TwistWithCovarianceStamped _eagleye_twist_with_covariance; static ros::Publisher _pub1; static ros::Publisher _pub2; static ros::Publisher _pub3; +static ros::Publisher _pub4; struct TrajectoryParameter _trajectory_parameter; struct TrajectoryStatus _trajectory_status; @@ -144,7 +146,8 @@ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg) _eagleye_twist.header = msg->header; _eagleye_twist.header.frame_id = "base_link"; trajectory3d_estimate(_imu, _correction_velocity, velocity_enable_status, _heading_interpolate_3rd, _yawrate_offset_stop, - _yawrate_offset_2nd, _pitching, _trajectory_parameter, &_trajectory_status, &_enu_vel, &_enu_relative_pos, &_eagleye_twist); + _yawrate_offset_2nd, _pitching, _trajectory_parameter, &_trajectory_status, &_enu_vel, &_enu_relative_pos, &_eagleye_twist, + &_eagleye_twist_with_covariance); if(_heading_interpolate_3rd.status.enabled_status) { @@ -152,6 +155,7 @@ void imu_callback(const sensor_msgs::Imu::ConstPtr& msg) _pub2.publish(_enu_relative_pos); } _pub3.publish(_eagleye_twist); + _pub4.publish(_eagleye_twist_with_covariance); } } @@ -161,7 +165,7 @@ int main(int argc, char** argv) ros::NodeHandle nh; - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string yaml_file; nh.getParam("yaml_file",yaml_file); @@ -173,11 +177,14 @@ int main(int argc, char** argv) _use_canless_mode = conf["use_canless_mode"].as(); - subscribe_twist_topic_name = conf["twist_topic"].as(); - _trajectory_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); - _trajectory_parameter.curve_judgment_threshold = conf["trajectory"]["curve_judgment_threshold"].as(); + + _trajectory_parameter.sensor_noise_velocity = conf["trajectory"]["sensor_noise_velocity"].as(); + _trajectory_parameter.sensor_scale_noise_velocity = conf["trajectory"]["sensor_scale_noise_velocity"].as(); + _trajectory_parameter.sensor_noise_yawrate = conf["trajectory"]["sensor_noise_yawrate"].as(); + _trajectory_parameter.sensor_bias_noise_yawrate = conf["trajectory"]["sensor_bias_noise_yawrate"].as(); + _timer_updata_rate = conf["trajectory"]["timer_updata_rate"].as(); _deadlock_threshold = conf["trajectory"]["deadlock_threshold"].as(); @@ -186,8 +193,13 @@ int main(int argc, char** argv) std::cout<< "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; std::cout << "stop_judgment_threshold " << _trajectory_parameter.stop_judgment_threshold << std::endl; - std::cout << "curve_judgment_threshold " << _trajectory_parameter.curve_judgment_threshold << std::endl; + + std::cout << "sensor_noise_velocity " << _trajectory_parameter.sensor_noise_velocity << std::endl; + std::cout << "sensor_scale_noise_velocity " << _trajectory_parameter.sensor_scale_noise_velocity << std::endl; + std::cout << "sensor_noise_yawrate " << _trajectory_parameter.sensor_noise_yawrate << std::endl; + std::cout << "sensor_bias_noise_yawrate " << _trajectory_parameter.sensor_bias_noise_yawrate << std::endl; + std::cout << "timer_updata_rate " << _timer_updata_rate << std::endl; std::cout << "deadlock_threshold " << _deadlock_threshold << std::endl; } @@ -209,6 +221,7 @@ int main(int argc, char** argv) _pub1 = nh.advertise("enu_vel", 1000); _pub2 = nh.advertise("enu_relative_pos", 1000); _pub3 = nh.advertise("twist", 1000); + _pub4 = nh.advertise("twist_with_covariance", 1000); ros::Timer timer = nh.createTimer(ros::Duration(1/_timer_updata_rate), timer_callback); diff --git a/eagleye_rt/src/twist_relay_node.cpp b/eagleye_rt/src/twist_relay_node.cpp new file mode 100644 index 00000000..1f2ce7b9 --- /dev/null +++ b/eagleye_rt/src/twist_relay_node.cpp @@ -0,0 +1,120 @@ +// Copyright (c) 2022, Map IV, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of the Map IV, Inc. nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +/* + * correction_imu.cpp + * Author MapIV Sasaki + */ + +#include "ros/ros.h" +#include "coordinate/coordinate.hpp" +#include "navigation/navigation.hpp" + + +class TwistRelay +{ +public: + TwistRelay(); + ~TwistRelay(); + +private: + ros::NodeHandle nh_; + ros::Publisher pub_; + ros::Subscriber sub_, sub2_; + + void twist_callback(const geometry_msgs::TwistStamped::ConstPtr& msg); + void twist_with_covariance_callback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg); + +}; + +TwistRelay::TwistRelay() +{ + int subscribe_twist_topic_type = 0; + std::string subscribe_twist_topic_name = "/can_twist"; + std::string publish_twist_topic_name = "vehicle/twist"; + + std::string yaml_file; + nh_.getParam("yaml_file",yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + subscribe_twist_topic_type = conf["twist"]["twist_type"].as(); + subscribe_twist_topic_name = conf["twist"]["twist_topic"].as(); + std::cout<< "subscribe_twist_topic_type: " << subscribe_twist_topic_type << std::endl; + std::cout<< "subscribe_twist_topic_name: " << subscribe_twist_topic_name << std::endl; + + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;twist_relay Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } + + // TwistStamped : 0, TwistWithCovarianceStamped: 1 + if(subscribe_twist_topic_type == 0) + { + sub_ = nh_.subscribe(subscribe_twist_topic_name, 1000, &TwistRelay::twist_callback, this, ros::TransportHints().tcpNoDelay()); + } + else if(subscribe_twist_topic_type == 1) + { + sub2_ = nh_.subscribe(subscribe_twist_topic_name, 1000, &TwistRelay::twist_with_covariance_callback, this, ros::TransportHints().tcpNoDelay()); + } + else + { + ROS_ERROR("Invalid twist topic type"); + ros::shutdown(); + } + + pub_ = nh_.advertise("vehicle/twist", 1000); +}; + +TwistRelay::~TwistRelay(){}; + +void TwistRelay::twist_callback(const geometry_msgs::TwistStamped::ConstPtr& msg) +{ + pub_.publish(*msg); +}; + +void TwistRelay::twist_with_covariance_callback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg) +{ + geometry_msgs::TwistStamped twist; + twist.header.stamp = msg->header.stamp; + twist.twist = msg->twist.twist; + pub_.publish(twist); +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "twist_relay"); + + TwistRelay main_node; + + ros::spin(); + + return 0; +} diff --git a/eagleye_rt/src/velocity_scale_factor_node.cpp b/eagleye_rt/src/velocity_scale_factor_node.cpp index 58f26456..17270d19 100644 --- a/eagleye_rt/src/velocity_scale_factor_node.cpp +++ b/eagleye_rt/src/velocity_scale_factor_node.cpp @@ -190,9 +190,9 @@ int main(int argc, char** argv) ros::init(argc, argv, "velocity_scale_factor"); ros::NodeHandle nh; - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string subscribe_imu_topic_name = "/imu/data_raw"; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "navsat/rtklib_nav"; std::string subscribe_rmc_topic_name = "navsat/rmc"; double velocity_scale_factor_save_duration; // [sec] @@ -208,8 +208,6 @@ int main(int argc, char** argv) _use_gnss_mode = conf["use_gnss_mode"].as(); subscribe_imu_topic_name = conf["imu_topic"].as(); - subscribe_twist_topic_name = conf["twist_topic"].as(); - subscribe_rtklib_nav_topic_name = conf["rtklib_nav_topic"].as(); _velocity_scale_factor_parameter.imu_rate = conf["common"]["imu_rate"].as(); _velocity_scale_factor_parameter.gnss_rate = conf["common"]["gnss_rate"].as(); diff --git a/eagleye_rt/src/yawrate_offset_stop_node.cpp b/eagleye_rt/src/yawrate_offset_stop_node.cpp index eb8ac503..d7e0fa8f 100644 --- a/eagleye_rt/src/yawrate_offset_stop_node.cpp +++ b/eagleye_rt/src/yawrate_offset_stop_node.cpp @@ -72,7 +72,7 @@ int main(int argc, char** argv) ros::init(argc, argv, "yawrate_offset_stop"); ros::NodeHandle nh; - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string yaml_file; nh.getParam("yaml_file",yaml_file); @@ -82,8 +82,6 @@ int main(int argc, char** argv) { YAML::Node conf = YAML::LoadFile(yaml_file); - subscribe_twist_topic_name = conf["twist_topic"].as(); - _yawrate_offset_stop_parameter.imu_rate = conf["common"]["imu_rate"].as(); _yawrate_offset_stop_parameter.stop_judgment_threshold = conf["common"]["stop_judgment_threshold"].as(); diff --git a/eagleye_util/fix2pose/CMakeLists.txt b/eagleye_util/fix2pose/CMakeLists.txt index fc2ec47a..21cb28ed 100644 --- a/eagleye_util/fix2pose/CMakeLists.txt +++ b/eagleye_util/fix2pose/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS tf tf2_ros eagleye_coordinate + llh_converter ) catkin_package( @@ -18,6 +19,7 @@ catkin_package( rospy std_msgs eagleye_coordinate + llh_converter ) include_directories( diff --git a/eagleye_util/fix2pose/launch/fix2pose.launch b/eagleye_util/fix2pose/launch/fix2pose.launch index 61c20f76..85363639 100644 --- a/eagleye_util/fix2pose/launch/fix2pose.launch +++ b/eagleye_util/fix2pose/launch/fix2pose.launch @@ -10,10 +10,20 @@ + + + + + + + + + + @@ -27,6 +37,9 @@ + + + diff --git a/eagleye_util/fix2pose/package.xml b/eagleye_util/fix2pose/package.xml index a2adbc36..fe9b63c1 100644 --- a/eagleye_util/fix2pose/package.xml +++ b/eagleye_util/fix2pose/package.xml @@ -36,6 +36,7 @@ tf eagleye_coordinate tf2_ros + llh_converter diff --git a/eagleye_util/fix2pose/src/fix2pose.cpp b/eagleye_util/fix2pose/src/fix2pose.cpp index b07fe2f7..471de860 100644 --- a/eagleye_util/fix2pose/src/fix2pose.cpp +++ b/eagleye_util/fix2pose/src/fix2pose.cpp @@ -38,6 +38,7 @@ #include "eagleye_msgs/Position.h" #include "tf/transform_broadcaster.h" #include "coordinate/coordinate.hpp" +#include "llh_converter/llh_converter.hpp" #include #include @@ -52,14 +53,15 @@ static ros::Publisher _pose_pub, _pose_with_covariance_pub; static geometry_msgs::PoseStamped _pose; static geometry_msgs::PoseWithCovarianceStamped _pose_with_covariance; -static int _convert_height_num = 0; -static int _plane = 7; -static int _tf_num = 1; static std::string _parent_frame_id, _child_frame_id, _base_link_frame_id, _gnss_frame_id; -static ConvertHeight _convert_height; +llh_converter::LLHConverter _lc; +llh_converter::LLHParam _llh_param; bool _fix_only_publish = false; +int _fix_judgement_type = 0; +int _fix_gnss_status = 0; +double _fix_std_pos_thres = 0.1; // [m] void heading_callback(const eagleye_msgs::Heading::ConstPtr& msg) { @@ -78,7 +80,22 @@ void pitching_callback(const eagleye_msgs::Pitching::ConstPtr& msg) void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg, tf2_ros::TransformListener* tf_listener, tf2_ros::Buffer* tf_buffer) { - if(_fix_only_publish && msg->status.status != 0) + bool fix_flag = false; + if(_fix_judgement_type == 0) + { + if(msg->status.status == 0 && _eagleye_heading_ptr->status.enabled_status) fix_flag = true; + } + else if(_fix_judgement_type == 1) + { + if(msg->position_covariance[0] < _fix_std_pos_thres * _fix_std_pos_thres && _eagleye_heading_ptr->status.enabled_status) fix_flag = true; + } + else + { + ROS_ERROR("fix_judgement_type is not valid"); + ros::shutdown(); + } + + if(_fix_only_publish && !fix_flag) { return; } @@ -91,32 +108,14 @@ void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg, tf2_ros::Transfor llh[1] = msg->longitude* M_PI / 180; llh[2] = msg->altitude; - if (_convert_height_num == 1) - { - _convert_height.setLLH(msg->latitude, msg->longitude, msg->altitude); - llh[2] = _convert_height.convert2altitude(); - } - else if(_convert_height_num == 2) - { - _convert_height.setLLH(msg->latitude, msg->longitude, msg->altitude); - llh[2] = _convert_height.convert2ellipsoid(); - } - - if (_tf_num == 1) - { - ll2xy(_plane, llh, xyz); - } - else if (_tf_num == 2) - { - ll2xy_mgrs(llh, xyz); - } + _lc.convertRad2XYZ(llh[0], llh[1], llh[2], xyz[0], xyz[1], xyz[2], _llh_param); double eagleye_heading = 0; if (_eagleye_heading_ptr != nullptr) { - eagleye_heading = fmod(_eagleye_heading_ptr->heading_angle, 2*M_PI); + eagleye_heading = fmod((90* M_PI / 180) - _eagleye_heading_ptr->heading_angle, 2*M_PI); tf::Quaternion tf_quat = tf::createQuaternionFromRPY(_eagleye_rolling.rolling_angle, - _eagleye_pitching.pitching_angle, (90* M_PI / 180) - eagleye_heading); + _eagleye_pitching.pitching_angle, eagleye_heading); quaternionTFToMsg(tf_quat, _quat); } else @@ -164,7 +163,7 @@ void fix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg, tf2_ros::Transfor double std_dev_yaw = 100; // [rad] if(_eagleye_rolling.status.enabled_status) std_dev_roll = 0.5 / 180 * M_PI; if(_eagleye_pitching.status.enabled_status) std_dev_pitch = 0.5 / 180 * M_PI; - if(_eagleye_heading_ptr != nullptr && _eagleye_heading_ptr->status.enabled_status) std_dev_yaw = 0.2 / 180 * M_PI; + if(_eagleye_heading_ptr != nullptr && _eagleye_heading_ptr->status.enabled_status) std_dev_yaw = std::sqrt(_eagleye_heading_ptr->variance); _pose_with_covariance.pose.covariance[0] = msg->position_covariance[0]; _pose_with_covariance.pose.covariance[7] = msg->position_covariance[4]; _pose_with_covariance.pose.covariance[14] = msg->position_covariance[8]; @@ -188,25 +187,85 @@ int main(int argc, char** argv) ros::init(argc, argv, "fix2pose"); ros::NodeHandle nh; - nh.getParam("fix2pose_node/plane", _plane); - nh.getParam("fix2pose_node/plane", _plane); - nh.getParam("fix2pose_node/tf_num", _tf_num); - nh.getParam("fix2pose_node/convert_height_num", _convert_height_num); + int plane = 7; + int tf_num = 1; + int convert_height_num = 2; + int geoid_type = 0; + + nh.getParam("fix2pose_node/plane", plane); + nh.getParam("fix2pose_node/tf_num", tf_num); + nh.getParam("fix2pose_node/convert_height_num", convert_height_num); + nh.getParam("fix2pose_node/geoid_type", geoid_type); nh.getParam("fix2pose_node/parent_frame_id", _parent_frame_id); nh.getParam("fix2pose_node/child_frame_id", _child_frame_id); nh.getParam("fix2pose_node/fix_only_publish", _fix_only_publish); + nh.getParam("fix2pose_node/fix_judgement_type", _fix_judgement_type); + nh.getParam("fix2pose_node/fix_gnss_status", _fix_gnss_status); + nh.getParam("fix2pose_node/_fix_std_pos_thres", _fix_std_pos_thres); nh.getParam("fix2pose_node/base_link_frame_id", _base_link_frame_id); nh.getParam("fix2pose_node/gnss_frame_id", _gnss_frame_id); - std::cout<< "plane " << _plane << std::endl; - std::cout<< "tf_num " << _tf_num << std::endl; - std::cout<< "convert_height_num " << _convert_height_num << std::endl; + std::cout<< "plane " << plane << std::endl; + std::cout<< "tf_num " << tf_num << std::endl; + std::cout<< "convert_height_num " << convert_height_num << std::endl; + std::cout<< "geoid_type " << geoid_type << std::endl; std::cout<< "parent_frame_id " << _parent_frame_id << std::endl; std::cout<< "child_frame_id " << _child_frame_id << std::endl; std::cout<< "fix_only_publish " << _fix_only_publish << std::endl; + std::cout<< "fix_judgement_type " << _fix_judgement_type << std::endl; + std::cout<< "fix_gnss_status " << _fix_gnss_status << std::endl; + std::cout<< "fix_std_pos_thres " << _fix_std_pos_thres << std::endl; std::cout<< "base_link_frame_id " << _base_link_frame_id << std::endl; std::cout<< "gnss_frame_id " << _gnss_frame_id << std::endl; + + if (tf_num == 1) + { + _llh_param.use_mgrs = false; + _llh_param.plane_num = plane; + } + else if (tf_num == 2) + { + _llh_param.use_mgrs = true; + } + else + { + ROS_ERROR("tf_num is not valid"); + ros::shutdown(); + } + + if (convert_height_num == 0) + { + ROS_INFO("convert_height_num is 0(no convert)"); + } + else if (convert_height_num == 1) + { + _llh_param.height_convert_type = llh_converter::ConvertType::ELLIPS2ORTHO; + } + else if (convert_height_num == 2) + { + _llh_param.height_convert_type = llh_converter::ConvertType::ORTHO2ELLIPS; + } + else + { + ROS_ERROR("convert_height_num is not valid"); + ros::shutdown(); + } + + if(geoid_type == 0) + { + _llh_param.geoid_type = llh_converter::GeoidType::EGM2008; + } + else if(geoid_type == 1) + { + _llh_param.geoid_type = llh_converter::GeoidType::GSIGEO2011; + } + else + { + ROS_ERROR("GeoidType is not valid"); + ros::shutdown(); + } + std::string fix_name; if(!_fix_only_publish) { diff --git a/eagleye_util/nmea2fix/CMakeLists.txt b/eagleye_util/gnss_converter/CMakeLists.txt similarity index 57% rename from eagleye_util/nmea2fix/CMakeLists.txt rename to eagleye_util/gnss_converter/CMakeLists.txt index 5762a1d8..e58af6b6 100644 --- a/eagleye_util/nmea2fix/CMakeLists.txt +++ b/eagleye_util/gnss_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(eagleye_nmea2fix) +project(eagleye_gnss_converter) set(CMAKE_CXX_FLAGS "-O2 -std=c++11 -Wall") @@ -7,11 +7,15 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs nmea_msgs + rtklib_msgs + geometry_msgs + ublox_msgs + eagleye_coordinate ) catkin_package( INCLUDE_DIRS include - LIBRARIES nmea2fix + LIBRARIES gnss_converter ) include_directories( @@ -20,35 +24,35 @@ include_directories( ${catkin_INCLUDE_DIRS} ) -add_library(nmea2fix +add_library(gnss_converter src/nmea2fix_core.cpp ) -install(DIRECTORY include/nmea2fix/ - DESTINATION include/nmea2fix/ +install(DIRECTORY include/gnss_converter/ + DESTINATION include/gnss_converter/ FILES_MATCHING PATTERN "*.hpp" ) -target_link_libraries(nmea2fix +target_link_libraries(gnss_converter ${catkin_LIBRARIES} ) -install(TARGETS nmea2fix +install(TARGETS gnss_converter ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) find_package(catkin REQUIRED COMPONENTS - eagleye_nmea2fix + eagleye_gnss_converter ) -add_executable(nmea2fix_node src/nmea2fix_node.cpp) -target_link_libraries(nmea2fix_node ${catkin_LIBRARIES}) -add_dependencies(nmea2fix_node ${catkin_EXPORTED_TARGETS}) +add_executable(gnss_converter_node src/gnss_converter_node.cpp) +target_link_libraries(gnss_converter_node ${catkin_LIBRARIES}) +add_dependencies(gnss_converter_node ${catkin_EXPORTED_TARGETS}) install(TARGETS - nmea2fix_node + gnss_converter_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/eagleye_util/nmea2fix/README.md b/eagleye_util/gnss_converter/README.md similarity index 100% rename from eagleye_util/nmea2fix/README.md rename to eagleye_util/gnss_converter/README.md diff --git a/eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp b/eagleye_util/gnss_converter/include/gnss_converter/nmea2fix.hpp similarity index 100% rename from eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp rename to eagleye_util/gnss_converter/include/gnss_converter/nmea2fix.hpp diff --git a/eagleye_util/gnss_converter/launch/gnss_converter.launch b/eagleye_util/gnss_converter/launch/gnss_converter.launch new file mode 100644 index 00000000..669ea471 --- /dev/null +++ b/eagleye_util/gnss_converter/launch/gnss_converter.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/eagleye_util/nmea2fix/launch/nmea2fix_dualantenna.launch b/eagleye_util/gnss_converter/launch/nmea2fix_dualantenna.launch similarity index 100% rename from eagleye_util/nmea2fix/launch/nmea2fix_dualantenna.launch rename to eagleye_util/gnss_converter/launch/nmea2fix_dualantenna.launch diff --git a/eagleye_util/nmea2fix/package.xml b/eagleye_util/gnss_converter/package.xml similarity index 74% rename from eagleye_util/nmea2fix/package.xml rename to eagleye_util/gnss_converter/package.xml index c5842fb9..47d1cbb3 100644 --- a/eagleye_util/nmea2fix/package.xml +++ b/eagleye_util/gnss_converter/package.xml @@ -1,8 +1,8 @@ - eagleye_nmea2fix + eagleye_gnss_converter 1.0.0 - nmea2fix package + gnss_converter package OsamuSekino @@ -18,6 +18,10 @@ roscpp sensor_msgs nmea_msgs + rtklib_msgs + geometry_msgs + ublox_msgs + eagleye_coordinate diff --git a/eagleye_util/gnss_converter/src/gnss_converter_node.cpp b/eagleye_util/gnss_converter/src/gnss_converter_node.cpp new file mode 100644 index 00000000..98785530 --- /dev/null +++ b/eagleye_util/gnss_converter/src/gnss_converter_node.cpp @@ -0,0 +1,178 @@ + +#include "ros/ros.h" +#include "gnss_converter/nmea2fix.hpp" +#include "rtklib_msgs/RtklibNav.h" +#include "geometry_msgs/TwistWithCovarianceStamped.h" +#include +#include + +static ros::Publisher pub1,pub2, pub3, pub4, pub5; +static nmea_msgs::Sentence sentence; +sensor_msgs::NavSatFix::ConstPtr nav_msg_ptr; + +static std::string sub_topic_name,pub_fix_topic_name,pub_gga_topic_name, pub_rmc_topic_name; +static bool output_gga, output_rmc; + +void nmea_callback(const nmea_msgs::Sentence::ConstPtr &msg) +{ + nmea_msgs::Gpgga gga; + nmea_msgs::Gprmc rmc; + sensor_msgs::NavSatFix fix; + + sentence.header = msg->header; + sentence.sentence = msg->sentence; + nmea2fix_converter(sentence, &fix, &gga, &rmc); + if (fix.header.stamp.toSec() != 0) + { + gga.header.frame_id = fix.header.frame_id = "gnss"; + pub1.publish(fix); + pub2.publish(gga); + } + if (rmc.header.stamp.toSec() != 0) + { + rmc.header.frame_id = "gnss"; + pub3.publish(rmc); + } +} + +void rtklib_nav_callback(const rtklib_msgs::RtklibNav::ConstPtr &msg) { + pub4.publish(*msg);; +} + +void navsatfix_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) { nav_msg_ptr = msg; } + +void navpvt_callback(const ublox_msgs::NavPVT::ConstPtr& msg) +{ + rtklib_msgs::RtklibNav r; + r.header.frame_id = "gps"; + r.header.stamp.sec = msg->sec; + r.header.stamp.nsec = msg->nano; + if (nav_msg_ptr != nullptr) + r.status = *nav_msg_ptr; + r.tow = msg->iTOW; + + double llh[3]; + llh[0] = msg->lat * 1e-7 * M_PI / 180.0; // [deg / 1e-7]->[rad] + llh[1] = msg->lon * 1e-7 * M_PI / 180.0; // [deg / 1e-7]->[rad] + llh[2] = msg->height * 1e-3; // [mm]->[m] + double ecef_pos[3]; + llh2xyz(llh, ecef_pos); + + double enu_vel[3] = {msg->velE * 1e-3, msg->velN * 1e-3, -msg->velD * 1e-3}; + double ecef_vel[3]; + enu2xyz_vel(enu_vel, ecef_pos, ecef_vel); + + r.ecef_pos.x = ecef_pos[0]; + r.ecef_pos.y = ecef_pos[1]; + r.ecef_pos.z = ecef_pos[2]; + r.ecef_vel.x = ecef_vel[0]; + r.ecef_vel.y = ecef_vel[1]; + r.ecef_vel.z = ecef_vel[2]; + + pub5.publish(r); +} + +void gnss_velocity_callback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg) +{ + if (nav_msg_ptr == nullptr) return; + rtklib_msgs::RtklibNav r; + r.header.frame_id = "gps"; + r.header.stamp = msg->header.stamp; + double gnss_velocity_time = msg->header.stamp.toSec(); + r.tow = gnss_velocity_time; + + double llh[3]; + llh[0] = nav_msg_ptr->latitude * M_PI / 180.0; + llh[1] = nav_msg_ptr->longitude * M_PI / 180.0; + llh[2] = nav_msg_ptr->altitude; + double ecef_pos[3]; + llh2xyz(llh, ecef_pos); + r.ecef_pos.x = ecef_pos[0]; + r.ecef_pos.y = ecef_pos[1]; + r.ecef_pos.z = ecef_pos[2]; + + r.ecef_vel.x = msg->twist.twist.linear.x; + r.ecef_vel.y = msg->twist.twist.linear.y; + r.ecef_vel.z = msg->twist.twist.linear.z; + + pub5.publish(r); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gnss_converter_node"); + ros::NodeHandle n; + + std::string use_gnss_mode; + + std::string node_name = ros::this_node::getName(); + + int velocity_source_type = 0; + // rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, ublox_msgs/NavPVT: 2, geometry_msgs/TwistWithCovarianceStamped: 3 + std::string velocity_source_topic; + int llh_source_type = 0; // rtklib_msgs/RtklibNav: 0, nmea_msgs/Sentence: 1, sensor_msgs/NavSatFix: 2 + std::string llh_source_topic; + + n.getParam("gnss/velocity_source_type",velocity_source_type); + // n.getParam("gnss/velocity_source_topic",velocity_source_topic); + n.getParam("gnss/velocity_source_topic",velocity_source_topic); + n.getParam("gnss/llh_source_type",llh_source_type); + n.getParam("gnss/llh_source_topic",llh_source_topic); + + std::cout<< "velocity_source_type "<("fix", 1000); + pub2 = n.advertise("gga", 1000); + pub3 = n.advertise("rmc", 1000); + pub4 = n.advertise("rtklib_nav", 1000); + + ros::spin(); + + return 0; +} diff --git a/eagleye_util/nmea2fix/src/nmea2fix_core.cpp b/eagleye_util/gnss_converter/src/nmea2fix_core.cpp similarity index 99% rename from eagleye_util/nmea2fix/src/nmea2fix_core.cpp rename to eagleye_util/gnss_converter/src/nmea2fix_core.cpp index 20a78a33..5c693fc9 100644 --- a/eagleye_util/nmea2fix/src/nmea2fix_core.cpp +++ b/eagleye_util/gnss_converter/src/nmea2fix_core.cpp @@ -23,7 +23,7 @@ // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#include "nmea2fix/nmea2fix.hpp" +#include "gnss_converter/nmea2fix.hpp" double stringToGPSTime(std::string& input, double header_time) { diff --git a/eagleye_util/kml_generator b/eagleye_util/kml_generator index 4f765529..2edbdef0 160000 --- a/eagleye_util/kml_generator +++ b/eagleye_util/kml_generator @@ -1 +1 @@ -Subproject commit 4f76552986bee00c0a3456c5010187681dada7eb +Subproject commit 2edbdef0a98415ad8f27f9c6c8690b81b7954fa3 diff --git a/eagleye_util/llh_converter b/eagleye_util/llh_converter new file mode 160000 index 00000000..72d335fc --- /dev/null +++ b/eagleye_util/llh_converter @@ -0,0 +1 @@ +Subproject commit 72d335fc62f550480717f7676ed9fe4a95e7cfe3 diff --git a/eagleye_util/nmea2fix/launch/nmea2fix.launch b/eagleye_util/nmea2fix/launch/nmea2fix.launch deleted file mode 100644 index 25910e18..00000000 --- a/eagleye_util/nmea2fix/launch/nmea2fix.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/eagleye_util/nmea2fix/src/nmea2fix_node.cpp b/eagleye_util/nmea2fix/src/nmea2fix_node.cpp deleted file mode 100644 index a92e341a..00000000 --- a/eagleye_util/nmea2fix/src/nmea2fix_node.cpp +++ /dev/null @@ -1,75 +0,0 @@ - -#include "ros/ros.h" -#include "nmea2fix/nmea2fix.hpp" - -static ros::Publisher pub1,pub2, pub3; -static nmea_msgs::Sentence sentence; - -static std::string sub_topic_name,pub_fix_topic_name,pub_gga_topic_name, pub_rmc_topic_name; -static bool output_gga, output_rmc; - -void nmea_callback(const nmea_msgs::Sentence::ConstPtr &msg) -{ - nmea_msgs::Gpgga gga; - nmea_msgs::Gprmc rmc; - sensor_msgs::NavSatFix fix; - - sentence.header = msg->header; - sentence.sentence = msg->sentence; - nmea2fix_converter(sentence, &fix, &gga, &rmc); - if (fix.header.stamp.toSec() != 0) - { - gga.header.frame_id = fix.header.frame_id = "gnss"; - pub1.publish(fix); - if(output_gga) pub2.publish(gga); - } - if (rmc.header.stamp.toSec() != 0 && output_rmc) - { - rmc.header.frame_id = "gnss"; - pub3.publish(rmc); - } -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "nmea2fix_node"); - ros::NodeHandle n; - - std::string use_gnss_mode; - - std::string node_name = ros::this_node::getName(); - - if(!n.getParam("nmea_sentence_topic",sub_topic_name)) - { - n.getParam(node_name + "/nmea_sentence_topic",sub_topic_name); - } - n.getParam(node_name + "/pub_fix_topic_name",pub_fix_topic_name); - n.getParam(node_name + "/pub_gga_topic_name",pub_gga_topic_name); - n.getParam(node_name + "/pub_rmc_topic_name",pub_rmc_topic_name); - n.getParam(node_name + "/output_gga",output_gga); - n.getParam(node_name + "/output_rmc",output_rmc); - n.getParam("eagleye/use_gnss_mode",use_gnss_mode); - - if (use_gnss_mode == "nmea" || use_gnss_mode == "NMEA") // use NMEA mode - { - output_gga = true; - output_rmc = true; - } - - std::cout<< "sub_topic_name "<(pub_fix_topic_name, 1000); - if(output_gga) pub2 = n.advertise(pub_gga_topic_name, 1000); - if(output_rmc) pub3 = n.advertise(pub_rmc_topic_name, 1000); - - ros::spin(); - - return 0; -} diff --git a/eagleye_util/trajectory_plot/config/trajectory_plot.yaml b/eagleye_util/trajectory_plot/config/trajectory_plot.yaml index c64d847c..aa4d4c73 100644 --- a/eagleye_util/trajectory_plot/config/trajectory_plot.yaml +++ b/eagleye_util/trajectory_plot/config/trajectory_plot.yaml @@ -63,7 +63,8 @@ twist_index: eagleye_log: tf_num: 0 # 0:plane, 1:mgrs - time_unit: 1 # 0:[s], 1:[ns] + time_unit: 0 # 0:[s], 1:[ns] + missing_gnss_type: 0 # 0:use all type, 1:rtklib_nav, 2:nmea ros_reverse_imu: False # [True]:Use reverse_imu in ros param: @@ -83,6 +84,7 @@ param: eval_step_max_param: 3.0 # Maximum value of error to be evaluated default (default:3.0 = 3.0[m]) ref_data_name_param: 'ref data' # Reference Legend Name. (default:'ref data') data_name_param: 'eagleye' # Target Legend Name. (default:'eagleye') + font_size_param: 12 # label size evaluation_plot: dr_error_ylim: 3.0 # ylabel data lim diff --git a/eagleye_util/trajectory_plot/scripts/eagleye_pp_single_evaluation.py b/eagleye_util/trajectory_plot/scripts/eagleye_pp_single_evaluation.py index 91c931e3..3621f6ea 100755 --- a/eagleye_util/trajectory_plot/scripts/eagleye_pp_single_evaluation.py +++ b/eagleye_util/trajectory_plot/scripts/eagleye_pp_single_evaluation.py @@ -52,9 +52,13 @@ config = yaml.safe_load(yml) # set param + missing_gnss_type = config["eagleye_log"]["missing_gnss_type"] reverse_imu = config["param"]["reverse_imu_flag"] plane = config["param"]["plane_num"] + plot_text_data = config["evaluation_plot"]["plot_text_data"] + plot_text_step = config["evaluation_plot"]["plot_text_step"] data_name = config["param"]["data_name_param"] + font_size = config["param"]["font_size_param"] print('plane',plane) @@ -85,11 +89,27 @@ dopplor = pd.concat([raw_df['elapsed_time'],vel, dopplor_heading],axis=1) eagleye_plot_rpy = pd.concat([eagleye_df['roll'],eagleye_df['pitch'],eagleye_df['yaw']],axis=1) - util_plot.plot_6DoF_single(eagleye_df['elapsed_time'],raw_df['elapsed_time'],raw_df['elapsed_time'], eagleye_xyz, rtk_xyz, raw_xyz, eagleye_plot_rpy,dopplor) + eagleye_6dof = pd.concat([eagleye_xyz,eagleye_plot_rpy],axis=1) + raw_6dof = pd.concat([raw_xyz,dopplor_heading],axis=1) + + if missing_gnss_type == 0: + util_plot.plot_6DoF_single(eagleye_df['elapsed_time'],raw_df['elapsed_time'],raw_df['elapsed_time'], eagleye_xyz, rtk_xyz, raw_xyz, eagleye_plot_rpy,dopplor, font_size) + util_plot.plot_traj_text_tree('2D Trajectory', raw_xyz, rtk_xyz, eagleye_xyz, eagleye_df[plot_text_data], plot_text_step, font_size, data_name, "gnss rtk data(nmea)") + util_plot.plot_traj_qual(eagleye_xyz,eagleye_df['qual'], eagleye_df[plot_text_data], plot_text_step, font_size) + util_plot.plot_traj_3d_three(raw_xyz, rtk_xyz, eagleye_xyz, font_size, data_name, "gnss rtk data(nmea)") + elif missing_gnss_type == 1: + util_plot.plot_6DoF(eagleye_df['elapsed_time'], eagleye_6dof, rtk_xyz , data_name, "gnss rtk data(nmea)",font_size) + util_plot.plot_traj_text('2D Trajectory', rtk_xyz, eagleye_xyz, eagleye_df[plot_text_data], plot_text_step, font_size, data_name, "gnss rtk data(nmea)") + util_plot.plot_traj_qual(eagleye_xyz,eagleye_df['qual'], eagleye_df[plot_text_data], plot_text_step, font_size) + util_plot.plot_traj_3d(rtk_xyz, eagleye_xyz, font_size, data_name, "gnss rtk data(nmea)") + elif missing_gnss_type == 2: + util_plot.plot_6DoF(eagleye_df['elapsed_time'], eagleye_6dof, raw_6dof , data_name, "gnss raw data(rtklib)",font_size) + util_plot.plot_traj_text('2D Trajectory', rtk_xyz, eagleye_xyz, eagleye_df[plot_text_data], plot_text_step, font_size, data_name, "gnss raw data(rtklib)") + util_plot.plot_traj_3d(raw_xyz, eagleye_xyz, font_size, data_name, "gnss raw data(rtklib)") fig2 = plt.figure() ax_sf = fig2.add_subplot(2, 1, 1) - util_plot.plot_one(ax_sf, eagleye_df, 'elapsed_time', 'sf', 'Velocity scal factor', 'Time [s]', 'Velocity scal factor []', "None", 1) + util_plot.plot_one(ax_sf, eagleye_df, 'elapsed_time', 'sf', 'Velocity scal factor', 'Time [s]', 'Velocity scal factor []', "None", 1, font_size) ax_vel = fig2.add_subplot(2, 1, 2) ax_vel.set_title('Velocity') @@ -101,11 +121,5 @@ ax_vel.legend(loc='upper right') ax_vel.grid() - util_plot.plot_traj_three(raw_xyz, rtk_xyz, eagleye_xyz, data_name, "gnss rtk data(nmea)") - - util_plot.plot_traj_qual(eagleye_xyz,eagleye_df['qual']) - - util_plot.plot_traj_3d_three(raw_xyz, rtk_xyz, eagleye_xyz, data_name, "gnss rtk data(nmea)") - plt.show() \ No newline at end of file diff --git a/eagleye_util/trajectory_plot/scripts/evaluation_plot.py b/eagleye_util/trajectory_plot/scripts/evaluation_plot.py index 4111cae9..890b387b 100644 --- a/eagleye_util/trajectory_plot/scripts/evaluation_plot.py +++ b/eagleye_util/trajectory_plot/scripts/evaluation_plot.py @@ -74,6 +74,7 @@ plot_text_step = config["evaluation_plot"]["plot_text_step"] ref_data_name = config["param"]["ref_data_name_param"] data_name = config["param"]["data_name_param"] + font_size = config["param"]["font_size_param"] print('plane',plane) print('sync_threshold_time',sync_threshold_time) @@ -156,7 +157,7 @@ eagleye_xyz = pd.concat([data_df['x'],data_df['y'],data_df['z']],axis=1) eaegleye_6dof = pd.concat([eagleye_xyz,eagleye_rpy],axis=1) ref_6dof = pd.concat([ref_data_xyz,ref_rpy],axis=1) - util_plot.plot_6DoF(ref_df['elapsed_time'], eaegleye_6dof, ref_6dof,data_name, ref_data_name) + util_plot.plot_6DoF(ref_df['elapsed_time'], eaegleye_6dof, ref_6dof,data_name, ref_data_name, font_size) # calc 6dof error @@ -173,9 +174,9 @@ # plot 6dof error error_plot_df = pd.concat([Error_data,error_rpy,error_velocity],axis=1) error_table = util_calc.error_evaluation(error_plot_df) - util_plot.plot_error_6DoF(error_plot_df,ref_data_name,error_table) - util_plot.plot_error(error_plot_df,ref_data_name) - util_plot.plot_error_distributiln(error_plot_df,ref_data_name) + util_plot.plot_error_6DoF(error_plot_df,ref_data_name,error_table, font_size) + util_plot.plot_error(error_plot_df,ref_data_name, font_size) + util_plot.plot_error_distributiln(error_plot_df, font_size,ref_data_name) # plot Cumulative Error Distribution diff_2d: List[float] = [] @@ -185,7 +186,7 @@ fig = plt.figure() ax1 = fig.add_subplot(1, 1, 1) - util_plot.plot_one(ax1, ErrTra_Rate, 'x_label', 'ErrTra', 'Cumulative Error Distribution', '2D error [m]', 'Rate [%]', '-', 10) + util_plot.plot_one(ax1, ErrTra_Rate, 'x_label', 'ErrTra', 'Cumulative Error Distribution', '2D error [m]', 'Rate [%]', '-', 10, font_size) ax1.set_xscale('log') ax1.get_xaxis().set_major_formatter(matplotlib.ticker.ScalarFormatter()) ax1.set_xticks([0.01, 0.05, 0.1, 0.5, 1, 3]) @@ -204,16 +205,16 @@ fig2 = plt.figure() ax_dr = fig2.add_subplot(2, 1, 1) - util_plot.plot_one(ax_dr, calc_error, 'start_distance', 'error_2d', 'relative position Error', 'start distance [m]', '2D Error [m]', '-', 1) + util_plot.plot_one(ax_dr, calc_error, 'start_distance', 'error_2d', 'relative position Error', 'start distance [m]', '2D Error [m]', '-', 1, font_size) ax_dr.set_ylim([0.0,dr_error_ylim]) ax_trarate_dr = fig2.add_subplot(2, 1, 2) - util_plot.plot_one(ax_trarate_dr, ErrTra_dr_df, 'x_label', 'ErrTra', 'Cumulative Error Distribution (relative position)', '2D error [m]', 'Rate [%]', '-', 10) + util_plot.plot_one(ax_trarate_dr, ErrTra_dr_df, 'x_label', 'ErrTra', 'Cumulative Error Distribution (relative position)', '2D error [m]', 'Rate [%]', '-', 10, font_size) ax_trarate_dr.set_xscale('log') ax_trarate_dr.get_xaxis().set_major_formatter(matplotlib.ticker.ScalarFormatter()) ax_trarate_dr.set_xticks([0.01, 0.05, 0.1, 0.5, 1, 3]) - util_plot.plot_traj_text('DR Trajectory', ref_xyz, dr_trajcetory,ref_df[plot_text_data], plot_text_step, data_name, ref_data_name) + util_plot.plot_traj_text('DR Trajectory', ref_xyz, dr_trajcetory,ref_df[plot_text_data], plot_text_step, font_size, data_name, ref_data_name) calc_error_csv = pd.concat([calc_error['start_distance'],calc_error['error_2d']],axis=1) calc_error_csv.to_csv("eagleye_dr_error.csv", header=True, index=False,float_format='%.9f') @@ -221,28 +222,28 @@ if 'velocity' in ref_df.columns and 'velocity' in data_df.columns: fig3 = plt.figure() ax_vel = fig3.add_subplot(2, 1, 1) - util_plot.plot_each(ax_vel, ref_df['elapsed_time'], data_df, ref_df, 'velocity', 'Velocity', 'Velocity [m/s]',data_name, ref_data_name) + util_plot.plot_each(ax_vel, ref_df['elapsed_time'], data_df, ref_df, 'velocity', 'Velocity', 'Velocity [m/s]',data_name, ref_data_name, font_size) ax_err_vel = fig3.add_subplot(2, 1, 2) fig3.suptitle(ref_data_name + ' - eagleye Error') - util_plot.plot_one(ax_err_vel, error_plot_df, 'elapsed_time', 'velocity', 'Velocity Error', 'time [s]', 'Velocity error[m/s]', 'None', 1) + util_plot.plot_one(ax_err_vel, error_plot_df, 'elapsed_time', 'velocity', 'Velocity Error', 'time [s]', 'Velocity error[m/s]', 'None', 1, font_size) elif 'velocity' in ref_df.columns or 'velocity' in data_df.columns: fig3 = plt.figure() ax_vel = fig3.add_subplot(1, 1, 1) - util_plot.plot_each(ax_vel, ref_df['elapsed_time'], data_df, ref_df, 'velocity', 'Velocity', 'Velocity [m/s]',data_name,ref_data_name) + util_plot.plot_each(ax_vel, ref_df['elapsed_time'], data_df, ref_df, 'velocity', 'Velocity', 'Velocity [m/s]',data_name,ref_data_name, font_size) # plot 2D trajectory - util_plot.plot_traj_text('2D Trajectory', ref_data_xyz, eagleye_xyz, ref_df[plot_text_data], plot_text_step, data_name, ref_data_name) + util_plot.plot_traj_text('2D Trajectory', ref_data_xyz, eagleye_xyz, ref_df[plot_text_data], plot_text_step, font_size, data_name, ref_data_name) if 'qual' in data_df.columns: - util_plot.plot_traj_qual(eagleye_xyz,data_df['qual']) + util_plot.plot_traj_qual(eagleye_xyz,data_df['qual'], data_df[plot_text_data], plot_text_step, font_size) elif 'qual' in ref_df.columns: - util_plot.plot_traj_qual(ref_data_xyz,ref_df['qual']) + util_plot.plot_traj_qual(ref_data_xyz,ref_df['qual'], ref_df[plot_text_data], plot_text_step, font_size) # plot 3d trajectory - util_plot.plot_traj_3d( ref_df, data_df, data_name, ref_data_name) + util_plot.plot_traj_3d( ref_df, data_df, font_size, data_name, ref_data_name) print(error_table) diff --git a/eagleye_util/trajectory_plot/scripts/twist_evaluation.py b/eagleye_util/trajectory_plot/scripts/twist_evaluation.py index cc6d4aa7..4d15f502 100644 --- a/eagleye_util/trajectory_plot/scripts/twist_evaluation.py +++ b/eagleye_util/trajectory_plot/scripts/twist_evaluation.py @@ -74,6 +74,7 @@ plot_text_step = config["twist_evaluation"]["plot_text_step"] ref_data_name = config["param"]["ref_data_name_param"] data_name = config["param"]["data_name_param"] + font_size = config["param"]["font_size_param"] print('plane',plane) print('sync_threshold_time',sync_threshold_time) @@ -172,18 +173,18 @@ fig2 = plt.figure() ax_dr = fig2.add_subplot(2, 1, 1) - util_plot.plot_one(ax_dr, dr_error, 'start_distance', 'error_2d', 'relative position Error', 'start distance [m]', '2D Error [m]', '-', 1) + util_plot.plot_one(ax_dr, dr_error, 'start_distance', 'error_2d', 'relative position Error', 'start distance [m]', '2D Error [m]', '-', 1, font_size) ax_dr.set_ylim([0.0,dr_error_ylim]) dr_ErrTra = util_calc.calc_TraRate(dr_error["error_2d"] , eval_step_max) ax_trarate_dr = fig2.add_subplot(2, 1, 2) - util_plot.plot_one(ax_trarate_dr, dr_ErrTra, 'x_label', 'ErrTra', 'Cumulative Error Distribution (relative position)', '2D error [m]', 'Rate [%]', '-', 10) + util_plot.plot_one(ax_trarate_dr, dr_ErrTra, 'x_label', 'ErrTra', 'Cumulative Error Distribution (relative position)', '2D error [m]', 'Rate [%]', '-', 10, font_size) ax_trarate_dr.set_xscale('log') ax_trarate_dr.get_xaxis().set_major_formatter(matplotlib.ticker.ScalarFormatter()) ax_trarate_dr.set_xticks([0.01, 0.05, 0.1, 0.5, 1.0, 3.0]) - util_plot.plot_traj_text('DR Trajectory', ref_xyz, dr_trajcetory,ref_df[plot_text_data], plot_text_step, data_name, ref_data_name) + util_plot.plot_traj_text('DR Trajectory', ref_xyz, dr_trajcetory,ref_df[plot_text_data], plot_text_step, font_size, data_name, ref_data_name) plt.show() diff --git a/eagleye_util/trajectory_plot/scripts/util/calc.py b/eagleye_util/trajectory_plot/scripts/util/calc.py index 95d48a9d..3556014a 100644 --- a/eagleye_util/trajectory_plot/scripts/util/calc.py +++ b/eagleye_util/trajectory_plot/scripts/util/calc.py @@ -190,196 +190,59 @@ def calc_distance_xy(xy): def sync_time(ref_data,csv_data,sync_threshold_time,leap_time): # Time synchronization sync_index = np.zeros(len(csv_data['TimeStamp'])) - first_flag_data = 0 - set_data_df: List[float] = [] - set_data_xyz: List[float] = [] - set_data_ori: List[float] = [] - set_data_rpy: List[float] = [] - set_data_velocity: List[float] = [] - set_data_vel: List[float] = [] - set_data_angular: List[float] = [] - set_data_distance: List[float] = [] - set_data_qual: List[float] = [] - set_data_yawrate_offset_stop: List[float] = [] - first_flag_ref = 0 - set_ref_data_df: List[float] = [] - set_ref_data_xyz: List[float] = [] - set_ref_data_ori: List[float] = [] - set_ref_data_rpy: List[float] = [] - set_ref_data_velocity: List[float] = [] - set_ref_data_vel: List[float] = [] - set_ref_data_angular: List[float] = [] - set_ref_data_distance: List[float] = [] - set_ref_data_qual: List[float] = [] - set_ref_yawrate_offset_stop: List[float] = [] + set_target_index: List[float] = [] + set_ref_index: List[float] = [] + + if ref_data['TimeStamp'][0] > csv_data['TimeStamp'][0]: + csv_data = csv_data[csv_data['TimeStamp'] > ref_data['TimeStamp'][0] - sync_threshold_time] + csv_data = csv_data.reset_index(drop=True) + else: + ref_data = ref_data[ref_data['TimeStamp'] > csv_data['TimeStamp'][0] - sync_threshold_time] + ref_data = ref_data.reset_index(drop=True) + + if len(csv_data) == 0 or len(ref_data) == 0: + print("[ERROR] Target time and ref time ranges do not match.") + sys.exit(1) + + if ref_data.iloc[-1]['TimeStamp'] < csv_data.iloc[-1]['TimeStamp']: + csv_data = csv_data[csv_data['TimeStamp'] < ref_data.iloc[-1]['TimeStamp'] + sync_threshold_time] + csv_data = csv_data.reset_index(drop=True) + else: + ref_data = ref_data[ref_data['TimeStamp'] < csv_data.iloc[-1]['TimeStamp'] + sync_threshold_time] + ref_data = ref_data.reset_index(drop=True) + + if len(csv_data) == 0 or len(ref_data) == 0: + print("[ERROR] Target time and ref time ranges do not match.") + sys.exit(1) + + sync_ref_time_tmp = ref_data['TimeStamp'] + + num = 0 + len_drop_num = 0 for i in range(len(csv_data)): if i == 0: continue time_tmp: List[float] = [] - time_tmp = abs(csv_data.iloc[i]['TimeStamp']-ref_data['TimeStamp'] + leap_time) - sync_index[i] = np.argmin(time_tmp) + time_tmp = csv_data.iloc[i]['TimeStamp']-sync_ref_time_tmp + leap_time + sync_index[i] = np.argmin(abs(time_tmp)) sync_time_tmp = time_tmp[sync_index[i]] if sync_time_tmp < sync_threshold_time: - num = int(sync_index[i]) - data_time_tmp = csv_data.iloc[i]['TimeStamp'] - if (first_flag_data == 0): - first_time_data = csv_data.iloc[i]['TimeStamp'] - first_flag_data = 1 - data_elapsed_time_tmp = csv_data.iloc[i]['TimeStamp'] - first_time_data - set_data_df.append([data_elapsed_time_tmp,data_time_tmp]) - if 'x' in csv_data.columns: - data_x_tmp = csv_data.iloc[i]['x'] - data_y_tmp = csv_data.iloc[i]['y'] - data_z_tmp = csv_data.iloc[i]['z'] - set_data_xyz.append([data_x_tmp,data_y_tmp,data_z_tmp]) - if 'ori_x' in csv_data.columns: - data_ori_x_tmp = csv_data.iloc[i]['ori_x'] - data_ori_y_tmp = csv_data.iloc[i]['ori_y'] - data_ori_z_tmp = csv_data.iloc[i]['ori_z'] - data_ori_w_tmp = csv_data.iloc[i]['ori_w'] - set_data_ori.append([data_ori_x_tmp,data_ori_y_tmp,data_ori_z_tmp,data_ori_w_tmp]) - if 'roll' in csv_data.columns: - data_roll_tmp = csv_data.iloc[i]['roll'] - data_pitch_tmp = csv_data.iloc[i]['pitch'] - data_yaw_tmp = csv_data.iloc[i]['yaw'] - set_data_rpy.append([data_roll_tmp,data_pitch_tmp,data_yaw_tmp]) - if 'velocity' in csv_data.columns: - velocity_tmp = csv_data.iloc[i]['velocity'] - set_data_velocity.append([velocity_tmp]) - if 'vel_x' in csv_data.columns: - data_vel_x_tmp = csv_data.iloc[i]['vel_x'] - data_vel_y_tmp = csv_data.iloc[i]['vel_y'] - data_vel_z_tmp = csv_data.iloc[i]['vel_z'] - set_data_vel.append([data_vel_x_tmp,data_vel_y_tmp,data_vel_z_tmp]) - if 'angular_x' in csv_data.columns: - data_angular_x_tmp = csv_data.iloc[i]['angular_x'] - data_angular_y_tmp = csv_data.iloc[i]['angular_y'] - data_angular_z_tmp = csv_data.iloc[i]['angular_z'] - set_data_angular.append([data_angular_x_tmp,data_angular_y_tmp,data_angular_z_tmp]) - if 'distance' in csv_data.columns: - data_distance_tmp = csv_data.iloc[i]['distance'] - set_data_distance.append([data_distance_tmp]) - if 'qual' in csv_data.columns: - data_qual_tmp = csv_data.iloc[i]['qual'] - set_data_qual.append([data_qual_tmp]) - if 'vel_x' in csv_data.columns: - data_yawrate_offset_stop_tmp = csv_data.iloc[i]['yawrate_offset_stop'] - data_yawrate_offset_tmp = csv_data.iloc[i]['yawrate_offset'] - data_slip_tmp = csv_data.iloc[i]['slip'] - set_data_yawrate_offset_stop.append([data_yawrate_offset_stop_tmp,data_yawrate_offset_tmp,data_slip_tmp]) - - ref_time_tmp = ref_data.iloc[num]['TimeStamp'] - if (first_flag_ref == 0): - first_time_ref = ref_data.iloc[num]['TimeStamp'] - first_flag_ref = 1 - ref_elapsed_time_tmp = ref_data.iloc[num]['TimeStamp'] - first_time_ref - set_ref_data_df.append([ref_elapsed_time_tmp,ref_time_tmp]) - if 'x' in ref_data.columns: - ref_x_tmp = ref_data.iloc[num]['x'] - ref_y_tmp = ref_data.iloc[num]['y'] - ref_z_tmp = ref_data.iloc[num]['z'] - set_ref_data_xyz.append([ref_x_tmp,ref_y_tmp,ref_z_tmp]) - if 'ori_x' in ref_data.columns: - ref_ori_x_tmp = ref_data.iloc[num]['ori_x'] - ref_ori_y_tmp = ref_data.iloc[num]['ori_y'] - ref_ori_z_tmp = ref_data.iloc[num]['ori_z'] - ref_ori_w_tmp = ref_data.iloc[num]['ori_w'] - set_ref_data_ori.append([ref_ori_x_tmp,ref_ori_y_tmp,ref_ori_z_tmp,ref_ori_w_tmp]) - if 'roll' in ref_data.columns: - ref_roll_tmp = ref_data.iloc[num]['roll'] - ref_pitch_tmp = ref_data.iloc[num]['pitch'] - ref_yaw_tmp = ref_data.iloc[num]['yaw'] - set_ref_data_rpy.append([ref_roll_tmp,ref_pitch_tmp,ref_yaw_tmp]) - if 'velocity' in ref_data.columns: - velocity_tmp = ref_data.iloc[num]['velocity'] - set_ref_data_velocity.append([velocity_tmp]) - if 'vel_x' in ref_data.columns: - ref_vel_x_tmp = ref_data.iloc[num]['vel_x'] - ref_vel_y_tmp = ref_data.iloc[num]['vel_y'] - ref_vel_z_tmp = ref_data.iloc[num]['vel_z'] - set_ref_data_vel.append([ref_vel_x_tmp,ref_vel_y_tmp,ref_vel_z_tmp]) - if 'angular_x' in ref_data.columns: - ref_angular_x_tmp = ref_data.iloc[num]['angular_x'] - ref_angular_y_tmp = ref_data.iloc[num]['angular_y'] - ref_angular_z_tmp = ref_data.iloc[num]['angular_z'] - set_ref_data_angular.append([ref_angular_x_tmp,ref_angular_y_tmp,ref_angular_z_tmp]) - if 'distance' in ref_data.columns: - ref_distance_tmp = ref_data.iloc[num]['distance'] - set_ref_data_distance.append([ref_distance_tmp]) - if 'qual' in ref_data.columns: - ref_qual_tmp = ref_data.iloc[num]['qual'] - set_ref_data_qual.append([ref_qual_tmp]) - if 'yawrate_offset_stop' in ref_data.columns: - ref_yareta_offset_stop_tmp = ref_data.iloc[num]['yawrate_offset_stop'] - ref_yawrate_offset_tmp = ref_data.iloc[num]['yawrate_offset'] - ref_slip_tmp = ref_data.iloc[num]['slip'] - set_ref_yawrate_offset_stop.append([ref_yareta_offset_stop_tmp,ref_yawrate_offset_tmp,ref_slip_tmp]) - - - if not set_data_df: - print("Time sync Error") - sys.exit(1) - data_xyz = pd.DataFrame() - data_ori = pd.DataFrame() - data_rpy = pd.DataFrame() - data_velocity = pd.DataFrame() - data_vel = pd.DataFrame() - data_angular = pd.DataFrame() - data_distance = pd.DataFrame() - data_qual = pd.DataFrame() - data_yawrate_offset_stop = pd.DataFrame() - data_df = pd.DataFrame(set_data_df,columns=['elapsed_time','TimeStamp']) - if 'x' in csv_data.columns: - data_xyz = pd.DataFrame(set_data_xyz,columns=['x', 'y', 'z']) - if 'ori_x' in csv_data.columns: - data_ori = pd.DataFrame(set_data_ori,columns=['ori_x', 'ori_y', 'ori_z', 'ori_w']) - if 'roll' in csv_data.columns: - data_rpy = pd.DataFrame(set_data_rpy,columns=['roll', 'pitch', 'yaw']) - if 'velocity' in csv_data.columns: - data_velocity = pd.DataFrame(set_data_velocity,columns=['velocity']) - if 'vel_x' in csv_data.columns: - data_vel = pd.DataFrame(set_data_vel,columns=['vel_x', 'vel_y', 'vel_z']) - if 'angular_x' in csv_data.columns: - data_angular = pd.DataFrame(set_data_angular,columns=['angular_x', 'angular_y', 'angular_z']) - if 'distance' in csv_data.columns: - data_distance = pd.DataFrame(set_data_distance,columns=['distance']) - if 'qual' in csv_data.columns: - data_qual = pd.DataFrame(set_data_qual,columns=['qual']) - if 'yawrate_offset_stop' in csv_data.columns: - data_yawrate_offset_stop = pd.DataFrame(set_data_yawrate_offset_stop,columns=['yawrate_offset_stop','yawrate_offset','slip']) - data_df_output = pd.concat([data_df,data_xyz,data_ori,data_rpy,data_velocity,data_vel,data_angular,data_distance,data_qual,data_yawrate_offset_stop],axis=1) - - ref_xyz = pd.DataFrame() - ref_ori = pd.DataFrame() - ref_rpy = pd.DataFrame() - ref_velocity = pd.DataFrame() - ref_vel = pd.DataFrame() - ref_angular = pd.DataFrame() - ref_distance = pd.DataFrame() - ref_qual = pd.DataFrame() - ref_yawrate_offset_stop = pd.DataFrame() - ref_qual = pd.DataFrame() - ref_qual = pd.DataFrame() - ref_df = pd.DataFrame(set_ref_data_df,columns=['elapsed_time','TimeStamp']) - if 'x' in ref_data.columns: - ref_xyz = pd.DataFrame(set_ref_data_xyz,columns=['x', 'y', 'z']) - if 'ori_x' in ref_data.columns: - ref_ori = pd.DataFrame(set_ref_data_ori,columns=['ori_x', 'ori_y', 'ori_z', 'ori_w']) - if 'roll' in ref_data.columns: - ref_rpy = pd.DataFrame(set_ref_data_rpy,columns=['roll', 'pitch', 'yaw']) - if 'velocity' in ref_data.columns: - ref_velocity = pd.DataFrame(set_ref_data_velocity,columns=['velocity']) - if 'vel_x' in ref_data.columns: - ref_vel = pd.DataFrame(set_ref_data_vel,columns=['vel_x', 'vel_y', 'vel_z']) - if 'angular_x' in ref_data.columns: - ref_angular = pd.DataFrame(set_ref_data_angular,columns=['angular_x', 'angular_y', 'angular_z']) - if 'distance' in ref_data.columns: - ref_distance = pd.DataFrame(set_ref_data_distance,columns=['distance']) - if 'qual' in ref_data.columns: - ref_qual = pd.DataFrame(set_ref_data_qual,columns=['qual']) - if 'yawrate_offset_stop' in ref_data.columns: - ref_yawrate_offset_stop = pd.DataFrame(set_ref_yawrate_offset_stop,columns=['yawrate_offset_stop','qual','qual']) - - ref_df_output = pd.concat([ref_df,ref_xyz,ref_ori,ref_rpy,ref_velocity,ref_vel,ref_angular,ref_distance,ref_qual,ref_yawrate_offset_stop],axis=1) + tmp_num = int(sync_index[i]) + set_target_index.append(i) + num = num + len_drop_num + drop_num = list(range(0,tmp_num - len_drop_num - 1,1)) + if not drop_num == None: + sync_ref_time_tmp = sync_ref_time_tmp.drop(drop_num, axis=0) + len_drop_num = len(drop_num) + sync_ref_time_tmp = sync_ref_time_tmp.reset_index(drop=True) + set_ref_index.append(num) + + data_df_output = csv_data.iloc[set_target_index] + data_df_output = data_df_output.reset_index(drop=True) + data_df_output['elapsed_time'] = data_df_output['TimeStamp'] - data_df_output['TimeStamp'][0] + ref_df_output = ref_data.iloc[set_ref_index] + ref_df_output = ref_df_output.reset_index(drop=True) + ref_df_output['elapsed_time'] = ref_df_output['TimeStamp'] - ref_df_output['TimeStamp'][0] + return ref_df_output , data_df_output def calc_error_xyz(elapsed_time,ref_time,eagleye_time,ref_xyz,data_xyz,ref_yaw): diff --git a/eagleye_util/trajectory_plot/scripts/util/plot.py b/eagleye_util/trajectory_plot/scripts/util/plot.py index 443d5f4e..624dfa57 100644 --- a/eagleye_util/trajectory_plot/scripts/util/plot.py +++ b/eagleye_util/trajectory_plot/scripts/util/plot.py @@ -32,6 +32,7 @@ from math import log10 , floor import matplotlib.pyplot as plt +# plt.rcParams["font.size"] = 18 def judge_qual(xyz,qual,qual_num): set_output_df: List[float] = [] @@ -45,42 +46,45 @@ def judge_qual(xyz,qual,qual_num): output_df = pd.DataFrame(set_output_df,columns=['x','y','z','qual']) return output_df -def plot_xyz(ax, eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, elem, title, y_label): +def plot_xyz(ax, eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, elem, title, y_label, font_size): if elem in raw_xyz.columns: ax.plot(raw_x_data , raw_xyz[elem] , marker=".", linestyle="None",markersize=1, color = "red", label="gnss raw data(rtklib)") if elem in rtk_xyz.columns: ax.plot(rtk_x_data , rtk_xyz[elem] , marker="o", linestyle="None",markersize=1, color = "green", label="gnss rtk data(nmea)") if elem in eagleye_xyz.columns: ax.plot(eagleye_x_data , eagleye_xyz[elem] , marker="s", linestyle="None",markersize=1, alpha=0.3, color = "blue", label="eagleye") - ax.set_xlabel('time [s]') - ax.set_ylabel(y_label) + ax.set_xlabel('time [s]', fontsize=font_size) + ax.set_ylabel(y_label, fontsize=font_size) ax.set_title(title) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() -def plot_rpy(ax, x_data, eagleye_plot_rpy, dopplor, elem, title, y_label): +def plot_rpy(ax, x_data, eagleye_plot_rpy, dopplor, elem, title, y_label, font_size): if elem in dopplor.columns: ax.plot(x_data , dopplor[elem] , marker="o", linestyle="None",markersize=1, color = "green", label="dopplor") if elem in eagleye_plot_rpy.columns: ax.plot(x_data , eagleye_plot_rpy[elem] , marker="s", linestyle="None",markersize=1, color = "blue", label="eagleye") - ax.set_xlabel('time [s]') - ax.set_ylabel(y_label) + ax.set_xlabel('time [s]', fontsize=font_size) + ax.set_ylabel(y_label, fontsize=font_size) ax.set_title(title) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() -def plot_each(ax, x_data , eagleye, ref, y_data, title, y_label,data_name, ref_data_name): +def plot_each(ax, x_data , eagleye, ref, y_data, title, y_label,data_name, ref_data_name, font_size): if y_data in ref.columns: ax.plot(x_data, ref[y_data] , marker="o", linestyle="None",markersize=1, color = "green", label=ref_data_name) if y_data in eagleye.columns: ax.plot(x_data , eagleye[y_data] , marker="s", linestyle="None",markersize=1, alpha=0.3, color = "blue", label=data_name) - ax.set_xlabel('time [s]') - ax.set_ylabel(y_label) + ax.set_xlabel('time [s]', fontsize=font_size) + ax.set_ylabel(y_label, fontsize=font_size) ax.set_title(title) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() -def plot_6DoF_single(eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, eagleye_plot_rpy,dopplor): +def plot_6DoF_single(eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, eagleye_plot_rpy,dopplor, font_size): fig1 = plt.figure() ax_x = fig1.add_subplot(2, 3, 1) ax_y = fig1.add_subplot(2, 3, 2) @@ -88,14 +92,14 @@ def plot_6DoF_single(eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xy ax_roll = fig1.add_subplot(2, 3, 4) ax_pitch = fig1.add_subplot(2, 3, 5) ax_yaw = fig1.add_subplot(2, 3, 6) - plot_xyz(ax_x, eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, 'x', 'X (East-West)','East [m]') - plot_xyz(ax_y, eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, 'y', 'Y (North-South)','North [m]') - plot_xyz(ax_z,eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, 'z', 'Z (Height)','Height [m]') - plot_rpy(ax_roll, eagleye_x_data, eagleye_plot_rpy, dopplor, 'roll', 'Roll' , 'Roll [deg]') - plot_rpy(ax_pitch, eagleye_x_data, eagleye_plot_rpy, dopplor, 'pitch', 'Pitch', 'Pitch [deg]') - plot_rpy(ax_yaw, eagleye_x_data, eagleye_plot_rpy, dopplor, 'yaw', 'Yaw', 'Yaw [deg]') + plot_xyz(ax_x, eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, 'x', 'X (East-West)','East [m]', font_size) + plot_xyz(ax_y, eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, 'y', 'Y (North-South)','North [m]', font_size) + plot_xyz(ax_z,eagleye_x_data, rtk_x_data, raw_x_data, eagleye_xyz, rtk_xyz, raw_xyz, 'z', 'Z (Height)','Height [m]', font_size) + plot_rpy(ax_roll, eagleye_x_data, eagleye_plot_rpy, dopplor, 'roll', 'Roll' , 'Roll [deg]', font_size) + plot_rpy(ax_pitch, eagleye_x_data, eagleye_plot_rpy, dopplor, 'pitch', 'Pitch', 'Pitch [deg]', font_size) + plot_rpy(ax_yaw, eagleye_x_data, eagleye_plot_rpy, dopplor, 'yaw', 'Yaw', 'Yaw [deg]', font_size) -def plot_6DoF(x_data,eagleye, ref,data_name, ref_data_name): +def plot_6DoF(x_data,eagleye, ref,data_name, ref_data_name, font_size): fig1 = plt.figure() ax_x = fig1.add_subplot(2, 3, 1) ax_y = fig1.add_subplot(2, 3, 2) @@ -103,47 +107,51 @@ def plot_6DoF(x_data,eagleye, ref,data_name, ref_data_name): ax_roll = fig1.add_subplot(2, 3, 4) ax_pitch = fig1.add_subplot(2, 3, 5) ax_yaw = fig1.add_subplot(2, 3, 6) - plot_each(ax_x, x_data, eagleye, ref, 'x', 'X (East-West)','East [m]',data_name, ref_data_name) - plot_each(ax_y, x_data ,eagleye, ref, 'y', 'Y (North-South)','North [m]',data_name, ref_data_name) - plot_each(ax_z, x_data , eagleye, ref, 'z', 'Z (Height)','Height [m]',data_name, ref_data_name) - plot_each(ax_roll, x_data , eagleye, ref, 'roll', 'Roll' , 'Roll [deg]',data_name, ref_data_name) - plot_each(ax_pitch, x_data , eagleye, ref, 'pitch', 'Pitch', 'Pitch [deg]',data_name, ref_data_name) - plot_each(ax_yaw, x_data , eagleye, ref, 'yaw', 'Yaw', 'Yaw [deg]',data_name, ref_data_name) + plot_each(ax_x, x_data, eagleye, ref, 'x', 'X (East-West)','East [m]',data_name, ref_data_name, font_size) + plot_each(ax_y, x_data ,eagleye, ref, 'y', 'Y (North-South)','North [m]',data_name, ref_data_name, font_size) + plot_each(ax_z, x_data , eagleye, ref, 'z', 'Z (Height)','Height [m]',data_name, ref_data_name, font_size) + plot_each(ax_roll, x_data , eagleye, ref, 'roll', 'Roll' , 'Roll [deg]',data_name, ref_data_name, font_size) + plot_each(ax_pitch, x_data , eagleye, ref, 'pitch', 'Pitch', 'Pitch [deg]',data_name, ref_data_name, font_size) + plot_each(ax_yaw, x_data , eagleye, ref, 'yaw', 'Yaw', 'Yaw [deg]',data_name, ref_data_name, font_size) -def plot_each_error(ax, error_data, y_data, title, y_label): +def plot_each_error(ax, error_data, y_data, title, y_label, font_size): ax.plot(error_data['elapsed_time'] , error_data[y_data] , marker="s", linestyle="None",markersize=1, color = "blue") - ax.set_xlabel('time [s]') - ax.set_ylabel(y_label) + ax.set_xlabel('time [s]', fontsize=font_size) + ax.set_ylabel(y_label, fontsize=font_size) ax.set_title(title) + ax.tick_params(labelsize=font_size) ax.grid() -def plot_one(ax, error_data, x_data, y_data, title, x_label, y_label, line_style, marker_size): +def plot_one(ax, error_data, x_data, y_data, title, x_label, y_label, line_style, marker_size, font_size): ax.plot(error_data[x_data] , error_data[y_data] , marker="s", linestyle=line_style, markersize=marker_size, color = "blue") - ax.set_xlabel(x_label) - ax.set_ylabel(y_label) + ax.set_xlabel(x_label, fontsize=font_size) + ax.set_ylabel(y_label, fontsize=font_size) ax.set_title(title) + ax.tick_params(labelsize=font_size) ax.grid() -def plot_two(ax, ref, eagleye, x_data, y_data, title, x_label, y_label, line_style, data_name, ref_data_name): +def plot_two(ax, ref, eagleye, x_data, y_data, title, x_label, y_label, line_style, font_size, data_name, ref_data_name): ax.plot(ref[x_data] , ref[y_data] , marker=".", linestyle=line_style, markersize=1, color = "red", label=ref_data_name) ax.plot(eagleye[x_data] , eagleye[y_data] , marker="s", linestyle=line_style, markersize=1, alpha=0.3, color = "blue", label=data_name) - ax.set_xlabel(x_label) - ax.set_ylabel(y_label) + ax.set_xlabel(x_label, fontsize=font_size) + ax.set_ylabel(y_label, fontsize=font_size) ax.set_title(title) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() -def plot_three(ax, ref, eagleye, raw, x_data, y_data, title, x_label, y_label, line_style, data_name, ref_data_name): +def plot_three(ax, ref, eagleye, raw, x_data, y_data, title, x_label, y_label, line_style, font_size, data_name, ref_data_name): ax.plot(ref[x_data] , ref[y_data] , marker=".", linestyle=line_style, markersize=1, color = "red", label=ref_data_name) ax.plot(raw[x_data] , raw[y_data] , marker="o", linestyle=line_style, markersize=1, color = "green", label="initial ref heading") ax.plot(eagleye[x_data] , eagleye[y_data] , marker="s", linestyle=line_style, markersize=1, alpha=0.3, color = "blue", label=data_name) - ax.set_xlabel(x_label) - ax.set_ylabel(y_label) + ax.set_xlabel(x_label, fontsize=font_size) + ax.set_ylabel(y_label, fontsize=font_size) ax.set_title(title) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() -def plot_error_6DoF(error_data,ref_data_name, error_table): +def plot_error_6DoF(error_data,ref_data_name, error_table, font_size): fig2 = plt.figure() fig2.suptitle(ref_data_name + ' - eagleye Error') ax_x = fig2.add_subplot(4, 3, 1) @@ -158,122 +166,147 @@ def plot_error_6DoF(error_data,ref_data_name, error_table): ax_roll_table = fig2.add_subplot(4, 3, 10) ax_pitch_table = fig2.add_subplot(4, 3, 11) ax_yaw_table = fig2.add_subplot(4, 3, 12) - plot_each_error(ax_x, error_data, 'x', 'X (East-West) Error','East error [m]') - plot_each_error(ax_y, error_data, 'y', 'Y (North-South) Error','North error [m]') - plot_each_error(ax_z, error_data, 'z', 'Z (Height) Error','Height error [m]') - plot_each_error(ax_roll, error_data, 'roll', 'Roll Error' , 'Roll error [deg]') - plot_each_error(ax_pitch, error_data, 'pitch', 'Pitch Error', 'Pitch error [deg]') - plot_each_error(ax_yaw, error_data, 'yaw', 'Yaw Error', 'Yaw error [deg]') - plot_table(ax_x_table, error_table[error_table["data"] == "x"], "[m]") - plot_table(ax_y_table, error_table[error_table["data"] == "y"], "[m]") - plot_table(ax_z_table, error_table[error_table["data"] == "z"], "[m]") - plot_table(ax_roll_table, error_table[error_table["data"] == "roll"], "[deg]") - plot_table(ax_pitch_table, error_table[error_table["data"] == "pitch"], "[deg]") - plot_table(ax_yaw_table, error_table[error_table["data"] == "yaw"], "[deg]") + plot_each_error(ax_x, error_data, 'x', 'X (East-West) Error','East error [m]', font_size) + plot_each_error(ax_y, error_data, 'y', 'Y (North-South) Error','North error [m]', font_size) + plot_each_error(ax_z, error_data, 'z', 'Z (Height) Error','Height error [m]', font_size) + plot_each_error(ax_roll, error_data, 'roll', 'Roll Error' , 'Roll error [deg]', font_size) + plot_each_error(ax_pitch, error_data, 'pitch', 'Pitch Error', 'Pitch error [deg]', font_size) + plot_each_error(ax_yaw, error_data, 'yaw', 'Yaw Error', 'Yaw error [deg]', font_size) + plot_table(ax_x_table, error_table[error_table["data"] == "x"], "[m]", font_size) + plot_table(ax_y_table, error_table[error_table["data"] == "y"], "[m]", font_size) + plot_table(ax_z_table, error_table[error_table["data"] == "z"], "[m]", font_size) + plot_table(ax_roll_table, error_table[error_table["data"] == "roll"], "[deg]", font_size) + plot_table(ax_pitch_table, error_table[error_table["data"] == "pitch"], "[deg]", font_size) + plot_table(ax_yaw_table, error_table[error_table["data"] == "yaw"], "[deg]", font_size) -def plot_table(ax, error_table, unit): +def plot_table(ax, error_table, unit, font_size): table_data = error_table.set_index('data') table = ax.table(cellText = table_data.values, colLabels = table_data.columns + unit, loc = 'center') table.auto_set_font_size(False) - table.set_fontsize(10) + table.set_fontsize(font_size) ax.axis('tight') ax.axis('off') -def plot_error(error_data,ref_data_name): +def plot_error(error_data,ref_data_name, font_size): fig4 = plt.figure() fig4.suptitle(ref_data_name + ' - eagleye Error') ax_x = fig4.add_subplot(2, 2, 1) ax_y = fig4.add_subplot(2, 2, 2) ax_across = fig4.add_subplot(2, 2, 3) ax_along = fig4.add_subplot(2, 2, 4) - plot_each_error(ax_x, error_data, 'x', 'X (East-West) Error','East error [m]') - plot_each_error(ax_y, error_data, 'y', 'Y (North-South) Error','North error [m]') - plot_each_error(ax_across, error_data, 'across', 'Across Error','across error [m]') - plot_each_error(ax_along, error_data, 'along', 'Along Error','along error [m]') + plot_each_error(ax_x, error_data, 'x', 'X (East-West) Error','East error [m]', font_size) + plot_each_error(ax_y, error_data, 'y', 'Y (North-South) Error','North error [m]', font_size) + plot_each_error(ax_across, error_data, 'across', 'Across Error','across error [m]', font_size) + plot_each_error(ax_along, error_data, 'along', 'Along Error','along error [m]', font_size) -def plot_error_distributiln(error_data,ref_data_name): +def plot_error_distributiln(error_data, font_size,ref_data_name): fig5 = plt.figure() fig5.suptitle(ref_data_name + ' - eagleye Error') ax_xy = fig5.add_subplot(1, 2, 1) ax_aa = fig5.add_subplot(1, 2, 2) - plot_one(ax_xy, error_data, 'x', 'y', 'X-Y error', 'x error [m]', 'y error [m]', 'None', 1) - plot_one(ax_aa, error_data, 'across', 'along', 'Across-Along error', 'across error [m]', 'along error [m]', 'None', 1) + plot_one(ax_xy, error_data, 'x', 'y', 'X-Y error', 'x error [m]', 'y error [m]', 'None', 1, font_size) + plot_one(ax_aa, error_data, 'across', 'along', 'Across-Along error', 'across error [m]', 'along error [m]', 'None', 1, font_size) ax_xy.set_aspect('equal') ax_xy.axis('square') ax_aa.set_aspect('equal') ax_aa.axis('square') -def plot_traj(ref_data, data,data_name, ref_data_name): +def plot_traj(ref_data, data, font_size,data_name, ref_data_name): fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.plot(ref_data['x']-ref_data['x'][0] , ref_data['y']-ref_data['y'][0] , marker=".",linestyle="None",markersize=3, color = "red", label=ref_data_name) ax.plot(data['x']-ref_data['x'][0] , data['y']-ref_data['y'][0] , marker="s",linestyle="None",markersize=3,alpha=0.3 , color = "blue", label=data_name) - ax.set_xlabel('East [m]') - ax.set_ylabel('North [m]') + ax.set_xlabel('East [m]', fontsize=font_size) + ax.set_ylabel('North [m]', fontsize=font_size) ax.set_title('2D Trajectory') ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() ax.set_aspect('equal') ax.axis('square') -def plot_traj_text(title, ref_data, data, text, step, data_name, ref_data_name): +def plot_traj_text(title, ref_data, data, text, step, font_size, data_name, ref_data_name): fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.plot(ref_data['x']-ref_data['x'][0] , ref_data['y']-ref_data['y'][0] , marker=".",linestyle="None",markersize=3, color = "red", label=ref_data_name) ax.plot(data['x']-ref_data['x'][0] , data['y']-ref_data['y'][0] , marker="s",linestyle="None",markersize=3,alpha=0.3 , color = "blue", label=data_name) cnt = 0 + for i in np.arange(0, len(text.index), step): + if text[i] >= cnt * step: + ax.text(ref_data['x'][i] - ref_data['x'][0] , ref_data['y'][i] - ref_data['y'][0] , int(text[i]), size=font_size) + cnt = cnt + 1 + ax.set_xlabel('East [m]', fontsize=font_size) + ax.set_ylabel('North [m]', fontsize=font_size) + ax.set_title(title) + ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) + ax.grid() + ax.set_aspect('equal') + ax.axis('square') + +def plot_traj_text_tree(title, raw_data, ref_data, data, text, step, font_size, data_name, ref_data_name): + fig = plt.figure() + ax = fig.add_subplot(1, 1, 1) + ax.plot(raw_data['x']-ref_data['x'][0] , raw_data['y']-ref_data['y'][0] , marker=".",linestyle="None",markersize=1, color = "red", label="gnss raw data(rtklib)") + ax.plot(ref_data['x']-ref_data['x'][0] , ref_data['y']-ref_data['y'][0] , marker=".",linestyle="None",markersize=3, color = "green", label=ref_data_name) + ax.plot(data['x']-ref_data['x'][0] , data['y']-ref_data['y'][0] , marker="s",linestyle="None",markersize=3,alpha=0.3 , color = "blue", label=data_name) + cnt = 0 for i in np.arange(0, len(text.index), step): if text[i] >= cnt * step: ax.text(ref_data['x'][i] - ref_data['x'][0] , ref_data['y'][i] - ref_data['y'][0] , int(text[i]), size=16) cnt = cnt + 1 - ax.set_xlabel('East [m]') - ax.set_ylabel('North [m]') + ax.set_xlabel('East [m]', fontsize=font_size) + ax.set_ylabel('North [m]', fontsize=font_size) ax.set_title(title) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() ax.set_aspect('equal') ax.axis('square') -def plot_traj_three(raw_data, ref_data, data, data_name, ref_data_name): +def plot_traj_three(raw_data, ref_data, data, font_size, data_name, ref_data_name): fig = plt.figure() ax = fig.add_subplot(1, 1, 1) ax.plot(raw_data['x']-ref_data['x'][0] , raw_data['y']-ref_data['y'][0] , marker=".",linestyle="None",markersize=1, color = "red", label="gnss raw data(rtklib)") ax.plot(ref_data['x']-ref_data['x'][0] , ref_data['y']-ref_data['y'][0] , marker=".",linestyle="None",markersize=1, color = "green", label=ref_data_name) ax.plot(data['x']-ref_data['x'][0] , data['y']-ref_data['y'][0] , marker="s",linestyle="None",markersize=1,alpha=0.3 , color = "blue", label=data_name) - ax.set_xlabel('East [m]') - ax.set_ylabel('North [m]') + ax.set_xlabel('East [m]', fontsize=font_size) + ax.set_ylabel('North [m]', fontsize=font_size) ax.set_title('2D Trajectory') ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() ax.set_aspect('equal') ax.axis('square') -def plot_traj_3d(ref_data, data, data_name, ref_data_name): +def plot_traj_3d(ref_data, data, font_size, data_name, ref_data_name): fig = plt.figure() ax = fig.add_subplot(projection='3d') ax.set_title('3D Trajectory') ax.plot3D(ref_data['x'] - ref_data['x'][0] , ref_data['y'] - ref_data['y'][0] ,ref_data['z'] - ref_data['z'][0] , marker=".",linestyle="None",markersize=1, color = "red",label=ref_data_name) ax.plot3D(data['x'] - ref_data['x'][0] , data['y'] - ref_data['y'][0] ,data['z'] - ref_data['z'][0] , marker="s",linestyle="None",markersize=1, alpha=0.3, color = "blue",label=data_name) - ax.set_xlabel('East [m]') - ax.set_ylabel('North [m]') - ax.set_zlabel('Height [m]') + ax.set_xlabel('East [m]', fontsize=font_size) + ax.set_ylabel('North [m]', fontsize=font_size) + ax.set_zlabel('Height [m]', fontsize=font_size) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() -def plot_traj_3d_three(raw_data, ref_data, data, data_name, ref_data_name): +def plot_traj_3d_three(raw_data, ref_data, data, font_size, data_name, ref_data_name): fig = plt.figure() ax = fig.add_subplot(projection='3d') ax.set_title('3D Trajectory') ax.plot3D(raw_data['x'] - ref_data['x'][0] , raw_data['y'] - ref_data['y'][0] ,raw_data['z'] - ref_data['z'][0] , marker=".",linestyle="None",markersize=1, color = "red",label="gnss raw data(rtklib)") ax.plot3D(ref_data['x'] - ref_data['x'][0] , ref_data['y'] - ref_data['y'][0] ,ref_data['z'] - ref_data['z'][0] , marker=".",linestyle="None",markersize=1, color = "green",label=ref_data_name) ax.plot3D(data['x'] - ref_data['x'][0] , data['y'] - ref_data['y'][0] ,data['z'] - ref_data['z'][0] , marker="s",linestyle="None",markersize=1, alpha=0.3, color = "blue",label=data_name) - ax.set_xlabel('East [m]') - ax.set_ylabel('North [m]') - ax.set_zlabel('Height [m]') + ax.set_xlabel('East [m]', fontsize=font_size) + ax.set_ylabel('North [m]', fontsize=font_size) + ax.set_zlabel('Height [m]', fontsize=font_size) ax.legend(loc='upper right') + ax.tick_params(labelsize=font_size) ax.grid() -def plot_traj_qual(xyz, qual): +def plot_traj_qual(xyz, qual, text, step, font_size): gnss_single = judge_qual(xyz,qual,1) gnss_differential = judge_qual(xyz,qual,2) gnss_fix = judge_qual(xyz,qual,4) @@ -285,10 +318,16 @@ def plot_traj_qual(xyz, qual): ax_fix.plot(gnss_single['x']-xyz['x'][0] , gnss_single['y']-xyz['y'][0] , marker=".",linestyle="None",markersize=1, color = "red", label="gnss single") ax_fix.plot(gnss_differential['x']-xyz['x'][0] , gnss_differential['y']-xyz['y'][0] , marker=".",linestyle="None",markersize=1, color = "blue", label="gnss differential") ax_fix.plot(gnss_fix['x']-xyz['x'][0] , gnss_fix['y']-xyz['y'][0] , marker=".",linestyle="None",markersize=1, color = "green", label="gnss fix") - ax_fix.plot(gnss_float['x']-xyz['x'][0] , gnss_float['y']-xyz['y'][0] , marker=".",linestyle="None",markersize=1, color = "yellow", label="gnss float") - ax_fix.set_xlabel('East [m]') - ax_fix.set_ylabel('North [m]') + ax_fix.plot(gnss_float['x']-xyz['x'][0] , gnss_float['y']-xyz['y'][0] , marker=".",linestyle="None",markersize=1, color = "yellow", label="gnss float") + cnt = 0 + for i in np.arange(0, len(text.index), step): + if text[i] >= cnt * step: + ax_fix.text(xyz['x'][i] - xyz['x'][0] , xyz['y'][i] - xyz['y'][0] , int(text[i]), size=16) + cnt = cnt + 1 + ax_fix.set_xlabel('East [m]', fontsize=font_size) + ax_fix.set_ylabel('North [m]', fontsize=font_size) ax_fix.legend(loc='upper right') + ax_fix.tick_params(labelsize=font_size) ax_fix.grid() ax_fix.set_aspect('equal','box') ax_fix.axis('square') diff --git a/eagleye_util/trajectory_plot/scripts/util/preprocess.py b/eagleye_util/trajectory_plot/scripts/util/preprocess.py index 4d2863f3..12ac0487 100644 --- a/eagleye_util/trajectory_plot/scripts/util/preprocess.py +++ b/eagleye_util/trajectory_plot/scripts/util/preprocess.py @@ -188,17 +188,15 @@ def set_log_df(input,plane,config): # Creation of dataset with reference to labe index_time_unit = config["eagleye_log"]["time_unit"] tf_num = config["eagleye_log"]["tf_num"] ros_reverse_imu = config["eagleye_log"]["ros_reverse_imu"] + missing_gnss_type = config["eagleye_log"]["missing_gnss_type"] eagleye_df = df[["timestamp", "rtklib_nav.tow", "velocity_scale_factor.scale_factor", "correction_velocity.twist.linear.x", "distance.distance", - "enu_absolute_pos_interpolate.enu_pos.x", - "enu_absolute_pos_interpolate.enu_pos.y", - "enu_absolute_pos_interpolate.enu_pos.z", - "enu_absolute_pos.ecef_base_pos.x", - "enu_absolute_pos.ecef_base_pos.y", - "enu_absolute_pos.ecef_base_pos.z", + "enu_absolute_pos_interpolate.ecef_base_pos.x", + "enu_absolute_pos_interpolate.ecef_base_pos.y", + "enu_absolute_pos_interpolate.ecef_base_pos.z", "rolling.rolling_angle", "pitching.pitching_angle", "heading_interpolate_1st.heading_angle", @@ -224,9 +222,9 @@ def set_log_df(input,plane,config): # Creation of dataset with reference to labe 'velocity_scale_factor.scale_factor': 'sf', 'correction_velocity.twist.linear.x': 'velocity', 'distance.distance': 'distance', - 'enu_absolute_pos.ecef_base_pos.x': 'ecef_base_x', - 'enu_absolute_pos.ecef_base_pos.y': 'ecef_base_y', - 'enu_absolute_pos.ecef_base_pos.z': 'ecef_base_z', + 'enu_absolute_pos_interpolate.ecef_base_pos.x': 'ecef_base_x', + 'enu_absolute_pos_interpolate.ecef_base_pos.y': 'ecef_base_y', + 'enu_absolute_pos_interpolate.ecef_base_pos.z': 'ecef_base_z', 'rolling.rolling_angle': 'roll_rad', 'pitching.pitching_angle': 'pitch_rad', 'heading_interpolate_1st.heading_angle': 'heading_1st', @@ -287,7 +285,13 @@ def set_log_df(input,plane,config): # Creation of dataset with reference to labe raw_index = raw_df[raw_df['latitude'] == 0].index rtk_index = raw_df[raw_df['rtk_latitude'] == 0].index - if len(rtk_index) < len(raw_index): + if missing_gnss_type == 1: + raw_df = raw_df.drop(rtk_index) + eagleye_df = eagleye_df.drop(rtk_index) + elif missing_gnss_type == 2: + raw_df = raw_df.drop(raw_index) + eagleye_df = eagleye_df.drop(raw_index) + elif len(rtk_index) < len(raw_index): raw_df = raw_df.drop(raw_index) eagleye_df = eagleye_df.drop(raw_index) else: