diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index cb60206b..3ba62918 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -16,7 +16,9 @@ jobs: steps: - name: checkout - uses: actions/checkout@v2 + uses: actions/checkout@v1 + with: + submodules: true - name: rosdep update run: | @@ -40,44 +42,6 @@ jobs: rosdep install --from-paths src --ignore-src -r -y colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests - galactic_build: - runs-on: ubuntu-20.04 - - container: ros:galactic - services: - ros: - image: ros - - defaults: - run: - shell: bash - - steps: - - name: checkout - uses: actions/checkout@v2 - - - name: rosdep update - run: | - apt-get update - rosdep update - - name: Create Workspace - run: | - mkdir -p ~/eagleye/src/ - cp -r `pwd` ~/eagleye/src/ - - name: Clone rtklib_msgs - run: | - cd ~/eagleye/src/ - git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0 - - name: Install GeographicLib - run: | - apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc - - name: Build - run: | - cd ~/eagleye/ - source /opt/ros/galactic/setup.bash - rosdep install --from-paths src --ignore-src -r -y - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests - foxy_build: runs-on: ubuntu-20.04 @@ -92,7 +56,9 @@ jobs: steps: - name: checkout - uses: actions/checkout@v2 + uses: actions/checkout@v1 + with: + submodules: true - name: rosdep update run: | diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..24e992dc --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "eagleye_util/llh_converter"] + path = eagleye_util/llh_converter + url = https://github.com/MapIV/llh_converter + branch = ros2 \ No newline at end of file diff --git a/README.md b/README.md index 3240dd99..d9f283c0 100755 --- a/README.md +++ b/README.md @@ -46,7 +46,11 @@ Clone and build the necessary packages for Eagleye. ([rtklib_ros_bridge](https:/ git clone https://github.com/MapIV/eagleye.git -b main-ros2 git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0 git clone https://github.com/MapIV/nmea_ros_bridge.git -b ros2-v0.1.0 + git clone https://github.com/MapIV/gnss_compass_ros.git -b main-ros2 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 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release @@ -98,7 +102,7 @@ The estimated results will be output about 100 seconds after playing the rosbag. 1. Check if wheel speed (vehicle speed) is published in `/can_twist` topic. * Topic name: /can_twist -* Message type: geometry_msgs/TwistStamped twist.liner.x +* Message type: geometry_msgs/TwistStamped twist.liner.x or geometry_msgs/TwistWithCovarianceStamped twist.twist.liner.x 2. Check if the IMU data is published in `/imu/data_raw` topic. @@ -128,7 +132,7 @@ The estimated results will be output about 100 seconds after playing the rosbag. ### Subscribed Topics - /navsat/nmea_sentence (nmea_msgs/Sentence) - - /can_twist (geometry_msgs/TwistStamped) + - /can_twist (geometry_msgs/TwistStamped or geometry_msgs/TwistWithCovarianceStamped) - /rtklib_nav (rtklib_msgs/RtklibNav) @@ -138,7 +142,9 @@ The estimated results will be output about 100 seconds after playing the rosbag. - /eagleye/fix (sensor_msgs/NavSatFix) - - /eagleye/twist (ngeometry_msgs/TwistStamped) + - /eagleye/twist (geometry_msgs/TwistStamped) + + - /eagleye/twist_with_covariance (geometry_msgs/TwistWithCovarianceStamped) ### Note diff --git a/eagleye_core/coordinate/src/ll2xy_mgrs.cpp b/eagleye_core/coordinate/src/ll2xy_mgrs.cpp index 046e560a..37f01c91 100644 --- a/eagleye_core/coordinate/src/ll2xy_mgrs.cpp +++ b/eagleye_core/coordinate/src/ll2xy_mgrs.cpp @@ -105,8 +105,8 @@ void ll2xy_mgrs(double llh[3], double xyz[3]) //wgs_point.longitude = llh[1]; wgs_point.altitude = llh[2]; - std::cout << wgs_point.latitude << std::endl; - std::cout << wgs_point.longitude << std::endl; + // std::cout << wgs_point.latitude << std::endl; + // std::cout << wgs_point.longitude << std::endl; geodesy::UTMPoint utm_point; diff --git a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp index e140637d..7acc826d 100644 --- a/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp +++ b/eagleye_core/navigation/include/eagleye_navigation/eagleye_navigation.hpp @@ -24,6 +24,7 @@ // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include #include "sensor_msgs/msg/imu.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" @@ -144,12 +145,14 @@ struct HeadingParameter double outlier_threshold; double outlier_ratio_threshold; double curve_judgment_threshold; + double init_STD; }; struct HeadingStatus { int tow_last; double rmc_time_last; + double ros_time_last; int estimated_number; std::vector time_buffer; std::vector heading_angle_buffer; @@ -202,6 +205,7 @@ struct HeadingInterpolateParameter double imu_rate; double stop_judgment_threshold; double sync_search_period; + double proc_noise; }; struct HeadingInterpolateStatus @@ -214,6 +218,7 @@ struct HeadingInterpolateStatus double provisional_heading_angle; std::vector provisional_heading_angle_buffer; std::vector imu_stamp_buffer; + double heading_variance_last; }; struct PositionParameter @@ -227,7 +232,7 @@ struct PositionParameter double tf_gnss_rotation_x; double tf_gnss_rotation_y; double tf_gnss_rotation_z; - double tf_gnss_rotation_w; + double tf_gnss_rotation_w = 1.0; std::string tf_gnss_parent_frame; std::string tf_gnss_child_frame; @@ -330,6 +335,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 @@ -419,6 +428,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 @@ -436,6 +447,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 @@ -499,6 +511,8 @@ extern void heading_estimate(const rtklib_msgs::msg::RtklibNav, const sensor_msg extern void heading_estimate(const nmea_msgs::msg::Gprmc, const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::SlipAngle, const eagleye_msgs::msg::Heading, const HeadingParameter, HeadingStatus*,eagleye_msgs::msg::Heading*); +extern void heading_estimate(const eagleye_msgs::msg::Heading, const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::YawrateOffset, + const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::SlipAngle, const eagleye_msgs::msg::Heading, const HeadingParameter, HeadingStatus*,eagleye_msgs::msg::Heading*); extern void position_estimate(const rtklib_msgs::msg::RtklibNav, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped, const eagleye_msgs::msg::Distance, const eagleye_msgs::msg::Heading, const geometry_msgs::msg::Vector3Stamped, const PositionParameter, PositionStatus*, eagleye_msgs::msg::Position*); @@ -513,7 +527,7 @@ extern void smoothing_estimate(const rtklib_msgs::msg::RtklibNav,const geometry_ eagleye_msgs::msg::Position*); extern void trajectory_estimate(const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped, const eagleye_msgs::msg::Heading, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset,const TrajectoryParameter,TrajectoryStatus*, - geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*); + geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*,geometry_msgs::msg::TwistWithCovarianceStamped*); extern void heading_interpolate_estimate(const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset, const eagleye_msgs::msg::Heading, const eagleye_msgs::msg::SlipAngle, const HeadingInterpolateParameter,HeadingInterpolateStatus*, eagleye_msgs::msg::Heading*); @@ -524,7 +538,7 @@ extern void pitching_estimate(const sensor_msgs::msg::Imu, const nmea_msgs::msg: eagleye_msgs::msg::AccXScaleFactor*); extern void trajectory3d_estimate(const sensor_msgs::msg::Imu, const geometry_msgs::msg::TwistStamped, const eagleye_msgs::msg::StatusStamped, const eagleye_msgs::msg::Heading, const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::YawrateOffset,const eagleye_msgs::msg::Pitching,const TrajectoryParameter,TrajectoryStatus*, - geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*); + geometry_msgs::msg::Vector3Stamped*,eagleye_msgs::msg::Position*,geometry_msgs::msg::TwistStamped*,geometry_msgs::msg::TwistWithCovarianceStamped*); extern void angular_velocity_offset_stop_estimate(const geometry_msgs::msg::TwistStamped, const sensor_msgs::msg::Imu, const AngularVelocityOffsetStopParameter, AngularVelocityOffsetStopStatus*, eagleye_msgs::msg::AngularVelocityOffset*); extern void rtk_deadreckoning_estimate(const rtklib_msgs::msg::RtklibNav,const geometry_msgs::msg::Vector3Stamped,const nmea_msgs::msg::Gpgga, diff --git a/eagleye_core/navigation/src/heading.cpp b/eagleye_core/navigation/src/heading.cpp index 65be34f6..9d2da855 100755 --- a/eagleye_core/navigation/src/heading.cpp +++ b/eagleye_core/navigation/src/heading.cpp @@ -150,10 +150,10 @@ void heading_estimate_(sensor_msgs::msg::Imu imu, geometry_msgs::msg::TwistStamp std::vector inversion_up_index; std::vector inversion_down_index; - if(heading_interpolate.status.enabled_status == false) - { - heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]]; - } + if(heading_interpolate.status.enabled_status == false) + { + heading_interpolate.heading_angle = heading_status->heading_angle_buffer [index[index_length-1]]; + } int ref_cnt; std::vector heading_angle_buffer2; @@ -238,6 +238,9 @@ void heading_estimate_(sensor_msgs::msg::Imu imu, geometry_msgs::msg::TwistStamp 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; } } } @@ -323,4 +326,36 @@ void heading_estimate(const nmea_msgs::msg::Gprmc nmea_rmc,sensor_msgs::msg::Imu heading_status->gnss_status_buffer.push_back(gnss_status); heading_estimate_(imu, velocity, yawrate_offset_stop, yawrate_offset, slip_angle, heading_interpolate, heading_parameter, heading_status, heading); +} + +void heading_estimate(const eagleye_msgs::msg::Heading multi_antenna_heading,sensor_msgs::msg::Imu imu,geometry_msgs::msg::TwistStamped velocity, + eagleye_msgs::msg::YawrateOffset yawrate_offset_stop,eagleye_msgs::msg::YawrateOffset yawrate_offset,eagleye_msgs::msg::SlipAngle slip_angle, + eagleye_msgs::msg::Heading heading_interpolate,HeadingParameter heading_parameter, HeadingStatus* heading_status,eagleye_msgs::msg::Heading* heading) +{ + bool gnss_status; + double heading_angle = 0.0; + + rclcpp::Time multi_anttena_clock(multi_antenna_heading.header.stamp); + double multi_anttena_time = multi_anttena_clock.seconds(); + if (heading_status->ros_time_last == multi_anttena_time || multi_anttena_time == 0) + { + gnss_status = false; + heading_angle = 0; + heading_status->ros_time_last = multi_anttena_time; + } + else + { + gnss_status = true; + heading_angle = multi_antenna_heading.heading_angle; + heading_status->ros_time_last = multi_anttena_time; + } + + heading_status->heading_angle_buffer .push_back(heading_angle); + heading_status->gnss_status_buffer .push_back(gnss_status); + + heading_estimate_(imu,velocity,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,heading_status,heading); + + if(!heading->status.estimate_status) heading->heading_angle = heading_angle; + if(gnss_status) heading->status.estimate_status = true; + } \ No newline at end of file diff --git a/eagleye_core/navigation/src/heading_interpolate.cpp b/eagleye_core/navigation/src/heading_interpolate.cpp index 5c3fde78..1e8a1881 100755 --- 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::msg::Imu imu, const geometr bool heading_estimate_status; 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; rclcpp::Time ros_clock(heading.header.stamp); @@ -85,6 +87,7 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometr heading_interpolate_parameter.stop_judgment_threshold) { heading_interpolate_status->provisional_heading_angle = heading_interpolate_status->provisional_heading_angle + (yawrate * (imu_time - heading_interpolate_status->time_last)); + heading_interpolate_status->heading_variance_last += proc_noise*proc_noise; } // data buffer generate @@ -114,23 +117,27 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometr if (heading_estimate_status == true && estimate_index > 0 && heading_interpolate_status->number_buffer >= estimate_index && heading_interpolate_status->heading_estimate_status_count > 1) { + double heading_variance = heading.variance; diff_estimate_heading_angle = (heading_interpolate_status->provisional_heading_angle_buffer[estimate_index-1] - heading.heading_angle); for (i = estimate_index; i <= heading_interpolate_status->number_buffer; i++) { heading_interpolate_status->provisional_heading_angle_buffer[i-1] = 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 { @@ -141,6 +148,7 @@ void heading_interpolate_estimate(const sensor_msgs::msg::Imu imu, const geometr if (heading_interpolate_status->heading_estimate_start_status == true) { 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/position.cpp b/eagleye_core/navigation/src/position.cpp index 191bbf05..45fb2cae 100755 --- a/eagleye_core/navigation/src/position.cpp +++ b/eagleye_core/navigation/src/position.cpp @@ -77,15 +77,16 @@ void position_estimate_(geometry_msgs::msg::TwistStamped velocity,eagleye_msgs:: q.setRPY(0, 0, (90* M_PI / 180)-heading_interpolate_3rd.heading_angle); transform.setRotation(q); - tf2::Transform transform2; + tf2::Transform transform2, transform3; tf2::Quaternion q2(position_parameter.tf_gnss_rotation_x,position_parameter.tf_gnss_rotation_y, position_parameter.tf_gnss_rotation_z,position_parameter.tf_gnss_rotation_w); - transform2.setOrigin(transform*tf2::Vector3(-position_parameter.tf_gnss_translation_x, - -position_parameter.tf_gnss_translation_y, -position_parameter.tf_gnss_translation_z)); - transform2.setRotation(transform*q2); + transform2.setOrigin(tf2::Vector3(position_parameter.tf_gnss_translation_x, position_parameter.tf_gnss_translation_y, + position_parameter.tf_gnss_translation_z)); + transform2.setRotation(q2); + transform3 = transform * transform2.inverse(); tf2::Vector3 tmp_pos; - tmp_pos = transform2.getOrigin(); + tmp_pos = transform3.getOrigin(); enu_pos[0] = tmp_pos.getX(); enu_pos[1] = tmp_pos.getY(); diff --git a/eagleye_core/navigation/src/rtk_deadreckoning.cpp b/eagleye_core/navigation/src/rtk_deadreckoning.cpp index 3ea5ebe4..77b9e7cf 100755 --- a/eagleye_core/navigation/src/rtk_deadreckoning.cpp +++ b/eagleye_core/navigation/src/rtk_deadreckoning.cpp @@ -53,7 +53,7 @@ void rtk_deadreckoning_estimate_(geometry_msgs::msg::Vector3Stamped enu_vel, nme auto gga_time = ros_clock.seconds(); auto enu_vel_time = ros_clock2.seconds(); - 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; @@ -111,6 +111,10 @@ void rtk_deadreckoning_estimate_(geometry_msgs::msg::Vector3Stamped enu_vel, nme 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; @@ -118,17 +122,65 @@ void rtk_deadreckoning_estimate_(geometry_msgs::msg::Vector3Stamped enu_vel, nme 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_time - rtk_deadreckoning_status->time_last); + jacobian(1,5) = -enu_vel.vector.x*(enu_vel_time - 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_time; rtk_deadreckoning_status->position_stamp_last = gga_time; diff --git a/eagleye_core/navigation/src/trajectory.cpp b/eagleye_core/navigation/src/trajectory.cpp index 6ca2bd41..b4d6a55c 100755 --- a/eagleye_core/navigation/src/trajectory.cpp +++ b/eagleye_core/navigation/src/trajectory.cpp @@ -31,11 +31,43 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +void calculate_covariance(const geometry_msgs::msg::TwistStamped velocity, const eagleye_msgs::msg::StatusStamped velocity_status, + const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, const TrajectoryParameter trajectory_parameter, + geometry_msgs::msg::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::msg::Imu imu, const geometry_msgs::msg::TwistStamped velocity, const eagleye_msgs::msg::StatusStamped velocity_status, const eagleye_msgs::msg::Heading heading_interpolate_3rd, const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, const eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd, const TrajectoryParameter trajectory_parameter, TrajectoryStatus* trajectory_status, geometry_msgs::msg::Vector3Stamped* enu_vel, - eagleye_msgs::msg::Position* enu_relative_pos, geometry_msgs::msg::TwistStamped* eagleye_twist) + eagleye_msgs::msg::Position* enu_relative_pos, geometry_msgs::msg::TwistStamped* eagleye_twist, + geometry_msgs::msg::TwistWithCovarianceStamped* eagleye_twist_with_covariance) { rclcpp::Time ros_clock(imu.header.stamp); auto imu_time = ros_clock.seconds(); @@ -43,13 +75,18 @@ void trajectory_estimate(const sensor_msgs::msg::Imu imu, const geometry_msgs:: if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold && yawrate_offset_2nd.status.enabled_status == true) { eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed } else { eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); //Inverted because the coordinate system is reversed + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); //Inverted because the coordinate system is reversed } 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) { @@ -99,7 +136,8 @@ void trajectory3d_estimate(const sensor_msgs::msg::Imu imu, const geometry_msgs: const eagleye_msgs::msg::StatusStamped velocity_status, const eagleye_msgs::msg::Heading heading_interpolate_3rd, const eagleye_msgs::msg::YawrateOffset yawrate_offset_stop, const eagleye_msgs::msg::YawrateOffset yawrate_offset_2nd, const eagleye_msgs::msg::Pitching pitching, const TrajectoryParameter trajectory_parameter, TrajectoryStatus* trajectory_status, - geometry_msgs::msg::Vector3Stamped* enu_vel, eagleye_msgs::msg::Position* enu_relative_pos, geometry_msgs::msg::TwistStamped* eagleye_twist) + geometry_msgs::msg::Vector3Stamped* enu_vel, eagleye_msgs::msg::Position* enu_relative_pos, geometry_msgs::msg::TwistStamped* eagleye_twist, + geometry_msgs::msg::TwistWithCovarianceStamped* eagleye_twist_with_covariance) { rclcpp::Time ros_clock(imu.header.stamp); auto imu_time = ros_clock.seconds(); @@ -107,12 +145,18 @@ void trajectory3d_estimate(const sensor_msgs::msg::Imu imu, const geometry_msgs: if (std::abs(velocity.twist.linear.x) > trajectory_parameter.stop_judgment_threshold && yawrate_offset_2nd.status.enabled_status == true) { eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_2nd.yawrate_offset); //Inverted because the coordinate system is reversed } else { eagleye_twist->twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); //Inverted because the coordinate system is reversed + eagleye_twist_with_covariance->twist.twist.angular.z = -1 * (imu.angular_velocity.z + yawrate_offset_stop.yawrate_offset); //Inverted because the coordinate system is reversed } + 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) { diff --git a/eagleye_msgs/msg/Heading.msg b/eagleye_msgs/msg/Heading.msg index a3528e80..38ee0045 100644 --- a/eagleye_msgs/msg/Heading.msg +++ b/eagleye_msgs/msg/Heading.msg @@ -1,4 +1,5 @@ std_msgs/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 6ea57e04..610eb91e 100644 --- a/eagleye_msgs/msg/Position.msg +++ b/eagleye_msgs/msg/Position.msg @@ -2,4 +2,5 @@ std_msgs/Header header geometry_msgs/Point enu_pos geometry_msgs/Point ecef_base_pos +float64[9] covariance eagleye_msgs/Status status diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt index 70264ef4..278a4e54 100644 --- a/eagleye_rt/CMakeLists.txt +++ b/eagleye_rt/CMakeLists.txt @@ -22,6 +22,10 @@ ament_auto_add_executable(tf_converted_imu src/tf_converted_imu.cpp ) +ament_auto_add_executable(twist_relay + src/twist_relay.cpp +) + ament_auto_add_executable(correction_imu src/correction_imu.cpp ) @@ -102,16 +106,11 @@ ament_auto_add_executable(rtk_heading src/rtk_heading_node.cpp ) -ament_auto_add_executable(navpvt2rtk - src/navpvt2rtk_node.cpp -) - ament_auto_add_executable(velocity_estimator src/velocity_estimator_node.cpp ) install(TARGETS - navpvt2rtk tf_converted_imu correction_imu distance @@ -135,7 +134,6 @@ install(TARGETS install(DIRECTORY config launch log DESTINATION share/${PROJECT_NAME} - #DESTINATION lib/${PROJECT_NAME} ) diff --git a/eagleye_rt/README.md b/eagleye_rt/README.md index a3650a02..22e74c0a 100644 --- a/eagleye_rt/README.md +++ b/eagleye_rt/README.md @@ -26,7 +26,7 @@ The estimated results will be output about 100 seconds after playing the rosbag. 1. Check if wheel speed (vehicle speed) is published in `/can_twist` topic. * Topic name: /can_twist -* Message type: geometry_msgs/TwistStamped twist.liner.x +* Message type: geometry_msgs/TwistStamped twist.liner.x or geometry_msgs/TwistWithCovarianceStamped twist.twist.liner.x 2. Check if the IMU data is published in `/imu/data_raw` topic. 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 a9b081bc..74825d05 100644 --- a/eagleye_rt/config/eagleye_config.yaml +++ b/eagleye_rt/config/eagleye_config.yaml @@ -5,13 +5,15 @@ 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: @@ -25,6 +27,8 @@ z : 0.0 use_ecef_base_position : false + reverse_imu_wz: false + # Eagleye Navigation Parameters # Basic Navigation Functions common: @@ -63,9 +67,11 @@ 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 @@ -84,6 +90,10 @@ 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 @@ -121,6 +131,10 @@ 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.xml b/eagleye_rt/launch/eagleye_calibration.launch.xml index ee3cd7c7..7d4c2fe2 100644 --- a/eagleye_rt/launch/eagleye_calibration.launch.xml +++ b/eagleye_rt/launch/eagleye_calibration.launch.xml @@ -3,6 +3,7 @@ + @@ -11,104 +12,104 @@ - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - - + + + - - - + + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - + diff --git a/eagleye_rt/launch/eagleye_rt.launch.xml b/eagleye_rt/launch/eagleye_rt.launch.xml index 9665b77c..2973fb09 100644 --- a/eagleye_rt/launch/eagleye_rt.launch.xml +++ b/eagleye_rt/launch/eagleye_rt.launch.xml @@ -4,6 +4,8 @@ + + @@ -11,123 +13,135 @@ + + + + + - - + + - - + + - - + + - - + + - - + + - - + + + - - + + + - - + + + - - + + - - + + - - + + - - + + - - + + - - + + - - - + + + - - - + + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + + + + - - - + + + + diff --git a/eagleye_rt/launch/eagleye_rt_canless.launch.xml b/eagleye_rt/launch/eagleye_rt_canless.launch.xml index 4ac0e567..f2aa6280 100644 --- a/eagleye_rt/launch/eagleye_rt_canless.launch.xml +++ b/eagleye_rt/launch/eagleye_rt_canless.launch.xml @@ -4,6 +4,7 @@ + @@ -13,108 +14,108 @@ - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + - + diff --git a/eagleye_rt/launch/navpvt_rt.launch.xml b/eagleye_rt/launch/navpvt_rt.launch.xml deleted file mode 100644 index a1a13eb7..00000000 --- a/eagleye_rt/launch/navpvt_rt.launch.xml +++ /dev/null @@ -1,16 +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 2a704d96..becffdcc 100644 --- a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp +++ b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp @@ -62,9 +62,9 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("angular_velocity_offset_stop"); + auto node = rclcpp::Node::make_shared("eagleye_angular_velocity_offset_stop"); - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -75,8 +75,6 @@ int main(int argc, char** argv) { YAML::Node conf = YAML::LoadFile(yaml_file); - subscribe_twist_topic_name = conf["/**"]["ros__parameters"]["twist_topic"].as(); - angular_velocity_offset_stop_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); angular_velocity_offset_stop_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); angular_velocity_offset_stop_parameter.estimated_interval = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["estimated_interval"].as(); diff --git a/eagleye_rt/src/correction_imu.cpp b/eagleye_rt/src/correction_imu.cpp index 31df5ca9..17a706dc 100644 --- a/eagleye_rt/src/correction_imu.cpp +++ b/eagleye_rt/src/correction_imu.cpp @@ -95,7 +95,7 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("correction_imu"); + auto node = rclcpp::Node::make_shared("eagleye_correction_imu"); auto sub1 = node->create_subscription("yawrate_offset_2nd", rclcpp::QoS(10), yawrate_offset_callback); //ros::TransportHints().tcpNoDelay() auto sub2 = node->create_subscription("angular_velocity_offset_stop", rclcpp::QoS(10), angular_velocity_offset_stop_callback); //ros::TransportHints().tcpNoDelay() diff --git a/eagleye_rt/src/distance_node.cpp b/eagleye_rt/src/distance_node.cpp index b07a9325..f0edaf1e 100644 --- a/eagleye_rt/src/distance_node.cpp +++ b/eagleye_rt/src/distance_node.cpp @@ -64,7 +64,7 @@ void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr ms int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("distance"); + auto node = rclcpp::Node::make_shared("eagleye_distance"); node->declare_parameter("use_canless_mode",_use_canless_mode); node->get_parameter("use_canless_mode",_use_canless_mode); diff --git a/eagleye_rt/src/enable_additional_rolling_node.cpp b/eagleye_rt/src/enable_additional_rolling_node.cpp index 21d5578f..52f8035a 100644 --- a/eagleye_rt/src/enable_additional_rolling_node.cpp +++ b/eagleye_rt/src/enable_additional_rolling_node.cpp @@ -121,7 +121,7 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("enable_additional_rolling"); + auto node = rclcpp::Node::make_shared("eagleye_enable_additional_rolling"); std::string subscribe_localization_pose_topic_name; diff --git a/eagleye_rt/src/heading_interpolate_node.cpp b/eagleye_rt/src/heading_interpolate_node.cpp index 3dc067a4..73dcba66 100644 --- a/eagleye_rt/src/heading_interpolate_node.cpp +++ b/eagleye_rt/src/heading_interpolate_node.cpp @@ -93,7 +93,7 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("heading_interpolate"); + auto node = rclcpp::Node::make_shared("eagleye_heading_interpolate"); std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -107,10 +107,12 @@ int main(int argc, char** argv) heading_interpolate_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); heading_interpolate_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); heading_interpolate_parameter.sync_search_period = conf["/**"]["ros__parameters"]["heading_interpolate"]["sync_search_period"].as(); + heading_interpolate_parameter.proc_noise = conf["/**"]["ros__parameters"]["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 d6bfc002..afa449ba 100644 --- a/eagleye_rt/src/heading_node.cpp +++ b/eagleye_rt/src/heading_node.cpp @@ -34,6 +34,7 @@ static rtklib_msgs::msg::RtklibNav rtklib_nav; static nmea_msgs::msg::Gprmc nmea_rmc; +static eagleye_msgs::msg::Heading multi_antenna_heading; static sensor_msgs::msg::Imu imu; static geometry_msgs::msg::TwistStamped velocity; static eagleye_msgs::msg::StatusStamped velocity_status; @@ -50,10 +51,11 @@ struct HeadingStatus heading_status; std::string use_gnss_mode; static bool use_canless_mode; +static bool use_multi_antenna_mode; bool is_first_correction_velocity = false; -std::string node_name = "heading"; +std::string node_name = "eagleye_heading"; void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { @@ -69,6 +71,18 @@ void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr ms } } +void pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) +{ + tf2::Quaternion orientation; + tf2::fromMsg(msg->pose.orientation, orientation); + double roll, pitch, yaw; + tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw); + double heading = - yaw + (90* M_PI / 180); + + multi_antenna_heading.header = msg->header; + multi_antenna_heading.heading_angle = heading; +} + void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { velocity_status = *msg; @@ -112,11 +126,17 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) imu = *msg; heading.header = msg->header; heading.header.frame_id = "base_link"; - if (use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB") // use RTKLIB mode + bool use_rtklib_mode = use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB"; + bool use_nmea_mode = use_gnss_mode == "nmea" || use_gnss_mode == "NMEA"; + if (use_rtklib_mode && !use_multi_antenna_mode) // use RTKLIB mode heading_estimate(rtklib_nav,imu,velocity,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); - else if (use_gnss_mode == "nmea" || use_gnss_mode == "NMEA") // use NMEA mode + else if (use_nmea_mode && !use_multi_antenna_mode) // use NMEA mode heading_estimate(nmea_rmc,imu,velocity,yawrate_offset_stop,yawrate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); - if (heading.status.estimate_status == true) + else if (use_multi_antenna_mode) + heading_estimate(multi_antenna_heading, imu, velocity, yawrate_offset_stop, yawrate_offset, slip_angle, + heading_interpolate, heading_parameter, &heading_status, &heading); + + if (heading.status.estimate_status == true || use_multi_antenna_mode) { pub->publish(heading); } @@ -128,13 +148,16 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared(node_name); - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_rmc_topic_name = "/navsat/rmc"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_rmc_topic_name = "gnss/rmc"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); node->get_parameter("yaml_file",yaml_file); + node->declare_parameter("use_multi_antenna_mode",use_multi_antenna_mode); + node->get_parameter("use_multi_antenna_mode",use_multi_antenna_mode); std::cout << "yaml_file: " << yaml_file << std::endl; + std::cout << "use_multi_antenna_mode: " << use_multi_antenna_mode << std::endl; try { @@ -142,8 +165,6 @@ int main(int argc, char** argv) use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["/**"]["ros__parameters"]["rtklib_nav_topic"].as(); - heading_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); heading_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); heading_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); @@ -154,6 +175,7 @@ int main(int argc, char** argv) heading_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["heading"]["outlier_threshold"].as(); heading_parameter.outlier_ratio_threshold = conf["/**"]["ros__parameters"]["heading"]["outlier_ratio_threshold"].as(); heading_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["heading"]["curve_judgment_threshold"].as(); + heading_parameter.init_STD = conf["/**"]["ros__parameters"]["heading"]["init_STD"].as(); std::cout<< "use_gnss_mode " << use_gnss_mode << std::endl; @@ -171,6 +193,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) { @@ -217,12 +240,13 @@ int main(int argc, char** argv) auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); auto sub3 = node->create_subscription(subscribe_rmc_topic_name, 1000, rmc_callback); - auto sub4 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub5 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub6 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); - auto sub7 = node->create_subscription(subscribe_topic_name, 1000, yawrate_offset_callback); - auto sub8 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); - auto sub9 = node->create_subscription(subscribe_topic_name2 , 1000, heading_interpolate_callback); + auto sub4 = node->create_subscription("gnss_compass_pose", 1000, pose_callback); + auto sub5 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); + auto sub6 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); + auto sub7 = node->create_subscription("yawrate_offset_stop", rclcpp::QoS(10), yawrate_offset_stop_callback); + auto sub8 = node->create_subscription(subscribe_topic_name, 1000, yawrate_offset_callback); + auto sub9 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); + auto sub10 = node->create_subscription(subscribe_topic_name2 , 1000, heading_interpolate_callback); pub = node->create_publisher(publish_topic_name, rclcpp::QoS(10)); rclcpp::spin(node); diff --git a/eagleye_rt/src/height_node.cpp b/eagleye_rt/src/height_node.cpp index 6f3345ba..9a5afd03 100644 --- a/eagleye_rt/src/height_node.cpp +++ b/eagleye_rt/src/height_node.cpp @@ -105,9 +105,9 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("height"); + auto node = rclcpp::Node::make_shared("eagleye_height"); - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_gga_topic_name = "gnss/gga"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); diff --git a/eagleye_rt/src/monitor_node.cpp b/eagleye_rt/src/monitor_node.cpp index 6e7952c0..f6ee03be 100755 --- a/eagleye_rt/src/monitor_node.cpp +++ b/eagleye_rt/src/monitor_node.cpp @@ -892,7 +892,7 @@ void outputLog(void) output_log_file << "timestamp,imu.angular_velocity.x,imu.angular_velocity.y,imu.angular_velocity.z,imu.linear_acceleration.x,imu.linear_acceleration.y,imu.linear_acceleration.z\ ,rtklib_nav.tow,rtklib_nav.ecef_pos.x,rtklib_nav.ecef_pos.y,rtklib_nav.ecef_pos.z,rtklib_nav.ecef_vel.x,rtklib_nav.ecef_vel.y,rtklib_nav.ecef_vel.z,rtklib_nav.status.status.status,rtklib_nav.status.status.service,rtklib_nav.status.latitude,rtklib_nav.status.longitude,rtklib_nav.status.altitude\ ,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,velocity_scale_factor.correction_velocity.linear.x,velocity_scale_factor.correction_velocity.linear.y,velocity_scale_factor.correction_velocity.linear.z,velocity_scale_factor.correction_velocity.angular.x,velocity_scale_factor.correction_velocity.angular.y,velocity_scale_factor.correction_velocity.angular.z,velocity_scale_factor.status.enabled_status,velocity_scale_factor.status.estimate_status\ +,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\ @@ -928,8 +928,10 @@ void outputLog(void) { std::ofstream output_log_file(_output_log_dir, std::ios_base::app); rclcpp::Time imu_clock(_imu.header.stamp); - double imu_time = imu_clock.seconds(); - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << imu_time << ","; // timestamp + long double nano_sec = imu_clock.nanoseconds(); + long double sec_digits = std::pow(10,9); + long double imu_time = nano_sec/sec_digits; + output_log_file << std::fixed << std::setprecision(9) << imu_time << ","; // timestamp output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.angular_velocity.x << ","; output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.angular_velocity.y << ","; output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.angular_velocity.z << ","; @@ -1091,16 +1093,14 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("monitor"); + auto node = rclcpp::Node::make_shared("eagleye_monitor"); - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_gga_topic_name = "gnss/gga"; std::string comparison_twist_topic_name = "/calculated_twist"; - node->declare_parameter("twist_topic",subscribe_twist_topic_name); - node->declare_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); node->declare_parameter("gga_topic",subscribe_gga_topic_name); node->declare_parameter("monitor.comparison_twist_topic",comparison_twist_topic_name); @@ -1110,7 +1110,6 @@ int main(int argc, char** argv) node->declare_parameter("monitor.th_diff_rad_per_sec",_th_diff_rad_per_sec); node->declare_parameter("monitor.th_num_continuous_abnormal_yawrate",_th_num_continuous_abnormal_yawrate); - node->get_parameter("twist_topic",subscribe_twist_topic_name); node->get_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); node->get_parameter("gga_topic",subscribe_gga_topic_name); node->get_parameter("monitor.comparison_twist_topic",comparison_twist_topic_name); @@ -1120,8 +1119,6 @@ int main(int argc, char** argv) node->get_parameter("monitor.th_diff_rad_per_sec",_th_diff_rad_per_sec); node->get_parameter("monitor.th_num_continuous_abnormal_yawrate",_th_num_continuous_abnormal_yawrate); - - std::cout<< "subscribe_twist_topic_name "<create_subscription("twist", rclcpp::QoS(10), eagleye_twist_callback); auto sub26 = node->create_subscription("rolling", rclcpp::QoS(10), rolling_callback); auto sub27 = node->create_subscription(comparison_twist_topic_name, 1000, comparison_velocity_callback); + auto sub28 = node->create_subscription("velocity", 1000, correction_velocity_callback); rclcpp::spin(node); diff --git a/eagleye_rt/src/navpvt2rtk_node.cpp b/eagleye_rt/src/navpvt2rtk_node.cpp deleted file mode 100644 index cf988afa..00000000 --- a/eagleye_rt/src/navpvt2rtk_node.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) 2019, 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. - -#include -#include -#include -#include -#include - -rclcpp::Publisher::SharedPtr _pub_rtk; -sensor_msgs::msg::NavSatFix::ConstSharedPtr _nav_msg_ptr; - -void navsatfix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { _nav_msg_ptr = msg; } - -void navpvt_callback(const ublox_msgs::msg::NavPVT::ConstSharedPtr msg) -{ - rtklib_msgs::msg::RtklibNav r; - r.header.frame_id = "gps"; - r.header.stamp.sec = msg->sec; - r.header.stamp.nanosec = msg->nano; - if (_nav_msg_ptr != nullptr) - r.status = *_nav_msg_ptr; - r.tow = msg->i_tow; - - 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->vel_e * 1e-3, msg->vel_n * 1e-3, -msg->vel_d * 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]; - - _pub_rtk->publish(r); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("navpvt2rtk"); - - std::string ublox_navpvt_topic = "/sensing/gnss/ublox/navpvt"; - std::string nav_sat_fix_topic = "/sensing/gnss/ublox/nav_sat_fix"; - std::string rtklib_nav_topic = "/rtklib_nav"; - - node->declare_parameter("rtklib_nav_topic", rtklib_nav_topic); - node->declare_parameter("nav_sat_fix_topic", nav_sat_fix_topic); - node->declare_parameter("ublox_navpvt_topic", ublox_navpvt_topic); - - node->get_parameter("rtklib_nav_topic", rtklib_nav_topic); - node->get_parameter("ublox_navpvt_topic", ublox_navpvt_topic); - node->get_parameter("nav_sat_fix_topic", nav_sat_fix_topic); - - std::cout << "rtklib_nav_topic: " << rtklib_nav_topic << std::endl; - std::cout << "ublox_navpvt_topic: " << ublox_navpvt_topic << std::endl; - std::cout << "nav_sat_fix_topic: " << nav_sat_fix_topic << std::endl; - - - auto sub_ublox = node->create_subscription(ublox_navpvt_topic, 1000, navpvt_callback); - auto sub_fix = node->create_subscription(nav_sat_fix_topic, 1000, navsatfix_callback); - _pub_rtk = node->create_publisher(rtklib_nav_topic, 10); - - rclcpp::spin(node); - - return 0; -} diff --git a/eagleye_rt/src/position_interpolate_node.cpp b/eagleye_rt/src/position_interpolate_node.cpp index 5728f9a1..4a328415 100644 --- a/eagleye_rt/src/position_interpolate_node.cpp +++ b/eagleye_rt/src/position_interpolate_node.cpp @@ -97,9 +97,9 @@ void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr m int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("position_interpolate"); + auto node = rclcpp::Node::make_shared("eagleye_position_interpolate"); - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_gga_topic_name = "gnss/gga"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); diff --git a/eagleye_rt/src/position_node.cpp b/eagleye_rt/src/position_node.cpp index 0dc1aea3..1da64187 100644 --- a/eagleye_rt/src/position_node.cpp +++ b/eagleye_rt/src/position_node.cpp @@ -56,6 +56,8 @@ static bool use_canless_mode; rclcpp::Clock clock_(RCL_ROS_TIME); tf2_ros::Buffer tfBuffer_(std::make_shared(clock_)); +std::string node_name = "eagleye_position"; + void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { rtklib_nav = *msg; @@ -109,7 +111,7 @@ void on_timer() } catch (tf2::TransformException& ex) { - // RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + RCLCPP_WARN(rclcpp::get_logger(node_name), "%s", ex.what()); return; } } @@ -148,12 +150,10 @@ void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr m int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("position"); - - // tfBuffer_(node->get_clock()); + auto node = rclcpp::Node::make_shared(node_name); - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_gga_topic_name = "gnss/gga"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -167,8 +167,6 @@ int main(int argc, char** argv) use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); use_canless_mode = conf["/**"]["ros__parameters"]["use_canless_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["/**"]["ros__parameters"]["rtklib_nav_topic"].as(); - position_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); position_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); position_parameter.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); @@ -236,6 +234,8 @@ int main(int argc, char** argv) node->get_node_base_interface()->get_context()); node->get_node_timers_interface()->add_timer(timer, nullptr); + tf2_ros::TransformListener listener(tfBuffer_); + rclcpp::spin(node); return 0; diff --git a/eagleye_rt/src/rolling_node.cpp b/eagleye_rt/src/rolling_node.cpp index 07d03e65..c541c970 100644 --- a/eagleye_rt/src/rolling_node.cpp +++ b/eagleye_rt/src/rolling_node.cpp @@ -126,7 +126,7 @@ void rolling_node(rclcpp::Node::SharedPtr node) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("rolling"); + auto node = rclcpp::Node::make_shared("eagleye_rolling"); rolling_node(node); diff --git a/eagleye_rt/src/rtk_deadreckoning_node.cpp b/eagleye_rt/src/rtk_deadreckoning_node.cpp index c4ada533..6be80636 100644 --- a/eagleye_rt/src/rtk_deadreckoning_node.cpp +++ b/eagleye_rt/src/rtk_deadreckoning_node.cpp @@ -127,10 +127,10 @@ void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr m int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("rtk_deadreckoning"); + auto node = rclcpp::Node::make_shared("eagleye_rtk_deadreckoning"); - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_gga_topic_name = "gnss/gga"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -142,7 +142,6 @@ int main(int argc, char** argv) YAML::Node conf = YAML::LoadFile(yaml_file); use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["/**"]["ros__parameters"]["rtklib_nav_topic"].as(); rtk_deadreckoning_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); rtk_deadreckoning_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); @@ -151,9 +150,10 @@ int main(int argc, char** argv) rtk_deadreckoning_parameter.tf_gnss_parent_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); rtk_deadreckoning_parameter.tf_gnss_child_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); rtk_deadreckoning_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + rtk_deadreckoning_parameter.rtk_fix_STD = conf["/**"]["ros__parameters"]["rtk_deadreckoning"]["rtk_fix_STD"].as(); + rtk_deadreckoning_parameter.proc_noise = conf["/**"]["ros__parameters"]["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; std::cout << "ecef_base_pos_x " << rtk_deadreckoning_parameter.ecef_base_pos_x << std::endl; std::cout << "ecef_base_pos_y " << rtk_deadreckoning_parameter.ecef_base_pos_y << std::endl; @@ -162,6 +162,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) { @@ -174,8 +176,8 @@ int main(int argc, char** argv) auto sub3 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); auto sub4 = node->create_subscription("heading_interpolate_3rd", 1000, heading_interpolate_3rd_callback); - pub1 = node->create_publisher("enu_absolute_rtk_deadreckoning", 1000); - pub2 = node->create_publisher("rtk_fix", 1000); + pub1 = node->create_publisher("enu_absolute_pos_interpolate", 1000); + pub2 = node->create_publisher("fix", 1000); const auto period_ns = std::chrono::duration_cast(std::chrono::duration(0.5)); diff --git a/eagleye_rt/src/rtk_heading_node.cpp b/eagleye_rt/src/rtk_heading_node.cpp index 209c2622..7d300781 100644 --- a/eagleye_rt/src/rtk_heading_node.cpp +++ b/eagleye_rt/src/rtk_heading_node.cpp @@ -109,10 +109,10 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("rtk_heading"); + auto node = rclcpp::Node::make_shared("eagleye_rtk_heading"); - std::string subscribe_gga_topic_name = "/navsat/gga"; + std::string subscribe_gga_topic_name = "gnss/gga"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); diff --git a/eagleye_rt/src/slip_angle_node.cpp b/eagleye_rt/src/slip_angle_node.cpp index 80d5f348..682d9cd9 100644 --- a/eagleye_rt/src/slip_angle_node.cpp +++ b/eagleye_rt/src/slip_angle_node.cpp @@ -97,7 +97,7 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("slip_angle"); + auto node = rclcpp::Node::make_shared("eagleye_slip_angle"); std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); diff --git a/eagleye_rt/src/slip_coefficient_node.cpp b/eagleye_rt/src/slip_coefficient_node.cpp index 8a1a8bd7..a643ae49 100644 --- a/eagleye_rt/src/slip_coefficient_node.cpp +++ b/eagleye_rt/src/slip_coefficient_node.cpp @@ -102,9 +102,9 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("slip_coefficient"); + auto node = rclcpp::Node::make_shared("eagleye_slip_coefficient"); - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -117,8 +117,6 @@ int main(int argc, char** argv) use_canless_mode = conf["/**"]["ros__parameters"]["use_canless_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["/**"]["ros__parameters"]["rtklib_nav_topic"].as(); - slip_coefficient_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); slip_coefficient_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); slip_coefficient_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); @@ -130,8 +128,6 @@ int main(int argc, char** argv) std::cout<< "use_canless_mode " << use_canless_mode << std::endl; - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout << "imu_rate " << slip_coefficient_parameter.imu_rate << std::endl; std::cout << "stop_judgment_threshold " << slip_coefficient_parameter.stop_judgment_threshold << std::endl; std::cout << "moving_judgment_threshold " << slip_coefficient_parameter.moving_judgment_threshold << std::endl; diff --git a/eagleye_rt/src/smoothing_node.cpp b/eagleye_rt/src/smoothing_node.cpp index eca524d9..c4f62413 100644 --- a/eagleye_rt/src/smoothing_node.cpp +++ b/eagleye_rt/src/smoothing_node.cpp @@ -67,9 +67,9 @@ void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("smoothing"); + auto node = rclcpp::Node::make_shared("eagleye_smoothing"); - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -82,8 +82,6 @@ int main(int argc, char** argv) use_canless_mode = conf["/**"]["ros__parameters"]["use_canless_mode"].as(); - subscribe_rtklib_nav_topic_name = conf["/**"]["ros__parameters"]["rtklib_nav_topic"].as(); - smoothing_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); smoothing_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); smoothing_parameter.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); diff --git a/eagleye_rt/src/tf_converted_imu.cpp b/eagleye_rt/src/tf_converted_imu.cpp index b51d18b7..7a9da4d2 100644 --- a/eagleye_rt/src/tf_converted_imu.cpp +++ b/eagleye_rt/src/tf_converted_imu.cpp @@ -60,11 +60,13 @@ class TFConvertedIMU: public rclcpp::Node std::string tf_base_link_frame_; + bool reverse_imu_wz_; + void imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg); }; -TFConvertedIMU::TFConvertedIMU() : Node("tf_converted_imu"), +TFConvertedIMU::TFConvertedIMU() : Node("eagleye_tf_converted_imu"), clock_(RCL_ROS_TIME), tfbuffer_(std::make_shared(clock_)), tflistener_(tfbuffer_), @@ -76,14 +78,17 @@ TFConvertedIMU::TFConvertedIMU() : Node("tf_converted_imu"), declare_parameter("imu_topic", subscribe_imu_topic_name); declare_parameter("publish_imu_topic", publish_imu_topic_name); declare_parameter("tf_gnss_frame.parent", tf_base_link_frame_); + declare_parameter("reverse_imu_wz", reverse_imu_wz_); get_parameter("imu_topic", subscribe_imu_topic_name); get_parameter("publish_imu_topic", publish_imu_topic_name); get_parameter("tf_gnss_frame.parent", tf_base_link_frame_); + get_parameter("reverse_imu_wz", reverse_imu_wz_); std::cout<< "subscribe_imu_topic_name: " << subscribe_imu_topic_name << std::endl; std::cout<< "publish_imu_topic_name: " << publish_imu_topic_name << std::endl; std::cout<< "tf_base_link_frame: " << tf_base_link_frame_ << std::endl; + std::cout<< "reverse_imu_wz: " << reverse_imu_wz_ << std::endl; sub_ = create_subscription(subscribe_imu_topic_name, rclcpp::QoS(10), std::bind(&TFConvertedIMU::imu_callback, this, std::placeholders::_1)); pub_ = create_publisher("imu/data_tf_converted", rclcpp::QoS(10)); @@ -112,6 +117,14 @@ void TFConvertedIMU::imu_callback(const sensor_msgs::msg::Imu::ConstPtr msg) tf2::doTransform(linear_acceleration, transformed_linear_acceleration, transform); tf_converted_imu_.angular_velocity = transformed_angular_velocity.vector; + if(reverse_imu_wz_) + { + tf_converted_imu_.angular_velocity.z = (-1) * transformed_angular_velocity.vector.z; + } + else + { + tf_converted_imu_.angular_velocity.z = transformed_angular_velocity.vector.z; + } tf_converted_imu_.linear_acceleration = transformed_linear_acceleration.vector; tf_converted_imu_.orientation = transformed_quaternion; diff --git a/eagleye_rt/src/trajectory_node.cpp b/eagleye_rt/src/trajectory_node.cpp index ef804bed..8157b8b5 100644 --- a/eagleye_rt/src/trajectory_node.cpp +++ b/eagleye_rt/src/trajectory_node.cpp @@ -45,6 +45,7 @@ static eagleye_msgs::msg::Pitching pitching; static geometry_msgs::msg::Vector3Stamped enu_vel; static eagleye_msgs::msg::Position enu_relative_pos; static geometry_msgs::msg::TwistStamped eagleye_twist; +static geometry_msgs::msg::TwistWithCovarianceStamped eagleye_twist_with_covariance; rclcpp::Publisher::SharedPtr pub1; rclcpp::Publisher::SharedPtr pub2; rclcpp::Publisher::SharedPtr pub3; @@ -61,6 +62,8 @@ static bool input_status; static bool use_canless_mode; +static std::string node_name = "eagleye_trajectory"; + void correction_velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { correction_velocity = *msg; @@ -116,7 +119,7 @@ void on_timer() else { input_status = false; - RCLCPP_WARN(rclcpp::get_logger("trajectory"), "Twist is missing the required input topics."); + RCLCPP_WARN(rclcpp::get_logger(node_name), "Twist is missing the required input topics."); } if (imu_time != imu_time_last) imu_time_last = imu_time; @@ -147,8 +150,10 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) enu_relative_pos.header.frame_id = "base_link"; eagleye_twist.header = msg->header; eagleye_twist.header.frame_id = "base_link"; + eagleye_twist_with_covariance.header = msg->header; + eagleye_twist_with_covariance.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); + trajectory_parameter,&trajectory_status,&enu_vel,&enu_relative_pos,&eagleye_twist, &eagleye_twist_with_covariance); if (heading_interpolate_3rd.status.enabled_status) { @@ -156,20 +161,6 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) pub2->publish(enu_relative_pos); } pub3->publish(eagleye_twist); - - geometry_msgs::msg::TwistWithCovarianceStamped eagleye_twist_with_covariance; - eagleye_twist_with_covariance.header = msg->header; - eagleye_twist_with_covariance.header.frame_id = "base_link"; - eagleye_twist_with_covariance.twist.twist = eagleye_twist.twist; - // TODO(Map IV): temporary value - // linear.y, linear.z, angular.x, and angular.y are not calculated values. - eagleye_twist_with_covariance.twist.covariance[0] = 0.2 * 0.2; - eagleye_twist_with_covariance.twist.covariance[7] = 10000.0; - eagleye_twist_with_covariance.twist.covariance[14] = 10000.0; - eagleye_twist_with_covariance.twist.covariance[21] = 10000.0; - eagleye_twist_with_covariance.twist.covariance[28] = 10000.0; - eagleye_twist_with_covariance.twist.covariance[35] = 0.1 * 0.1; - pub4->publish(eagleye_twist_with_covariance); } } @@ -177,9 +168,9 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("trajectory"); + auto node = rclcpp::Node::make_shared(node_name); - std::string subscribe_twist_topic_name; + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -191,12 +182,12 @@ int main(int argc, char** argv) YAML::Node conf = YAML::LoadFile(yaml_file); use_canless_mode = conf["/**"]["ros__parameters"]["use_canless_mode"].as(); - - subscribe_twist_topic_name = conf["/**"]["ros__parameters"]["twist_topic"].as(); - trajectory_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - trajectory_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["trajectory"]["curve_judgment_threshold"].as(); + trajectory_parameter.sensor_noise_velocity = conf["/**"]["ros__parameters"]["trajectory"]["sensor_noise_velocity"].as(); + trajectory_parameter.sensor_scale_noise_velocity = conf["/**"]["ros__parameters"]["trajectory"]["sensor_scale_noise_velocity"].as(); + trajectory_parameter.sensor_noise_yawrate = conf["/**"]["ros__parameters"]["trajectory"]["sensor_noise_yawrate"].as(); + trajectory_parameter.sensor_bias_noise_yawrate = conf["/**"]["ros__parameters"]["trajectory"]["sensor_bias_noise_yawrate"].as(); timer_updata_rate = conf["/**"]["ros__parameters"]["trajectory"]["timer_updata_rate"].as(); // deadlock_threshold = conf["/**"]["ros__parameters"]["trajectory"]["deadlock_threshold"].as(); @@ -207,6 +198,12 @@ int main(int argc, char** argv) 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; } diff --git a/eagleye_rt/src/twist_relay.cpp b/eagleye_rt/src/twist_relay.cpp new file mode 100644 index 00000000..9f02f5cd --- /dev/null +++ b/eagleye_rt/src/twist_relay.cpp @@ -0,0 +1,113 @@ +// 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. + +/* + * twist_relay.cpp + * Author MapIV Sasaki + */ + +#include "rclcpp/rclcpp.hpp" +#include "eagleye_coordinate/eagleye_coordinate.hpp" +#include "eagleye_navigation/eagleye_navigation.hpp" + +class TwistRelay: public rclcpp::Node +{ +public: + TwistRelay(); + ~TwistRelay(); + +private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr twist_with_covariance_sub_; + + rclcpp::Logger logger_; + rclcpp::Clock clock_; + + void twist_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg); + void twist_with_covariance_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstPtr msg); + +}; + +TwistRelay::TwistRelay() : Node("eagleye_twist_relay"), + clock_(RCL_ROS_TIME), + logger_(get_logger()) +{ + int subscribe_twist_topic_type = 0; + std::string subscribe_twist_topic_name = "/can_twist"; + std::string publish_twist_topic_name = "vehicle/twist"; + + declare_parameter("twist.twist_type", subscribe_twist_topic_type); + declare_parameter("twist.twist_topic", subscribe_twist_topic_name); + + get_parameter("twist.twist_type", subscribe_twist_topic_type); + get_parameter("twist.twist_topic", subscribe_twist_topic_name); + + std::cout<< "subscribe_twist_topic_type: " << subscribe_twist_topic_type << std::endl; + std::cout<< "subscribe_twist_topic_name: " << subscribe_twist_topic_name << std::endl; + + // TwistStamped : 0, TwistWithCovarianceStamped: 1 + if(subscribe_twist_topic_type == 0) + { + twist_sub_ = create_subscription(subscribe_twist_topic_name, rclcpp::QoS(10), + std::bind(&TwistRelay::twist_callback, this, std::placeholders::_1)); + } + else if(subscribe_twist_topic_type == 1) + { + twist_with_covariance_sub_ = create_subscription(subscribe_twist_topic_name, rclcpp::QoS(10), + std::bind(&TwistRelay::twist_with_covariance_callback, this, std::placeholders::_1)); + } + else + { + RCLCPP_ERROR(this->get_logger(),"Invalid twist topic type"); + rclcpp::shutdown(); + } + + pub_ = create_publisher(publish_twist_topic_name, rclcpp::QoS(10)); +}; + +TwistRelay::~TwistRelay(){}; + +void TwistRelay::twist_callback(const geometry_msgs::msg::TwistStamped::ConstPtr msg) +{ + pub_->publish(*msg); +}; + +void TwistRelay::twist_with_covariance_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstPtr msg) +{ + geometry_msgs::msg::TwistStamped twist; + twist.header.stamp = msg->header.stamp; + twist.twist = msg->twist.twist; + pub_->publish(twist); +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + rclcpp::spin(std::make_shared()); + + return 0; +} \ No newline at end of file diff --git a/eagleye_rt/src/velocity_estimator_node.cpp b/eagleye_rt/src/velocity_estimator_node.cpp index 76fd0208..0e8cbaf5 100644 --- a/eagleye_rt/src/velocity_estimator_node.cpp +++ b/eagleye_rt/src/velocity_estimator_node.cpp @@ -45,26 +45,8 @@ VelocityEstimator velocity_estimator; static geometry_msgs::msg::TwistStamped velocity_msg; static std::string yaml_file; -static std::string subscribe_rtklib_nav_topic_name; -static std::string subscribe_gga_topic_name; - -void setParam(std::string yaml_file) -{ - std::cout << "yaml_file: " << yaml_file << std::endl; - try - { - YAML::Node conf = YAML::LoadFile(yaml_file); - subscribe_rtklib_nav_topic_name = conf["/**"]["ros__parameters"]["rtklib_nav_topic"].as(); - subscribe_gga_topic_name = conf["/**"]["ros__parameters"]["gga_topic"].as(); - std::cout<< "subscribe_rtklib_nav_topic_name "<declare_parameter("yaml_file",yaml_file); node->get_parameter("yaml_file",yaml_file); - setParam(yaml_file); velocity_estimator.setParam(yaml_file); auto rtklib_sub = @@ -118,7 +99,7 @@ void velocity_estimator_node(rclcpp::Node::SharedPtr node) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("velocity_estimator"); + auto node = rclcpp::Node::make_shared("eagleye_velocity_estimator"); velocity_estimator_node(node); diff --git a/eagleye_rt/src/velocity_scale_factor_node.cpp b/eagleye_rt/src/velocity_scale_factor_node.cpp index ce2cd5e2..17005487 100644 --- a/eagleye_rt/src/velocity_scale_factor_node.cpp +++ b/eagleye_rt/src/velocity_scale_factor_node.cpp @@ -190,14 +190,14 @@ void on_timer() int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("velocity_scale_factor"); + auto node = rclcpp::Node::make_shared("eagleye_velocity_scale_factor"); double velocity_scale_factor_save_duration = 100.0; - std::string subscribe_twist_topic_name = "/can_twist"; + std::string subscribe_twist_topic_name = "vehicle/twist"; - std::string subscribe_rtklib_nav_topic_name = "/rtklib_nav"; - std::string subscribe_rmc_topic_name = "/navsat/rmc"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_rmc_topic_name = "gnss/rmc"; std::string yaml_file; node->declare_parameter("yaml_file",yaml_file); @@ -210,9 +210,6 @@ int main(int argc, char** argv) _use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); - subscribe_twist_topic_name = conf["/**"]["ros__parameters"]["twist_topic"].as(); - subscribe_rtklib_nav_topic_name = conf["/**"]["ros__parameters"]["rtklib_nav_topic"].as(); - _velocity_scale_factor_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); _velocity_scale_factor_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); _velocity_scale_factor_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); diff --git a/eagleye_rt/src/yawrate_offset_node.cpp b/eagleye_rt/src/yawrate_offset_node.cpp index d24365ac..59946711 100644 --- a/eagleye_rt/src/yawrate_offset_node.cpp +++ b/eagleye_rt/src/yawrate_offset_node.cpp @@ -99,7 +99,7 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("yawrate_offset"); + auto node = rclcpp::Node::make_shared("eagleye_yawrate_offset"); std::string publish_topic_name = "/publish_topic_name/invalid"; std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; diff --git a/eagleye_rt/src/yawrate_offset_stop_node.cpp b/eagleye_rt/src/yawrate_offset_stop_node.cpp index 2c526d01..b091a1da 100644 --- a/eagleye_rt/src/yawrate_offset_stop_node.cpp +++ b/eagleye_rt/src/yawrate_offset_stop_node.cpp @@ -70,8 +70,8 @@ void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("yawrate_offset_stop"); - std::string subscribe_twist_topic_name = "/can_twist"; + auto node = rclcpp::Node::make_shared("eagleye_yawrate_offset_stop"); + std::string subscribe_twist_topic_name = "vehicle/twist"; std::string yaml_file; node->declare_parameter("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["/**"]["ros__parameters"]["twist_topic"].as(); - _yawrate_offset_stop_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); _yawrate_offset_stop_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); diff --git a/eagleye_util/fix2pose/launch/fix2pose.xml b/eagleye_util/fix2pose/launch/fix2pose.xml index f53c68b1..53dec7ad 100644 --- a/eagleye_util/fix2pose/launch/fix2pose.xml +++ b/eagleye_util/fix2pose/launch/fix2pose.xml @@ -3,13 +3,33 @@ + + + + + + + + - + + + + + + + + + + + + + diff --git a/eagleye_util/fix2pose/package.xml b/eagleye_util/fix2pose/package.xml index eea016a9..5f022163 100644 --- a/eagleye_util/fix2pose/package.xml +++ b/eagleye_util/fix2pose/package.xml @@ -12,7 +12,6 @@ rclcpp rclpy std_msgs - geographic_msgs eagleye_msgs eagleye_navigation tf2_ros @@ -20,7 +19,6 @@ rclcpp rclpy std_msgs - geographic_msgs eagleye_msgs eagleye_navigation tf2_ros @@ -28,11 +26,12 @@ rclcpp rclpy std_msgs - geographic_msgs eagleye_msgs eagleye_navigation tf2_ros eagleye_coordinate + llh_converter + std_srvs ament_lint_auto ament_lint_common diff --git a/eagleye_util/fix2pose/src/fix2pose.cpp b/eagleye_util/fix2pose/src/fix2pose.cpp index d1510382..707bccbb 100644 --- a/eagleye_util/fix2pose/src/fix2pose.cpp +++ b/eagleye_util/fix2pose/src/fix2pose.cpp @@ -38,162 +38,306 @@ #include "eagleye_msgs/msg/heading.hpp" #include "eagleye_msgs/msg/position.hpp" #include "tf2_ros/transform_broadcaster.h" +#include +#include #include "tf2_geometry_msgs/tf2_geometry_msgs.h" #include "eagleye_coordinate/eagleye_coordinate.hpp" +#include -static eagleye_msgs::msg::Rolling eagleye_rolling; -static eagleye_msgs::msg::Pitching eagleye_pitching; -static eagleye_msgs::msg::Heading eagleye_heading; -static eagleye_msgs::msg::Position eagleye_position; +#include + +rclcpp::Clock _ros_clock(RCL_ROS_TIME); + +static eagleye_msgs::msg::Rolling _eagleye_rolling; +static eagleye_msgs::msg::Pitching _eagleye_pitching; +static eagleye_msgs::msg::Heading _eagleye_heading; static geometry_msgs::msg::Quaternion _quat; -rclcpp::Publisher::SharedPtr pub; -rclcpp::Publisher::SharedPtr pub2; -std::shared_ptr br; -std::shared_ptr br2; -static geometry_msgs::msg::PoseStamped pose; -static geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance; +rclcpp::Publisher::SharedPtr _pub; +rclcpp::Publisher::SharedPtr _pub2; +rclcpp::Publisher::SharedPtr _pub3; +std::shared_ptr _br; +std::shared_ptr _br2; +static geometry_msgs::msg::PoseStamped _pose; +static geometry_msgs::msg::PoseWithCovarianceStamped _pose_with_covariance; + +static std::string _parent_frame_id, _child_frame_id; +static std::string _base_link_frame_id, _gnss_frame_id; + +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] +bool _initial_pose_estimated = false; -static int convert_height_num = 0; -static int plane = 7; -static int tf_num = 1; -static std::string parent_frame_id, child_frame_id; +std::string _node_name = "eagleye_fix2pose"; -static ConvertHeight convert_height; +tf2_ros::Buffer _tf_buffer(std::make_shared(_ros_clock)); + +rclcpp::Client::SharedPtr _client_ekf_trigger; void heading_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - eagleye_heading = *msg; + _eagleye_heading = *msg; } void rolling_callback(const eagleye_msgs::msg::Rolling::ConstSharedPtr msg) { - eagleye_rolling = *msg; + _eagleye_rolling = *msg; } void pitching_callback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) { - eagleye_pitching = *msg; -} - -void position_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) -{ - eagleye_position = *msg; + _eagleye_pitching = *msg; } void fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { - double llh[3] = {0}; - double xyz[3] = {0}; - - llh[0] = msg->latitude * M_PI / 180; - llh[1] = msg->longitude* M_PI / 180; - llh[2] = msg->altitude; - - if (convert_height_num == 1) + bool fix_flag = false; + if(_fix_judgement_type == 0) { - convert_height.setLLH(msg->latitude,msg->longitude,msg->altitude); - llh[2] = convert_height.convert2altitude(); + if(msg->status.status == 0 && _eagleye_heading.status.enabled_status) fix_flag = true; } - else if(convert_height_num == 2) + else if(_fix_judgement_type == 1) { - convert_height.setLLH(msg->latitude,msg->longitude,msg->altitude); - llh[2] = convert_height.convert2ellipsoid(); + if(msg->position_covariance[0] < _fix_std_pos_thres * _fix_std_pos_thres && _eagleye_heading.status.enabled_status) fix_flag = true; } - - if (tf_num == 1) + else { - ll2xy(plane,llh,xyz); + RCLCPP_ERROR(rclcpp::get_logger(_node_name), "fix_judgement_type is not valid"); + rclcpp::shutdown(); } - else if (tf_num == 2) + + if(_fix_only_publish && !fix_flag) { - ll2xy_mgrs(llh,xyz); + return; } + double llh[3] = {0}; + double xyz[3] = {0}; + + llh[0] = msg->latitude * M_PI / 180; + llh[1] = msg->longitude* M_PI / 180; + llh[2] = msg->altitude; + + _lc.convertRad2XYZ(llh[0], llh[1], llh[2], xyz[0], xyz[1], xyz[2], _llh_param); + + double eagleye_heading = 0; tf2::Quaternion localization_quat; - if (eagleye_heading.status.enabled_status) + if (_eagleye_heading.status.enabled_status) { - eagleye_heading.heading_angle = fmod(eagleye_heading.heading_angle,2*M_PI); - localization_quat.setRPY(eagleye_rolling.rolling_angle,eagleye_pitching.pitching_angle,(90* M_PI / 180)-eagleye_heading.heading_angle); + eagleye_heading = fmod((90* M_PI / 180)-_eagleye_heading.heading_angle,2*M_PI); + localization_quat.setRPY(0, 0, eagleye_heading); } else { - tf2::Quaternion localization_quat; tf2::Matrix3x3(localization_quat).setRPY(0, 0, 0); } _quat = tf2::toMsg(localization_quat); - pose.header = msg->header; - pose.header.frame_id = "map"; - pose.pose.position.x = xyz[1]; - pose.pose.position.y = xyz[0]; - pose.pose.position.z = xyz[2]; - pose.pose.orientation = _quat; - pub->publish(pose); - pose_with_covariance.header = pose.header; - pose_with_covariance.pose.pose = pose.pose; + _pose.header = msg->header; + _pose.header.frame_id = "map"; + _pose.pose.position.x = xyz[0]; + _pose.pose.position.y = xyz[1]; + _pose.pose.position.z = xyz[2]; + _pose.pose.orientation = _quat; + + geometry_msgs::msg::PoseStamped::Ptr transformed_pose_msg_ptr( + new geometry_msgs::msg::PoseStamped); + + if(_fix_only_publish) + { + geometry_msgs::msg::TransformStamped::Ptr TF_sensor_to_base_ptr(new geometry_msgs::msg::TransformStamped); + try + { + *TF_sensor_to_base_ptr = _tf_buffer.lookupTransform(_gnss_frame_id, _base_link_frame_id, tf2::TimePointZero); + + tf2::Transform transform, transform2, transfrom3; + transform.setOrigin(tf2::Vector3(_pose.pose.position.x, _pose.pose.position.y, + _pose.pose.position.z)); + transform.setRotation(localization_quat); + tf2::Quaternion q2(TF_sensor_to_base_ptr->transform.rotation.x, TF_sensor_to_base_ptr->transform.rotation.y, + TF_sensor_to_base_ptr->transform.rotation.z, TF_sensor_to_base_ptr->transform.rotation.w); + transform2.setOrigin(tf2::Vector3(TF_sensor_to_base_ptr->transform.translation.x, + TF_sensor_to_base_ptr->transform.translation.y, TF_sensor_to_base_ptr->transform.translation.z)); + transform2.setRotation(q2); + transfrom3 = transform * transform2; + + _pose.header.frame_id = _parent_frame_id; + _pose.pose.position.x = transfrom3.getOrigin().getX(); + _pose.pose.position.y = transfrom3.getOrigin().getY(); + _pose.pose.position.z = transfrom3.getOrigin().getZ(); + _pose.pose.orientation.x = transfrom3.getRotation().getX(); + _pose.pose.orientation.y = transfrom3.getRotation().getY(); + _pose.pose.orientation.z = transfrom3.getRotation().getZ(); + _pose.pose.orientation.w = transfrom3.getRotation().getW(); + + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN(rclcpp::get_logger(_node_name), "%s", ex.what()); + return; + } + } + + _pub->publish(_pose); + + _pose_with_covariance.header = _pose.header; + _pose_with_covariance.pose.pose = _pose.pose; // TODO(Map IV): temporary value double std_dev_roll = 100; // [rad] double std_dev_pitch = 100; // [rad] 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.status.enabled_status) std_dev_yaw = 0.2 / 180 * M_PI; - 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]; - pose_with_covariance.pose.covariance[21] = std_dev_roll * std_dev_roll; - pose_with_covariance.pose.covariance[28] = std_dev_pitch * std_dev_pitch; - pose_with_covariance.pose.covariance[35] = std_dev_yaw * std_dev_yaw; - pub2->publish(pose_with_covariance); + 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.status.enabled_status) std_dev_yaw = std::sqrt(_eagleye_heading.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]; + _pose_with_covariance.pose.covariance[21] = std_dev_roll * std_dev_roll; + _pose_with_covariance.pose.covariance[28] = std_dev_pitch * std_dev_pitch; + _pose_with_covariance.pose.covariance[35] = std_dev_yaw * std_dev_yaw; + _pub2->publish(_pose_with_covariance); + + if(!_initial_pose_estimated) + { + _pub3->publish(_pose_with_covariance); + _initial_pose_estimated = true; + + const auto req = std::make_shared(); + req->data = true; + if (!_client_ekf_trigger->service_is_ready()) { + RCLCPP_WARN(rclcpp::get_logger(_node_name), "EKF localizar triggering service is not ready"); + } + auto future_ekf = _client_ekf_trigger->async_send_request(req); + } tf2::Transform transform; tf2::Quaternion q; - transform.setOrigin(tf2::Vector3(pose.pose.position.x,pose.pose.position.y,pose.pose.position.z)); - q.setRPY(0, 0, (90* M_PI / 180)-eagleye_heading.heading_angle); + transform.setOrigin(tf2::Vector3(_pose.pose.position.x, _pose.pose.position.y, _pose.pose.position.z)); + q.setRPY(0, 0, (90* M_PI / 180)- _eagleye_heading.heading_angle); transform.setRotation(q); geometry_msgs::msg::TransformStamped trans_msg; trans_msg.header.stamp = msg->header.stamp; - trans_msg.header.frame_id = parent_frame_id; - trans_msg.child_frame_id = child_frame_id; + trans_msg.header.frame_id = _parent_frame_id; + trans_msg.child_frame_id = _child_frame_id; trans_msg.transform = tf2::toMsg(transform); - br->sendTransform(trans_msg); + _br->sendTransform(trans_msg); } int main(int argc, char** argv) { + int plane = 7; + int tf_num = 7; + int convert_height_num = 7; + int geoid_type = 0; + rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("fix2pose"); - - node->declare_parameter("plane",plane); - node->declare_parameter("tf_num",tf_num); - node->declare_parameter("convert_height_num",convert_height_num); - node->declare_parameter("parent_frame_id",parent_frame_id); - node->declare_parameter("child_frame_id",child_frame_id); - - node->get_parameter("plane",plane); - node->get_parameter("tf_num",tf_num); - node->get_parameter("convert_height_num",convert_height_num); - node->get_parameter("parent_frame_id",parent_frame_id); - node->get_parameter("child_frame_id",child_frame_id); - - std::cout<< "plane"<create_subscription("/eagleye/heading_interpolate_3rd", 1000, heading_callback); - auto sub2 = node->create_subscription("/eagleye/enu_absolute_pos_interpolate", 1000, position_callback); - auto sub3 = node->create_subscription("/eagleye/fix", 1000, fix_callback); - auto sub4 = node->create_subscription("/eagleye/rolling", 1000, rolling_callback); - auto sub5 = node->create_subscription("/eagleye/pitching", 1000, pitching_callback); - pub = node->create_publisher("/eagleye/pose", 1000); - pub2 = node->create_publisher("/eagleye/pose_with_covariance", 1000); - br = std::make_shared(node, 100); - br2 = std::make_shared(node, 100); + auto node = rclcpp::Node::make_shared(_node_name); + + node->declare_parameter("plane", plane); + node->declare_parameter("tf_num", tf_num); + node->declare_parameter("convert_height_num", convert_height_num); + node->declare_parameter("geoid_type", geoid_type); + node->declare_parameter("parent_frame_id", _parent_frame_id); + node->declare_parameter("child_frame_id", _child_frame_id); + node->declare_parameter("fix_only_publish", _fix_only_publish); + node->declare_parameter("fix_judgement_type", _fix_judgement_type); + node->declare_parameter("fix_gnss_status", _fix_gnss_status); + node->declare_parameter("fix_std_pos_thres", _fix_std_pos_thres); + node->declare_parameter("base_link_frame_id", _base_link_frame_id); + node->declare_parameter("gnss_frame_id", _gnss_frame_id); + + node->get_parameter("plane", plane); + node->get_parameter("tf_num", tf_num); + node->get_parameter("convert_height_num", convert_height_num); + node->get_parameter("geoid_type", geoid_type); + node->get_parameter("parent_frame_id", _parent_frame_id); + node->get_parameter("child_frame_id", _child_frame_id); + node->get_parameter("fix_only_publish", _fix_only_publish); + node->get_parameter("fix_judgement_type", _fix_judgement_type); + node->get_parameter("fix_gnss_status", _fix_gnss_status); + node->get_parameter("fix_std_pos_thres", _fix_std_pos_thres); + node->get_parameter("base_link_frame_id", _base_link_frame_id); + node->get_parameter("gnss_frame_id", _gnss_frame_id); + + std::cout<< "plane "<< plane<create_subscription("eagleye/heading_interpolate_3rd", 1000, heading_callback); + auto sub3 = node->create_subscription("eagleye/fix", 1000, fix_callback); + auto sub4 = node->create_subscription("eagleye/rolling", 1000, rolling_callback); + auto sub5 = node->create_subscription("eagleye/pitching", 1000, pitching_callback); + _pub = node->create_publisher("eagleye/pose", 1000); + _pub2 = node->create_publisher("eagleye/pose_with_covariance", 1000); + _pub3 = node->create_publisher("/initialpose3d", 1000); + _br = std::make_shared(node, 100); + _br2 = std::make_shared(node, 100); + _client_ekf_trigger = node->create_client("ekf_trigger_node"); rclcpp::spin(node); return 0; diff --git a/eagleye_util/nmea2fix/CMakeLists.txt b/eagleye_util/gnss_converter/CMakeLists.txt similarity index 87% rename from eagleye_util/nmea2fix/CMakeLists.txt rename to eagleye_util/gnss_converter/CMakeLists.txt index 00afdf11..1358ef4e 100644 --- a/eagleye_util/nmea2fix/CMakeLists.txt +++ b/eagleye_util/gnss_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(eagleye_nmea2fix) +project(eagleye_gnss_converter) # Default to C99 if(NOT CMAKE_C_STANDARD) @@ -20,9 +20,9 @@ ament_auto_find_build_dependencies() include_directories(include) -ament_auto_add_executable(nmea2fix +ament_auto_add_executable(gnss_converter src/nmea2fix_core.cpp - src/nmea2fix_node.cpp + src/gnss_converter_node.cpp ) install(TARGETS diff --git a/eagleye_util/nmea2fix/README.md b/eagleye_util/gnss_converter/README.md similarity index 55% rename from eagleye_util/nmea2fix/README.md rename to eagleye_util/gnss_converter/README.md index a48ca5ff..2c146bea 100644 --- a/eagleye_util/nmea2fix/README.md +++ b/eagleye_util/gnss_converter/README.md @@ -1,4 +1,4 @@ -nmea2fix +gnss_converter ========== A ros package that converts gnss nmea messages to navsatfix messages @@ -7,31 +7,37 @@ A ros package that converts gnss nmea messages to navsatfix messages # Launch ~~~ -source $HOME/catkin_ws/devel/setup.bash -roslaunch nmea2fix nmea2fix.launch +source $HOME/ros2_ws/devel/setup.bash +ros2 launch eagleye_gnss_converter gnss_converter.xml ~~~ # Node ## Subscribed Topics - - /navsat/nmea_sentence (nmea_msgs/Sentence) + - /nmea_sentence (nmea_msgs/Sentence) ## Published Topics - - /navsat/fix (sensor_msgs/NavSatFix) (This topic will not be published unless its location has been estimated.) + - /fix (sensor_msgs/NavSatFix) - /gga (nmea_msgs/Gpgga) + - /rmc (nmea_msgs/Grmc) + # Parameter description -The parameters are set in `launch/nmea2fix.launch` . +The parameters are set in `launch/gnss_converter.xml` . |Name|Type|Description|Default value| |:---|:---|:---|:---| +<<<<<<< HEAD |nmea_sentence_topic|bool|Topic name of nmea_msgs/Sentence to subscribe|/navsat/nmea_sentence| |pub_fix_topic_name|double|Topic name of sensor_msgs/NavSatFix to publish|/navsat/fix| -|pub_gga_topic_name|bool|Topic name of nmea_msgs/Gpgga to publish|/navsat/gga| -|pub_rmc_topic_name|bool|Topic name of nmea_msgs/Gprmc to publish|/navsat/rmc| +|pub_gga_topic_name|bool|Topic name of nmea_msgs/Gpgga to publish|gnss/gga| +|pub_rmc_topic_name|bool|Topic name of nmea_msgs/Gprmc to publish|gnss/rmc| |output_gga|bool|Whether to output nmea_msgs/Gpgga|false| -|output_rmc|bool|Whether to output nmea_msgs/Gprmc|false| \ No newline at end of file +|output_rmc|bool|Whether to output nmea_msgs/Gprmc|false| +======= +|nmea_sentence_topic|bool|Topic name of nmea_msgs/Sentence to subscribe|/nmea_sentence| +>>>>>>> 47c9d0b83db1524e6347f2c39333881242678d6b diff --git a/eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp b/eagleye_util/gnss_converter/include/gnss_converter/nmea2fix.hpp similarity index 91% rename from eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp rename to eagleye_util/gnss_converter/include/gnss_converter/nmea2fix.hpp index 43a1d5da..983f4608 100644 --- a/eagleye_util/nmea2fix/include/nmea2fix/nmea2fix.hpp +++ b/eagleye_util/gnss_converter/include/gnss_converter/nmea2fix.hpp @@ -23,8 +23,8 @@ // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -#ifndef NMEA2FIX_H -#define NMEA2FIX_H +#ifndef gnss_converter_H +#define gnss_converter_H #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/nav_sat_fix.hpp" @@ -36,7 +36,7 @@ #include extern double stringToGPSTime(std::string&, double); -extern void nmea2fix_converter(const nmea_msgs::msg::Sentence, sensor_msgs::msg::NavSatFix*, +extern void gnss_converter_converter(const nmea_msgs::msg::Sentence, sensor_msgs::msg::NavSatFix*, nmea_msgs::msg::Gpgga*, nmea_msgs::msg::Gprmc*); -#endif /*NMEA2FIX_H */ +#endif /*gnss_converter_H */ diff --git a/eagleye_util/gnss_converter/launch/gnss_converter.xml b/eagleye_util/gnss_converter/launch/gnss_converter.xml new file mode 100644 index 00000000..2185dd81 --- /dev/null +++ b/eagleye_util/gnss_converter/launch/gnss_converter.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/eagleye_util/gnss_converter/package.xml b/eagleye_util/gnss_converter/package.xml new file mode 100644 index 00000000..160039c2 --- /dev/null +++ b/eagleye_util/gnss_converter/package.xml @@ -0,0 +1,46 @@ + + + eagleye_gnss_converter + 1.0.0 + gnss_converter package + + OsamuSekino + + BSD3 + + ament_cmake + rclcpp + sensor_msgs + nmea_msgs + ublox_msgs + rtklib_msgs + eagleye_coordinate + eagleye_navigation + tf2_ros + tf2_geometry_msgs + rclcpp + sensor_msgs + nmea_msgs + ublox_msgs + rtklib_msgs + eagleye_coordinate + eagleye_navigation + tf2_ros + tf2_geometry_msgs + rclcpp + sensor_msgs + nmea_msgs + ublox_msgs + rtklib_msgs + eagleye_coordinate + eagleye_navigation + tf2_ros + tf2_geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + 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..c405d783 --- /dev/null +++ b/eagleye_util/gnss_converter/src/gnss_converter_node.cpp @@ -0,0 +1,193 @@ + +#include "rclcpp/rclcpp.hpp" +#include "gnss_converter/nmea2fix.hpp" +#include +#include +#include "eagleye_coordinate/eagleye_coordinate.hpp" +#include "eagleye_navigation/eagleye_navigation.hpp" + +rclcpp::Publisher::SharedPtr navsatfix_pub; +rclcpp::Publisher::SharedPtr gga_pub; +rclcpp::Publisher::SharedPtr rmc_pub; +rclcpp::Publisher::SharedPtr rtklib_nav_pub; + + +static nmea_msgs::msg::Sentence sentence; +sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_msg_ptr; + +static std::string sub_topic_name, pub_fix_topic_name = "fix", + pub_gga_topic_name = "gga", pub_rmc_topic_name = "rmc" ,pub_rtklib_nav_topic = "rtklib_nav"; + +void nmea_callback(const nmea_msgs::msg::Sentence::ConstSharedPtr msg) +{ + nmea_msgs::msg::Gpgga gga; + nmea_msgs::msg::Gprmc rmc; + sensor_msgs::msg::NavSatFix fix; + + sentence.header = msg->header; + sentence.sentence = msg->sentence; + gnss_converter_converter(sentence, &fix, &gga, &rmc); + + rclcpp::Time ros_clock(fix.header.stamp); + rclcpp::Time ros_clock2(rmc.header.stamp); + + if (ros_clock.seconds() != 0) + { + gga.header.frame_id = fix.header.frame_id = "gnss"; + navsatfix_pub->publish(fix); + gga_pub->publish(gga); + } + if (ros_clock2.seconds() != 0) + { + rmc.header.frame_id = "gnss"; + rmc_pub->publish(rmc); + } +} + +void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { + rtklib_nav_pub->publish(*msg);; +} + +void navsatfix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) { nav_msg_ptr = msg; } + + +void navpvt_callback(const ublox_msgs::msg::NavPVT::ConstSharedPtr msg) +{ + rtklib_msgs::msg::RtklibNav r; + r.header.frame_id = "gps"; + r.header.stamp.sec = msg->sec; + r.header.stamp.nanosec = msg->nano; + if (nav_msg_ptr != nullptr) + r.status = *nav_msg_ptr; + r.tow = msg->i_tow; + + 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->vel_e * 1e-3, msg->vel_n * 1e-3, -msg->vel_d * 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]; + + rtklib_nav_pub->publish(r); +} + +void gnss_velocity_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr msg) +{ + if (nav_msg_ptr == nullptr) return; + rtklib_msgs::msg::RtklibNav r; + r.header.frame_id = "gps"; + r.header.stamp = msg->header.stamp; + rclcpp::Time ros_clock(msg->header.stamp); + double gnss_velocity_time = ros_clock.seconds(); + 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; + + rtklib_nav_pub->publish(r); +} + + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("gnss_converter_node"); + + 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; + + rclcpp::Subscription::SharedPtr rtklib_nav_sub; + rclcpp::Subscription::SharedPtr nmea_sentence_sub; + rclcpp::Subscription::SharedPtr navpvt_sub; + rclcpp::Subscription::SharedPtr gnss_velocity_sub; + rclcpp::Subscription::SharedPtr navsatfix_sub; + + node->declare_parameter("gnss.velocity_source_type",velocity_source_type); + node->declare_parameter("gnss.velocity_source_topic",velocity_source_topic); + node->declare_parameter("gnss.llh_source_type",llh_source_type); + node->declare_parameter("gnss.llh_source_topic",llh_source_topic); + node->get_parameter("gnss.velocity_source_type",velocity_source_type); + node->get_parameter("gnss.velocity_source_topic",velocity_source_topic); + node->get_parameter("gnss.llh_source_type",llh_source_type); + node->get_parameter("gnss.llh_source_topic",llh_source_topic); + + std::cout<< "velocity_source_type "<create_subscription(velocity_source_topic, 1000, rtklib_nav_callback); + } + else if(velocity_source_type == 1) + { + nmea_sentence_sub = node->create_subscription(velocity_source_topic, 1000, nmea_callback); + } + else if(velocity_source_type == 2) + { + navpvt_sub = node->create_subscription(velocity_source_topic, 1000, navpvt_callback); + } + else if(velocity_source_type == 3) + { + gnss_velocity_sub = node->create_subscription( + velocity_source_topic, 1000, gnss_velocity_callback); + } + else + { + RCLCPP_ERROR(node->get_logger(),"Invalid velocity_source_type"); + rclcpp::shutdown(); + } + + if(llh_source_type == 0) + { + rtklib_nav_sub = node->create_subscription(llh_source_topic, 1000, rtklib_nav_callback); + } + else if(llh_source_type == 1) + { + nmea_sentence_sub = node->create_subscription(llh_source_topic, 1000, nmea_callback); + } + else if(llh_source_type == 2) + { + navsatfix_sub = node->create_subscription(llh_source_topic, 1000, navsatfix_callback); + } + else + { + RCLCPP_ERROR(node->get_logger(),"Invalid llh_source_type"); + rclcpp::shutdown(); + } + + navsatfix_pub = node->create_publisher(pub_fix_topic_name, 1000); + gga_pub = node->create_publisher(pub_gga_topic_name, 1000); + rmc_pub = node->create_publisher(pub_rmc_topic_name, 1000); + rtklib_nav_pub = node->create_publisher(pub_rtklib_nav_topic, 1000); + + rclcpp::spin(node); + + return 0; +} diff --git a/eagleye_util/nmea2fix/src/nmea2fix_core.cpp b/eagleye_util/gnss_converter/src/nmea2fix_core.cpp similarity index 96% rename from eagleye_util/nmea2fix/src/nmea2fix_core.cpp rename to eagleye_util/gnss_converter/src/nmea2fix_core.cpp index 60f42dbb..a3c3334c 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) { @@ -50,7 +50,7 @@ double stringToGPSTime(std::string& input, double header_time) return GPSTime; } -void nmea2fix_converter(const nmea_msgs::msg::Sentence sentence, sensor_msgs::msg::NavSatFix* fix, nmea_msgs::msg::Gpgga* gga, nmea_msgs::msg::Gprmc* rmc) +void gnss_converter_converter(const nmea_msgs::msg::Sentence sentence, sensor_msgs::msg::NavSatFix* fix, nmea_msgs::msg::Gpgga* gga, nmea_msgs::msg::Gprmc* rmc) { std::vector linedata,nmea_data; diff --git a/eagleye_util/llh_converter b/eagleye_util/llh_converter new file mode 160000 index 00000000..35815585 --- /dev/null +++ b/eagleye_util/llh_converter @@ -0,0 +1 @@ +Subproject commit 3581558547afab43daf807f1d19b2e34963b05d8 diff --git a/eagleye_util/nmea2fix/launch/nmea2fix.xml b/eagleye_util/nmea2fix/launch/nmea2fix.xml deleted file mode 100644 index c6f6f813..00000000 --- a/eagleye_util/nmea2fix/launch/nmea2fix.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/eagleye_util/nmea2fix/package.xml b/eagleye_util/nmea2fix/package.xml deleted file mode 100644 index e8be36d2..00000000 --- a/eagleye_util/nmea2fix/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - eagleye_nmea2fix - 1.0.0 - nmea2fix package - - OsamuSekino - - BSD3 - - ament_cmake - rclcpp - sensor_msgs - nmea_msgs - rclcpp - sensor_msgs - nmea_msgs - rclcpp - sensor_msgs - nmea_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/eagleye_util/nmea2fix/src/nmea2fix_node.cpp b/eagleye_util/nmea2fix/src/nmea2fix_node.cpp deleted file mode 100644 index 45860453..00000000 --- a/eagleye_util/nmea2fix/src/nmea2fix_node.cpp +++ /dev/null @@ -1,84 +0,0 @@ - -#include "rclcpp/rclcpp.hpp" -#include "nmea2fix/nmea2fix.hpp" - -rclcpp::Publisher::SharedPtr pub1; -rclcpp::Publisher::SharedPtr pub2; -rclcpp::Publisher::SharedPtr pub3; -static nmea_msgs::msg::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::msg::Sentence::ConstSharedPtr msg) -{ - nmea_msgs::msg::Gpgga gga; - nmea_msgs::msg::Gprmc rmc; - sensor_msgs::msg::NavSatFix fix; - - sentence.header = msg->header; - sentence.sentence = msg->sentence; - nmea2fix_converter(sentence, &fix, &gga, &rmc); - - rclcpp::Time ros_clock(fix.header.stamp); - rclcpp::Time ros_clock2(rmc.header.stamp); - - if (ros_clock.seconds() != 0) - { - gga.header.frame_id = fix.header.frame_id = "gnss"; - pub1->publish(fix); - if(output_gga) pub2->publish(gga); - } - if (ros_clock2.seconds() != 0 && output_rmc) - { - rmc.header.frame_id = "gnss"; - pub3->publish(rmc); - } -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("nmea2fix_node"); - - std::string use_gnss_mode; - - node->declare_parameter("nmea_sentence_topic",sub_topic_name); - node->declare_parameter("pub_fix_topic_name",pub_fix_topic_name); - node->declare_parameter("pub_gga_topic_name",pub_gga_topic_name); - node->declare_parameter("pub_rmc_topic_name",pub_rmc_topic_name); - node->declare_parameter("output_gga",output_gga); - node->declare_parameter("output_rmc",output_rmc); - node->declare_parameter("use_gnss_mode",use_gnss_mode); - - node->get_parameter("nmea_sentence_topic",sub_topic_name); - node->get_parameter("pub_fix_topic_name",pub_fix_topic_name); - node->get_parameter("pub_gga_topic_name",pub_gga_topic_name); - node->get_parameter("pub_rmc_topic_name",pub_rmc_topic_name); - node->get_parameter("output_gga",output_gga); - node->get_parameter("output_rmc",output_rmc); - node->get_parameter("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 "<create_subscription(sub_topic_name, 1000, nmea_callback); - pub1 = node->create_publisher(pub_fix_topic_name, 1000); - if(output_gga) pub2 = node->create_publisher(pub_gga_topic_name, 1000); - if(output_rmc) pub3 = node->create_publisher(pub_rmc_topic_name, 1000); - - rclcpp::spin(node); - - return 0; -} diff --git a/eagleye_util/trajectory_plot/config/trajectory_plot.yaml b/eagleye_util/trajectory_plot/config/trajectory_plot.yaml index c64d847c..73f9c77d 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: 2 # 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 @@ -94,4 +96,4 @@ twist_evaluation: plot_text_data: 'elapsed_time' # Text data to be displayed in the graph of trajectory(distance or elapsed_time) plot_text_step: 50 # Text data step to be displayed in the graph of trajectory(default:50 = 50[m]) - \ No newline at end of file + 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..cc0913f1 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(rtk_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 217313ef..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", - "velocity.twist.linear.x", + "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", @@ -213,20 +211,20 @@ def set_log_df(input,plane,config): # Creation of dataset with reference to labe "eagleye_pp_llh.latitude", "eagleye_pp_llh.longitude", "eagleye_pp_llh.altitude", - "imu.angular_velocity.x", - "imu.angular_velocity.y", - "imu.angular_velocity.z", + "imu.angular_velocity.x", + "imu.angular_velocity.y", + "imu.angular_velocity.z", 'gga_llh.gps_qual', ]] eagleye_df = eagleye_df.rename(columns={'timestamp': 'TimeStamp_tmp', 'rtklib_nav.tow': 'TOW', 'velocity_scale_factor.scale_factor': 'sf', - 'velocity.twist.linear.x': 'velocity', + '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', @@ -285,17 +283,22 @@ def set_log_df(input,plane,config): # Creation of dataset with reference to labe 'gga_llh.gps_qual': 'qual', }) - eagleye_index = eagleye_df[eagleye_df['latitude'] == 0].index - eagleye_df = eagleye_df.drop(eagleye_index) - eagleye_df = eagleye_df.reset_index() - 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: raw_df = raw_df.drop(rtk_index) + eagleye_df = eagleye_df.drop(rtk_index) raw_df = raw_df.reset_index() + eagleye_df = eagleye_df.reset_index() if index_time_unit == 1: eagleye_df['TimeStamp'] = eagleye_df['TimeStamp_tmp'] * 10 ** (-9)